Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(motion_utils)!: add autoware prefix and include dir #7539

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
The table of contents is too big for display.
Diff view
Diff view
  •  
  •  
  •  
2 changes: 1 addition & 1 deletion .github/CODEOWNERS
Validating CODEOWNERS rules …
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
common/autoware_grid_map_utils/** maxime.clement@tier4.jp
common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp
common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp
common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp
common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp
common/path_distance_calculator/** isamu.takagi@tier4.jp
Expand Down Expand Up @@ -51,13 +51,13 @@
control/autoware_lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp zulfaqar.azmi@tier4.jp
control/autoware_mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
control/autoware_pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
control/autoware_smart_mpc_trajectory_follower/** kosuke.takeuchi@tier4.jp masayuki.aino@proxima-ai-tech.com takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp

Check warning on line 54 in .github/CODEOWNERS

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (masayuki)

Check warning on line 54 in .github/CODEOWNERS

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (aino)
control/autoware_vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp
control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
control/predicted_path_checker/** berkay@leodrive.ai
control/autoware_smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com

Check warning on line 60 in .github/CODEOWNERS

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (masayuki)

Check warning on line 60 in .github/CODEOWNERS

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (aino)
control/autoware_trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
control/pure_pursuit/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
control/shift_decider/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
Expand Down
4 changes: 2 additions & 2 deletions common/.pages
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,8 @@ nav:
- 'Glog Component': common/glog_component
- 'interpolation': common/interpolation
- 'Kalman Filter': common/kalman_filter
- 'Motion Utils': common/motion_utils
- 'Vehicle Utils': common/motion_utils/docs/vehicle/vehicle
- 'Motion Utils': common/autoware_motion_utils
- 'Vehicle Utils': common/autoware_motion_utils/docs/vehicle/vehicle
- 'Object Recognition Utils': common/object_recognition_utils
- 'OSQP Interface': common/osqp_interface/design/osqp_interface-design
- 'Perception Utils': common/perception_utils
Expand Down
2 changes: 1 addition & 1 deletion common/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -13,4 +13,4 @@ The Autoware.Universe Common folder consists of common and testing libraries tha
Some of the commonly used libraries are:

1. `autoware_universe_utils`
2. `motion_utils`
2. `autoware_motion_utils`
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
cmake_minimum_required(VERSION 3.14)
project(motion_utils)
project(autoware_motion_utils)

find_package(autoware_cmake REQUIRED)
autoware_package()

find_package(Boost REQUIRED)

ament_auto_add_library(motion_utils SHARED
ament_auto_add_library(autoware_motion_utils SHARED
DIRECTORY src
)

Expand All @@ -15,10 +15,10 @@ if(BUILD_TESTING)

file(GLOB_RECURSE test_files test/**/*.cpp)

ament_add_ros_isolated_gtest(test_motion_utils ${test_files})
ament_add_ros_isolated_gtest(test_autoware_motion_utils ${test_files})

target_link_libraries(test_motion_utils
motion_utils
target_link_libraries(test_autoware_motion_utils
autoware_motion_utils
)
endif()

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -114,4 +114,4 @@ const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_

Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time.

`motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing.
`autoware_motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing.
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__CONSTANTS_HPP_
#define MOTION_UTILS__CONSTANTS_HPP_
#ifndef AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_
#define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_

namespace motion_utils
namespace autoware_motion_utils
{
constexpr double overlap_threshold = 0.1;
} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__CONSTANTS_HPP_
#endif // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__DISTANCE__DISTANCE_HPP_
#define MOTION_UTILS__DISTANCE__DISTANCE_HPP_
#ifndef AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_
#define AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_

#include <algorithm>
#include <cmath>
Expand All @@ -22,12 +22,12 @@
#include <tuple>
#include <vector>

namespace motion_utils
namespace autoware_motion_utils
{
std::optional<double> calcDecelDistWithJerkAndAccConstraints(
const double current_vel, const double target_vel, const double current_acc, const double acc_min,
const double jerk_acc, const double jerk_dec);

} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__DISTANCE__DISTANCE_HPP_
#endif // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
#ifndef AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
#define AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_

#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
Expand All @@ -24,7 +24,7 @@
#include <string>
#include <vector>

namespace motion_utils
namespace autoware_motion_utils
{
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
using autoware_adapi_v1_msgs::msg::VelocityFactor;
Expand All @@ -49,6 +49,6 @@ class VelocityFactorInterface
VelocityFactor velocity_factor_{};
};

} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
#endif // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
#define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
#ifndef AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
#define AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_

#include <rclcpp/time.hpp>

#include <visualization_msgs/msg/marker_array.hpp>

#include <string>

namespace motion_utils
namespace autoware_motion_utils
{
using geometry_msgs::msg::Pose;

Expand All @@ -48,6 +48,6 @@ visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(

visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
const rclcpp::Time & now, const int32_t id);
} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
#endif // AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
#define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
#ifndef AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
#define AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_

#include <rclcpp/time.hpp>

Expand All @@ -25,7 +25,7 @@
#include <unordered_map>
#include <vector>

namespace motion_utils
namespace autoware_motion_utils
{

/// @brief type of virtual wall associated with different marker styles and namespace
Expand Down Expand Up @@ -76,6 +76,6 @@ class VirtualWallMarkerCreator
/// @param now current time to be used for displaying the markers
visualization_msgs::msg::MarkerArray create_markers(const rclcpp::Time & now = rclcpp::Time());
};
} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
#endif // AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
#define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_

#include "autoware_planning_msgs/msg/path.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"

#include <vector>

namespace motion_utils
namespace autoware_motion_utils
{
/**
* @brief A resampling function for a path(points). Note that in a default setting, position xy are
Expand Down Expand Up @@ -234,6 +234,6 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory(
const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true,
const bool use_zero_order_hold_for_twist = true,
const bool resample_input_trajectory_stop_point = true);
} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
#define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_

#include "autoware/universe_utils/system/backtrace.hpp"

#include <autoware/motion_utils/constants.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <motion_utils/constants.hpp>
#include <motion_utils/trajectory/trajectory.hpp>

#include <vector>

Expand All @@ -29,7 +29,7 @@ constexpr double close_s_threshold = 1e-6;

static inline rclcpp::Logger get_logger()
{
constexpr const char * logger{"motion_utils.resample_utils"};
constexpr const char * logger{"autoware_motion_utils.resample_utils"};
return rclcpp::get_logger(logger);
}

Expand All @@ -42,7 +42,7 @@ bool validate_size(const T & points)
template <class T>
bool validate_resampling_range(const T & points, const std::vector<double> & resampling_intervals)
{
const double points_length = motion_utils::calcArcLength(points);
const double points_length = autoware_motion_utils::calcArcLength(points);
return points_length >= resampling_intervals.back();
}

Expand Down Expand Up @@ -105,10 +105,10 @@ bool validate_arguments(const T & input_points, const double resampling_interval
}

// check resampling interval
if (resampling_interval < motion_utils::overlap_threshold) {
if (resampling_interval < autoware_motion_utils::overlap_threshold) {
RCLCPP_DEBUG(
get_logger(), "invalid argument: resampling interval is less than %f",
motion_utils::overlap_threshold);
autoware_motion_utils::overlap_threshold);
autoware_universe_utils::print_backtrace();
return false;
}
Expand All @@ -124,4 +124,4 @@ bool validate_arguments(const T & input_points, const double resampling_interval
}
} // namespace resample_utils

#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
#define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_

#include "autoware_planning_msgs/msg/detail/path__struct.hpp"
#include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp"
Expand All @@ -23,7 +23,7 @@

#include <vector>

namespace motion_utils
namespace autoware_motion_utils
{
using TrajectoryPoints = std::vector<autoware_planning_msgs::msg::TrajectoryPoint>;

Expand Down Expand Up @@ -115,6 +115,6 @@ inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
return output;
}

} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
#define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_

#include "autoware/universe_utils/geometry/geometry.hpp"

Expand All @@ -25,7 +25,7 @@
#include <algorithm>
#include <limits>

namespace motion_utils
namespace autoware_motion_utils
{
/**
* @brief An interpolation function that finds the closest interpolated point on the trajectory from
Expand Down Expand Up @@ -91,6 +91,6 @@ geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double tar
return autoware_universe_utils::getPose(points.back());
}

} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
#define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_

#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
#include <geometry_msgs/msg/point.hpp>

#include <optional>
#include <utility>
namespace motion_utils
namespace autoware_motion_utils
{
std::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id);
Expand All @@ -41,6 +41,6 @@ size_t findNearestSegmentIndexFromLaneId(
tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter(
const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog,
const bool enable_last_point_compensation = true);
} // namespace motion_utils
} // namespace autoware_motion_utils

#endif // MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
Loading
Loading