diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 22152911b46b2..1d6081d3ac62a 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -21,9 +21,12 @@ #include #include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -54,9 +57,9 @@ std::vector getAllKeys(const std::unordered_map & map) namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedPath; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; struct MinMaxValue { @@ -190,12 +193,12 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::string uuid{}; uint8_t label{}; geometry_msgs::msg::Pose pose{}; - autoware_auto_perception_msgs::msg::Shape shape; + autoware_perception_msgs::msg::Shape shape; double vel{0.0}; double lat_vel{0.0}; bool is_object_on_ego_path{false}; std::optional latest_time_inside_ego_path{std::nullopt}; - std::vector predicted_paths{}; + std::vector predicted_paths{}; // NOTE: Previous values of the following are used for low-pass filtering. // Therefore, they has to be initialized as nullopt. @@ -397,7 +400,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const std::optional & prev_object) const; bool willObjectBeOutsideEgoChangingPath( const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const; + const autoware_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; TimeWhileCollision calcTimeWhileCollision( @@ -407,15 +410,15 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; + const autoware_perception_msgs::msg::Shape & obj_shape) const; double calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const autoware_perception_msgs::msg::Shape & obj_shape, const bool is_object_same_direction) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, - const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape, const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoidRegulatedObject( const std::vector & ref_path_points_for_obj_poly, diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 9b64069eddafd..33db04f3c8e3c 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -16,9 +16,9 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_vehicle_msgs behavior_path_planner behavior_path_planner_common geometry_msgs diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index efc56c4d61f5f..b2c5d77402d9a 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -128,13 +128,13 @@ std::pair projectObstacleVelocityToTrajectory( return std::make_pair(projected_velocity[0], projected_velocity[1]); } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); @@ -148,13 +148,13 @@ double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & s throw std::logic_error("The shape type is not supported in dynamic_avoidance."); } -double calcObstacleWidth(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleWidth(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return shape.dimensions.y; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); @@ -458,7 +458,7 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::planWaitingApproval() ObjectType DynamicObstacleAvoidanceModule::getObjectType(const uint8_t label) const { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; if (label == ObjectClassification::CAR && parameters_->avoid_car) { return ObjectType::REGULATED; @@ -1121,8 +1121,8 @@ DynamicObstacleAvoidanceModule::DecisionWithReason DynamicObstacleAvoidanceModul } [[maybe_unused]] bool DynamicObstacleAvoidanceModule::willObjectBeOutsideEgoChangingPath( - const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const + const geometry_msgs::msg::Pose & obj_pose, const autoware_perception_msgs::msg::Shape & obj_shape, + const double obj_vel) const { if (!ref_path_before_lane_change_ || obj_vel < 0.0) { return false; @@ -1183,7 +1183,7 @@ DynamicObstacleAvoidanceModule::getAdjacentLanes( DynamicObstacleAvoidanceModule::LatLonOffset DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const + const autoware_perception_msgs::msg::Shape & obj_shape) const { const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); @@ -1218,7 +1218,7 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, - const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape, const TimeWhileCollision & time_while_collision) const { const size_t obj_seg_idx = @@ -1290,8 +1290,7 @@ MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, - const bool is_object_same_direction) const + const autoware_perception_msgs::msg::Shape & obj_shape, const bool is_object_same_direction) const { const auto & input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 56b9bad535e7d..10c0f57cfb6fe 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -19,14 +19,14 @@ #include #include -#include +#include #include #include namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; AvoidanceParameters getParameter(rclcpp::Node * node) diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp index afdfba30ce267..8bbe4b550ff00 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -23,20 +23,20 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include #include namespace behavior_path_planner { // auto msgs -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedPath; +using tier4_planning_msgs::msg::PathWithLaneId; // ROS 2 general msgs using geometry_msgs::msg::Point; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index f243e166e6790..1b7814874b498 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -21,8 +21,6 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_perception_msgs behavior_path_planner behavior_path_planner_common diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index b8e6bb99fbebb..682e23e7f8156 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -44,7 +44,7 @@ MarkerArray createObjectsCubeMarkerArray( const ObjectDataArray & objects, std::string && ns, const Vector3 & scale, const ColorRGBA & color) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; MarkerArray msg; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index bfc95e0730cd6..d43c7243fb238 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -28,7 +28,7 @@ namespace behavior_path_planner { void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; // init manager interface @@ -42,7 +42,7 @@ void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void StaticObstacleAvoidanceModuleManager::updateModuleParams( const std::vector & parameters) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::updateParam; auto p = parameters_; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 660b8d228b6f2..90c8ad7b66dcd 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -40,7 +40,7 @@ namespace behavior_path_planner::utils::static_obstacle_avoidance { -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_perception_msgs::msg::TrafficLightElement; using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index a68705e3f73a2..5d3e6e08a8810 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -29,21 +29,21 @@ So for example, in order to stop at a stop line with the vehicles' front on the ## Input topics -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | -| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | -| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | -| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | -| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | -| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | +| Name | Type | Description | +| ----------------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| `~input/path_with_lane_id` | tier4_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | +| `~input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | +| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | ## Output topics | Name | Type | Description | | ---------------------- | ----------------------------------------- | -------------------------------------- | -| `~output/path` | autoware_auto_planning_msgs::msg::Path | path to be followed | +| `~output/path` | autoware_planning_msgs::msg::Path | path to be followed | | `~output/stop_reasons` | tier4_planning_msgs::msg::StopReasonArray | reasons that cause the vehicle to stop | ## Node parameters diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index a9a096dae7d59..1ef2a919f6290 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -34,9 +34,9 @@ rosidl_default_generators - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_map_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_velocity_smoother behavior_velocity_planner_common diagnostic_msgs diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index dbbbe143658f4..ffbc4ef9174dc 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -57,10 +57,10 @@ namespace autoware::behavior_velocity_planner namespace { -autoware_auto_planning_msgs::msg::Path to_path( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path_with_id) +autoware_planning_msgs::msg::Path to_path( + const tier4_planning_msgs::msg::PathWithLaneId & path_with_id) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; for (const auto & path_point : path_with_id.points) { path.points.push_back(path_point.point); } @@ -79,13 +79,13 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Trigger Subscriber trigger_sub_path_with_lane_id_ = - this->create_subscription( + this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1), createSubscriptionOptions(this)); // Subscribers sub_predicted_objects_ = - this->create_subscription( + this->create_subscription( "~/input/dynamic_objects", 1, std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1), createSubscriptionOptions(this)); @@ -99,12 +99,12 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), createSubscriptionOptions(this)); - sub_lanelet_map_ = this->create_subscription( + sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), createSubscriptionOptions(this)); sub_traffic_signals_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); @@ -130,7 +130,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio onParam(); // Publishers - path_pub_ = this->create_publisher("~/output/path", 1); + path_pub_ = this->create_publisher("~/output/path", 1); stop_reason_diag_pub_ = this->create_publisher("~/output/stop_reason", 1); debug_viz_pub_ = this->create_publisher("~/debug/path", 1); @@ -231,7 +231,7 @@ void BehaviorVelocityPlannerNode::onOccupancyGrid( } void BehaviorVelocityPlannerNode::onPredictedObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.predicted_objects = msg; @@ -315,7 +315,7 @@ void BehaviorVelocityPlannerNode::onParam() } void BehaviorVelocityPlannerNode::onLaneletMap( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -324,7 +324,7 @@ void BehaviorVelocityPlannerNode::onLaneletMap( } void BehaviorVelocityPlannerNode::onTrafficSignals( - const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -333,28 +333,30 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( const auto traffic_light_id_map_last_observed_old = planner_data_.traffic_light_id_map_last_observed_; planner_data_.traffic_light_id_map_last_observed_.clear(); - for (const auto & signal : msg->signals) { + for (const auto & signal : msg->traffic_light_groups) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_light_group_id] = traffic_signal; const bool is_unknown_observation = std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { - return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + return element.color == autoware_perception_msgs::msg::TrafficLightElement::UNKNOWN; }); // if the observation is UNKNOWN and past observation is available, only update the timestamp // and keep the body of the info - const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + const auto old_data = + traffic_light_id_map_last_observed_old.find(signal.traffic_light_group_id); if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { // copy last observation - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = old_data->second; // update timestamp - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id].stamp = msg->stamp; } else { // if (1)the observation is not UNKNOWN or (2)the very first observation is UNKNOWN - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = + traffic_signal; } } } @@ -373,7 +375,7 @@ void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( } void BehaviorVelocityPlannerNode::onTrigger( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { std::unique_lock lk(mutex_); @@ -396,7 +398,7 @@ void BehaviorVelocityPlannerNode::onTrigger( return; } - const autoware_auto_planning_msgs::msg::Path output_path_msg = + const autoware_planning_msgs::msg::Path output_path_msg = generatePath(input_path_msg, planner_data_); lk.unlock(); @@ -410,11 +412,11 @@ void BehaviorVelocityPlannerNode::onTrigger( } } -autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, +autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data) { - autoware_auto_planning_msgs::msg::Path output_path_msg; + autoware_planning_msgs::msg::Path output_path_msg; // TODO(someone): support backward path const auto is_driving_forward = motion_utils::isDrivingForward(input_path_msg->points); @@ -456,8 +458,7 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath return output_path_msg; } -void BehaviorVelocityPlannerNode::publishDebugMarker( - const autoware_auto_planning_msgs::msg::Path & path) +void BehaviorVelocityPlannerNode::publishDebugMarker(const autoware_planning_msgs::msg::Path & path) { visualization_msgs::msg::MarkerArray output_msg; for (size_t i = 0; i < path.points.size(); ++i) { diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index 6e74203460542..62ceef5f04ea6 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -24,15 +24,15 @@ #include #include -#include -#include -#include -#include +#include +#include +#include #include #include #include #include #include +#include #include #include @@ -46,9 +46,9 @@ namespace autoware::behavior_velocity_planner { -using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; +using autoware_map_msgs::msg::LaneletMapBin; using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; @@ -63,31 +63,30 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // subscriber - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr trigger_sub_path_with_lane_id_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_objects_; rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; rclcpp::Subscription::SharedPtr sub_acceleration_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr sub_traffic_signals_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; rclcpp::Subscription::SharedPtr sub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; - void onTrigger( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); + void onTrigger(const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); void onPredictedObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); - void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onLaneletMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); void onTrafficSignals( - const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); @@ -95,11 +94,11 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onParam(); // publisher - rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr path_pub_; rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - void publishDebugMarker(const autoware_auto_planning_msgs::msg::Path & path); + void publishDebugMarker(const autoware_planning_msgs::msg::Path & path); // parameter double forward_path_length_; @@ -110,7 +109,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node PlannerData planner_data_; BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; bool has_received_map_; rclcpp::Service::SharedPtr srv_load_plugin_; @@ -127,8 +126,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped getCurrentPose(); bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const; - autoware_auto_planning_msgs::msg::Path generatePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, + autoware_planning_msgs::msg::Path generatePath( + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); std::unique_ptr logger_configure_; diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index f939f67a06f9e..f462fc963f17b 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -96,11 +96,11 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( } } -autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( +tier4_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg) + const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg) { - autoware_auto_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; + tier4_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; int first_stop_path_point_index = static_cast(output_path_msg.points.size() - 1); std::string stop_reason_msg("path_end"); diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index f9c1d0253de62..9e7f2942bb067 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -20,12 +20,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include #include #include +#include #include #include @@ -48,9 +48,9 @@ class BehaviorVelocityPlannerManager void launchScenePlugin(rclcpp::Node & node, const std::string & name); void removeScenePlugin(rclcpp::Node & node, const std::string & name); - autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg); + const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg); diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; diff --git a/planning/autoware_behavior_velocity_template_module/README.md b/planning/autoware_behavior_velocity_template_module/README.md index 2cc3e6db8a555..130da3f919482 100644 --- a/planning/autoware_behavior_velocity_template_module/README.md +++ b/planning/autoware_behavior_velocity_template_module/README.md @@ -50,13 +50,13 @@ The managing of your modules is defined in manager.hpp and manager.cpp. The mana #### `launchNewModules()` Method -- This is a private method that takes an argument of type `autoware_auto_planning_msgs::msg::PathWithLaneId`. +- This is a private method that takes an argument of type `tier4_planning_msgs::msg::PathWithLaneId`. - It is responsible for launching new modules based on the provided path information (PathWithLaneId). The implementation of this method involves initializing and configuring modules specific to your behavior velocity planner by using the `TemplateModule` class. - In the provided source code, it initializes a `module_id` to 0 and checks if a module with the same ID is already registered. If not, it registers a new `TemplateModule` with the module ID. Note that each module managed by the `TemplateModuleManager` should have a unique ID. The template code registers a single module, so the `module_id` is set as 0 for simplicity. #### `getModuleExpiredFunction()` Method -- This is a private method that takes an argument of type `autoware_auto_planning_msgs::msg::PathWithLaneId`. +- This is a private method that takes an argument of type `tier4_planning_msgs::msg::PathWithLaneId`. - It returns a `std::function&)>`. This function is used by the behavior velocity planner to determine whether a particular module has expired or not based on the given path. - The implementation of this method is expected to return a function that can be used to check the expiration status of modules. diff --git a/planning/autoware_behavior_velocity_template_module/package.xml b/planning/autoware_behavior_velocity_template_module/package.xml index fa3b20f5b5af8..a6bcbf5c34e76 100644 --- a/planning/autoware_behavior_velocity_template_module/package.xml +++ b/planning/autoware_behavior_velocity_template_module/package.xml @@ -13,7 +13,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/autoware_behavior_velocity_template_module/src/manager.cpp index c5861d032dee1..8f6621aaab835 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.cpp @@ -38,7 +38,7 @@ TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) } void TemplateModuleManager::launchNewModules( - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { @@ -49,7 +49,7 @@ void TemplateModuleManager::launchNewModules( std::function &)> TemplateModuleManager::getModuleExpiredFunction( - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) { return []([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { return false; diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/autoware_behavior_velocity_template_module/src/manager.hpp index aaaf0710eff46..8e95f516c337d 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -63,7 +63,7 @@ class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleMan * * @param path The path with lane ID information to determine module launch. */ - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; /** * @brief Get a function to check module expiration. @@ -75,7 +75,7 @@ class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleMan * @return A function for checking module expiration. */ std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; /** diff --git a/planning/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/autoware_behavior_velocity_template_module/src/scene.hpp index e8c00efcb3ee1..3ce5ddbd8729d 100644 --- a/planning/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/scene.hpp @@ -23,9 +23,9 @@ namespace autoware::behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using ::behavior_velocity_planner::SceneModuleInterface; using ::behavior_velocity_planner::StopReason; +using tier4_planning_msgs::msg::PathWithLaneId; class TemplateModule : public SceneModuleInterface { diff --git a/planning/autoware_path_optimizer/README.md b/planning/autoware_path_optimizer/README.md index 29ffee76b0c49..9222e5d32ea7d 100644 --- a/planning/autoware_path_optimizer/README.md +++ b/planning/autoware_path_optimizer/README.md @@ -19,16 +19,16 @@ Note that the velocity is just taken over from the input path. ### input -| Name | Type | Description | -| ------------------ | ------------------------------------ | -------------------------------------------------- | -| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | -| `~/input/odometry` | nav_msgs/msg/Odometry | Current Velocity of ego vehicle | +| Name | Type | Description | +| ------------------ | ------------------------------- | -------------------------------------------------- | +| `~/input/path` | autoware_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current Velocity of ego vehicle | ### output -| Name | Type | Description | -| --------------------- | ------------------------------------------ | ----------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/msg/Trajectory | Optimized trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs/msg/Trajectory | Optimized trajectory that is feasible to drive and collision-free | ## Flowchart diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp index 3ef47b865bf8e..d1d2abaeaad61 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp @@ -29,7 +29,7 @@ namespace autoware_path_optimizer { MarkerArray getDebugMarker( const DebugData & debug_data, - const std::vector & optimized_points, + const std::vector & optimized_points, const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); } // namespace autoware_path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp index f249b7486bce6..755d0b2ace297 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ #define AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -33,10 +33,10 @@ namespace autoware_path_optimizer // std_msgs using std_msgs::msg::Header; // planning -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // visualization diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp index d21d353f951c4..32ef8cd5941fc 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp @@ -25,8 +25,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp index c4d3f9c063433..1056d80ef37aa 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp @@ -24,8 +24,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 9b2d1e952d91a..f02473ebd015e 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index e05098d126a74..9d93cdc26a7ed 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -20,8 +20,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 66d0fc08ccf06..7983c5c2a3c2f 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -21,8 +21,8 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index 82636af20579d..08a6e1b9d6960 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -35,15 +35,14 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include +#include +#include #include #include #include @@ -57,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -72,13 +72,12 @@ namespace planning_test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseStamped; @@ -92,6 +91,7 @@ using tier4_api_msgs::msg::CrosswalkStatus; using tier4_api_msgs::msg::IntersectionStatus; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; @@ -190,7 +190,7 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr expand_stop_range_pub_; rclcpp::Publisher::SharedPtr occupancy_grid_pub_; rclcpp::Publisher::SharedPtr cost_map_pub_; - rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr map_pub_; rclcpp::Publisher::SharedPtr scenario_pub_; rclcpp::Publisher::SharedPtr parking_scenario_pub_; rclcpp::Publisher::SharedPtr lane_driving_scenario_pub_; @@ -201,7 +201,7 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr initial_pose_tf_pub_; rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; - rclcpp::Publisher::SharedPtr traffic_signals_pub_; + rclcpp::Publisher::SharedPtr traffic_signals_pub_; rclcpp::Publisher::SharedPtr virtual_traffic_light_states_pub_; // Subscriber diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index e2c00756c2ba4..3170ff5f854b9 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -13,12 +13,11 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils lanelet2_extension diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 9ee162ec02d1f..01a8d89ca8b4d 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -171,7 +171,7 @@ void PlanningInterfaceTestManager::publishTrafficSignals( rclcpp::Node::SharedPtr target_node, std::string topic_name) { test_utils::publishToTargetNode( - test_node_, target_node, topic_name, traffic_signals_pub_, TrafficSignalArray{}); + test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{}); } void PlanningInterfaceTestManager::publishVirtualTrafficLightState( @@ -293,7 +293,7 @@ void PlanningInterfaceTestManager::publishNominalPath( { test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - motion_utils::convertToPath( + motion_utils::convertToPath( test_utils::loadPathWithLaneIdInYaml()), 5); } diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index 2a88cdb57abf4..c87c397bd07aa 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include #include @@ -50,7 +50,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node private: using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Odometry = nav_msgs::msg::Odometry; using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit; using MissionRemainingDistanceTime = autoware_internal_msgs::msg::MissionRemainingDistanceTime; diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 62cdecc1f1eb2..6d6bfb794940a 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -15,10 +15,10 @@ rosidl_default_generators - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_path_optimizer + autoware_perception_msgs + autoware_planning_msgs behavior_path_planner_common geography_utils geometry_msgs diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py index fec3d21d20bec..3672165caed85 100755 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py @@ -26,7 +26,7 @@ from PyQt5.QtWidgets import QSizePolicy from PyQt5.QtWidgets import QSlider from PyQt5.QtWidgets import QWidget -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 74002916bb23c..24e95353eec93 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -173,7 +173,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( { // publishers pub_map_bin_ = - create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); + create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); pub_whole_centerline_ = create_publisher("output_whole_centerline", utils::create_transient_local_qos()); pub_centerline_ = @@ -357,7 +357,7 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ std::filesystem::copy_options::overwrite_existing); // load map by the map_loader package - map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { + map_bin_ptr_ = [&]() -> LaneletMapBin::ConstSharedPtr { // load map map_projector_info_ = std::make_unique(load_info_from_lanelet2_map(lanelet2_input_file_path)); @@ -379,7 +379,7 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ const auto map_bin_msg = Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); - return std::make_shared(map_bin_msg); + return std::make_shared(map_bin_msg); }(); // check if map_bin_ptr_ is not null pointer diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp index dfe5af68c270b..f8a65f2794809 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -80,7 +80,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node const CenterlineWithRoute & centerline_with_route); lanelet::LaneletMapPtr original_map_ptr_{nullptr}; - HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; std::unique_ptr map_projector_info_{nullptr}; @@ -95,7 +95,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; // publisher - rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; + rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index fb54804db105d..d40e519ef2704 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -17,29 +17,29 @@ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/lanelet_route.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace autoware::static_centerline_generator { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_velocity_smoother/README.ja.md b/planning/autoware_velocity_smoother/README.ja.md index 68957078e2bdb..d36a328ddf7ca 100644 --- a/planning/autoware_velocity_smoother/README.ja.md +++ b/planning/autoware_velocity_smoother/README.ja.md @@ -80,29 +80,29 @@ ### Input -| Name | Type | Description | -| ------------------------------------------ | ---------------------------------------- | ----------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | -| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | -| `/tf` | `tf2_msgs/TFMessage` | TF | -| `/tf_static` | `tf2_msgs/TFMessage` | TF static | +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | ### Output -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory | -| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | -| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | -| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | -| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | -| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | -| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | -| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | -| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | -| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | -| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | ## Parameters diff --git a/planning/autoware_velocity_smoother/README.md b/planning/autoware_velocity_smoother/README.md index b8a8a9eb7bb5f..1a506d8612aa6 100644 --- a/planning/autoware_velocity_smoother/README.md +++ b/planning/autoware_velocity_smoother/README.md @@ -92,30 +92,30 @@ After the optimization, a resampling called `post resampling` is performed befor ### Input -| Name | Type | Description | -| ------------------------------------------ | ---------------------------------------- | ----------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | -| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | -| `/tf` | `tf2_msgs/TFMessage` | TF | -| `/tf_static` | `tf2_msgs/TFMessage` | TF static | +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | ### Output -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory | -| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | -| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | -| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | -| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | -| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | -| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | -| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | -| `~/debug/trajectory_steering_rate_limited` | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | -| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | -| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | -| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_steering_rate_limited` | `autoware_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | ## Parameters diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp index c2fe415091a0a..aecfab342e7e0 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -37,8 +37,8 @@ #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary @@ -55,8 +55,8 @@ namespace autoware_velocity_smoother { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using autoware_adapi_v1_msgs::msg::OperationModeState; using geometry_msgs::msg::AccelWithCovarianceStamped; diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp index 4c820b4d04baa..dc85c2b2f336f 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ #define AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -24,7 +24,7 @@ namespace autoware_velocity_smoother { namespace resampling { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; struct ResampleParam diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 08d0a1cd1a8e1..e54a6e7b8afad 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 596a8aa94c707..3ab4fce11fd0b 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -18,7 +18,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -29,8 +29,8 @@ namespace autoware_velocity_smoother { namespace analytical_velocity_planning_utils { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; bool calcStopDistWithJerkAndAccConstraints( const double v0, const double a0, const double jerk_acc, const double jerk_dec, diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 57414b8e8109f..09c1e7e96be7b 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -20,7 +20,7 @@ #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 3d94102495839..066e0acef67f6 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -20,7 +20,7 @@ #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index c8bdc9d3c939c..b54de318e9702 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -20,7 +20,7 @@ #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp index 63d5e5e887124..43ccfcf193f14 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp @@ -18,14 +18,14 @@ #include "autoware_velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include namespace autoware_velocity_smoother { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; class SmootherBase diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp index 775f4109a4ad1..d35f80fb7fad8 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #define AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" -#include "geometry_msgs/msg/detail/pose__struct.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" #include #include @@ -25,7 +25,7 @@ namespace autoware_velocity_smoother::trajectory_utils { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Pose; diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index b390dcc12edc5..4b5a46e495545 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -20,7 +20,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index d2956e67c6ac1..9cdfc515d51b7 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -26,7 +26,7 @@ namespace { -using TrajectoryPoints = std::vector; +using TrajectoryPoints = std::vector; geometry_msgs::msg::Pose lerpByPose( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) diff --git a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index ace1d1f14d776..51ee84f535ca4 100644 --- a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -18,7 +18,7 @@ #include -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_velocity_smoother::trajectory_utils::TrajectoryPoints; TrajectoryPoints genStraightTrajectory(const size_t size) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index c7c00d38956c1..868b2585170f9 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -30,7 +30,7 @@ namespace behavior_path_planner void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; // init manager interface diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 4da625a7a9087..58203573d0c35 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -17,7 +17,7 @@ #include "behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" -#include +#include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 0926d1010b5c3..9112500e10ac3 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -17,14 +17,14 @@ #include "behavior_path_planner_common/data_manager.hpp" -#include #include +#include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp index 5ce6d83795069..ae1ee8c66a990 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index e67d458874d17..a76ee5f7529f0 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 8f456a57e4c78..012af88f4ca70 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -34,8 +34,8 @@ #include #include -#include -#include +#include +#include #include @@ -50,7 +50,7 @@ namespace behavior_path_planner { -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; using nav_msgs::msg::OccupancyGrid; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index 9700c44445a65..8da00f872f443 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -18,16 +18,16 @@ #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" -#include #include +#include #include #include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index d0b0aee83e20c..635816f17497f 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index be3dd9ba96e38..83e817a8d5a45 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -21,11 +21,11 @@ #include #include "visualization_msgs/msg/detail/marker_array__struct.hpp" -#include -#include -#include +#include +#include #include #include +#include #include @@ -35,11 +35,11 @@ namespace behavior_path_planner::goal_planner_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Polygon2d = tier4_autoware_utils::Polygon2d; diff --git a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index cbbe5f585dbe2..750efe89de6ab 100644 --- a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -26,7 +26,7 @@ namespace behavior_path_planner { using Point2d = tier4_autoware_utils::Point2d; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const { @@ -93,7 +93,7 @@ PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the // refine_goal_search_radius_range is recommended bool is_valid_path{false}; - autoware_auto_planning_msgs::msg::PathWithLaneId refined_path; + tier4_planning_msgs::msg::PathWithLaneId refined_path; while (goal_search_radius >= 0 && !is_valid_path) { refined_path = utils::refinePathForGoal(goal_search_radius, M_PI * 0.5, path, refined_goal, goal_lane_id); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 4c3d129ac687e..d442ca2455a4c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -25,9 +25,9 @@ #include -#include #include #include +#include #include @@ -37,11 +37,11 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeInterface : public SceneModuleInterface { diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 76181f2195a84..b66437f4cff0e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -23,7 +23,6 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; @@ -35,6 +34,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::Direction; +using tier4_planning_msgs::msg::PathWithLaneId; using utils::path_safety_checker::ExtendedPredictedObjects; class NormalLaneChange : public LaneChangeBase diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 792e8924e5df6..3f241b9d6598d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -26,9 +26,9 @@ #include #include -#include #include #include +#include #include #include @@ -36,13 +36,13 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::Direction; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeBase { diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index 3af4f487a0fa0..4297b4a0ed1cd 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -19,14 +19,14 @@ #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::TurnSignalInfo; +using tier4_planning_msgs::msg::PathWithLaneId; struct LaneChangePath { PathWithLaneId path{}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 2d427213141b4..4bcd61c54b1be 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -25,10 +25,10 @@ #include #include -#include -#include +#include #include #include +#include #include @@ -38,10 +38,9 @@ namespace behavior_path_planner::utils::lane_change { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; @@ -54,6 +53,7 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 9e44e51d8b72d..708c561bfb629 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -20,9 +20,9 @@ #include #include -#include -#include +#include #include +#include #include #include diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 4eccd00d78c09..dd706fb9eab6d 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -57,17 +57,17 @@ namespace behavior_path_planner::utils::lane_change { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using lanelet::ArcCoordinates; +using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index a799fad0a1c36..4d348e6c9ab58 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -89,32 +89,32 @@ The Planner Manager's responsibilities include: ### Input -| Name | Required? | Type | Description | -| :---------------------------- | :-------: | :----------------------------------------------------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | -| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | -| ~/input/objects | ○ | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | -| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficSignalArray` | traffic signals information from the perception module | -| ~/input/vector_map | ○ | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map information. | -| ~/input/route | ○ | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | -| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | -| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | +| Name | Required? | Type | Description | +| :---------------------------- | :-------: | :------------------------------------------------------ | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | +| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | +| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | +| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | +| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | +| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | +| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | - ○ Mandatory: Planning Module would not work if anyone of this is not present. - △ Optional: Some module would not work, but Planning Module can still be operated. ### Output -| Name | Type | Description | QoS Durability | -| :---------------------------- | :------------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | -| ~/output/path | `autoware_auto_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | -| ~/output/turn_indicators_cmd | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | -| ~/output/hazard_lights_cmd | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | -| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | -| ~/output/stop_reasons | `tier4_planning_msgs::msg::StopReasonArray` | describe the reason for ego vehicle stop | `volatile` | -| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | +| Name | Type | Description | QoS Durability | +| :---------------------------- | :-------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | +| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | +| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | +| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | +| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | +| ~/output/stop_reasons | `tier4_planning_msgs::msg::StopReasonArray` | describe the reason for ego vehicle stop | `volatile` | +| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | ### Debug @@ -125,8 +125,8 @@ The Planner Manager's responsibilities include: | ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | | ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | | ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | -| ~/planning/path_candidate/\* | `autoware_auto_planning_msgs::msg::Path` | the path before approval. | `volatile` | -| ~/planning/path_reference/\* | `autoware_auto_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | +| ~/planning/path_candidate/\* | `autoware_planning_msgs::msg::Path` | the path before approval. | `volatile` | +| ~/planning/path_reference/\* | `autoware_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | !!! note @@ -220,7 +220,7 @@ Large vehicles require much more space, which sometimes causes them to veer out ## Generating Turn Signal -The Behavior Path Planner module uses the `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` to output turn signal commands (see [TurnIndicatorsCommand.idl](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl)). The system evaluates the driving context and determines when to activate turn signals based on its maneuver planning—like turning, lane changing, or obstacle avoidance. +The Behavior Path Planner module uses the `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` to output turn signal commands (see [TurnIndicatorsCommand.idl](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg)). The system evaluates the driving context and determines when to activate turn signals based on its maneuver planning—like turning, lane changing, or obstacle avoidance. Within this framework, the system differentiates between **desired** and **required** blinker activations. **Desired** activations are those recommended by traffic laws for typical driving scenarios, such as signaling before a lane change or turn. **Required** activations are those that are deemed mandatory for safety reasons, like signaling an abrupt lane change to avoid an obstacle. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md index 7188c03bd2604..5ffdcafdb2497 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -119,8 +119,8 @@ Scene modules receives necessary data and RTC command, and outputs candidate pat | IN | `tier4_planning_msgs::srv::CooperateCommands` | contains approval data for scene module's path modification. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md)) | | OUT | `behavior_path_planner::BehaviorModuleOutput` | contains modified path, turn signal information, etc... | | OUT | `tier4_planning_msgs::msg::CooperateStatus` | contains RTC cooperate status. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md)) | -| OUT | `autoware_auto_planning_msgs::msg::Path` | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) | -| OUT | `autoware_auto_planning_msgs::msg::Path` | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization) | +| OUT | `autoware_planning_msgs::msg::Path` | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) | +| OUT | `autoware_planning_msgs::msg::Path` | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization) | | OUT | `visualization_msgs::msg::MarkerArray` | virtual wall, debug info, etc... | Scene modules running on the manager are stored on the **candidate modules stack** or **approved modules stack** depending on the condition whether the path modification has been approved or not. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 5a49f4d9ab66e..3be680aa06662 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -24,19 +24,19 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include #include +#include #include +#include +#include #include #include #include #include #include +#include #include #include #include @@ -52,22 +52,22 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::StopReasonArray; @@ -87,14 +87,14 @@ class BehaviorPathPlannerNode : public rclcpp::Node private: rclcpp::Subscription::SharedPtr route_subscriber_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr vector_map_subscriber_; rclcpp::Subscription::SharedPtr velocity_subscriber_; rclcpp::Subscription::SharedPtr acceleration_subscriber_; rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; rclcpp::Subscription::SharedPtr costmap_subscriber_; - rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; + rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; rclcpp::Subscription::SharedPtr operation_mode_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; @@ -118,7 +118,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::unique_ptr steering_factor_interface_ptr_; Scenario::SharedPtr current_scenario_{nullptr}; - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; bool has_received_map_{false}; bool has_received_route_{false}; @@ -140,8 +140,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onCostMap(const OccupancyGrid::ConstSharedPtr msg); - void onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg); - void onMap(const HADMapBin::ConstSharedPtr map_msg); + void onTrafficSignals(const TrafficLightGroupArray::ConstSharedPtr msg); + void onMap(const LaneletMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); void onLateralOffset(const LateralOffset::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 877a8c62cbb63..4eabe5f90d877 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include @@ -39,8 +39,8 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 91a2ef5951bda..2f8f0ad0646a6 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -37,12 +37,12 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_msgs behavior_path_planner_common + behaviortree_cpp_v3 freespace_planning_algorithms frenet_planner geometry_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index a12cadb0c4994..f50cb94f436e6 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -96,7 +96,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), createSubscriptionOptions(this)); traffic_signals_subscriber_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); lateral_offset_subscriber_ = this->create_subscription( @@ -119,7 +119,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod createSubscriptionOptions(this)); // route_handler - vector_map_subscriber_ = create_subscription( + vector_map_subscriber_ = create_subscription( "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1), createSubscriptionOptions(this)); route_subscriber_ = create_subscription( @@ -339,7 +339,7 @@ void BehaviorPathPlannerNode::run() } // check for map update - HADMapBin::ConstSharedPtr map_ptr{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr{nullptr}; { std::lock_guard lk_map(mutex_map_); // for has_received_map_ and map_ptr_ if (has_received_map_) { @@ -711,8 +711,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = motion_utils::convertToPath( - *path_candidate_ptr); + output = + motion_utils::convertToPath(*path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); @@ -790,19 +790,19 @@ void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) const std::lock_guard lock(mutex_pd_); planner_data_->costmap = msg; } -void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg) +void BehaviorPathPlannerNode::onTrafficSignals(const TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); planner_data_->traffic_light_id_map.clear(); - for (const auto & signal : msg->signals) { + for (const auto & signal : msg->traffic_light_groups) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_->traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + planner_data_->traffic_light_id_map[signal.traffic_light_group_id] = traffic_signal; } } -void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) +void BehaviorPathPlannerNode::onMap(const LaneletMapBin::ConstSharedPtr msg) { const std::lock_guard lock(mutex_map_); map_ptr_ = msg; diff --git a/planning/behavior_path_planner/test/input.cpp b/planning/behavior_path_planner/test/input.cpp index c2735abd3e932..ba87af1525692 100644 --- a/planning/behavior_path_planner/test/input.cpp +++ b/planning/behavior_path_planner/test/input.cpp @@ -15,7 +15,7 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Twist; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample) diff --git a/planning/behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/test/input.hpp index ededb32f78be0..33e538156990b 100644 --- a/planning/behavior_path_planner/test/input.hpp +++ b/planning/behavior_path_planner/test/input.hpp @@ -17,21 +17,21 @@ #endif // INPUT_HPP_ -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "tier4_planning_msgs/msg/path_point_with_lane_id.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 09f3c2c66bcd4..90a2bf58a4e13 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -27,12 +27,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include #include +#include +#include #include #include #include @@ -40,6 +39,7 @@ #include #include #include +#include #include #include @@ -51,19 +51,19 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroup; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using PlanResult = PathWithLaneId::SharedPtr; using lanelet::TrafficLight; using tier4_planning_msgs::msg::VelocityLimit; @@ -72,7 +72,7 @@ using unique_identifier_msgs::msg::UUID; struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; - TrafficSignal signal; + TrafficLightGroup signal; }; struct BoolStamped diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index b6dc2997c11e8..c752e2a7f813d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -36,8 +36,8 @@ #include #include -#include #include +#include #include #include #include @@ -58,7 +58,6 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; @@ -66,6 +65,7 @@ using steering_factor_interface::SteeringFactorInterface; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; @@ -520,7 +520,7 @@ class SceneModuleInterface void setObjectsOfInterestData( const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) + const autoware_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) { for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { if (ptr) { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index 6426f16a44259..205afb81b460b 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp @@ -20,10 +20,10 @@ #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -34,9 +34,8 @@ namespace marker_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; @@ -50,6 +49,7 @@ using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index 29fe05775739e..da39bf0d0d0ba 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -24,10 +24,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include @@ -42,13 +42,13 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using route_handler::RouteHandler; +using tier4_planning_msgs::msg::PathWithLaneId; const std::map g_signal_map = { {"left", TurnIndicatorsCommand::ENABLE_LEFT}, diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index 2a4343e2c3f6a..980da88207f6d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include namespace drivable_area_expansion @@ -43,7 +43,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths MultiPolygon2d create_object_footprints( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index e438105c6645e..aa9f9d3c1b081 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -17,10 +17,10 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include #include +#include #include @@ -29,12 +29,12 @@ namespace drivable_area_expansion { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiLineString2d; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 5b1e9b4b4c419..9d18caaa7bd28 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -17,9 +17,9 @@ #include -#include #include #include +#include #include @@ -99,8 +99,7 @@ class OccupancyGridBasedCollisionDetector bool hasObstacleOnPath( const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const; bool hasObstacleOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const bool check_out_of_range) const; + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const; virtual ~OccupancyGridBasedCollisionDetector() = default; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 74f6b843803df..5bc360d3e789a 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -17,13 +17,13 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include +#include #include namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 1489dd1beff07..a70862756f8e9 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -20,10 +20,10 @@ #include -#include -#include +#include #include #include +#include #include @@ -33,10 +33,10 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct ParallelParkingParameters { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index a0b2d0afa0fa8..ec5fdb42ead03 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -18,9 +18,9 @@ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include @@ -31,8 +31,8 @@ namespace behavior_path_planner::utils::path_safety_checker::filter { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using tier4_planning_msgs::msg::PathPointWithLaneId; bool velocity_filter( const PredictedObject & object, double velocity_threshold, double max_velocity); @@ -47,9 +47,9 @@ bool position_filter( namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using tier4_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters objects based on object centroid position. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index d83c748f214b4..45e3c9e5e9908 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -31,7 +31,7 @@ namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; @@ -77,8 +77,8 @@ struct ExtendedPredictedObject geometry_msgs::msg::PoseWithCovariance initial_pose; geometry_msgs::msg::TwistWithCovariance initial_twist; geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector classification; + autoware_perception_msgs::msg::Shape shape; + std::vector classification; std::vector predicted_paths; ExtendedPredictedObject() = default; @@ -230,7 +230,7 @@ struct CollisionCheckDebug std::vector obj_predicted_path; ///< object's predicted path. Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. - autoware_auto_perception_msgs::msg::Shape obj_shape; ///< object's shape. + autoware_perception_msgs::msg::Shape obj_shape; ///< object's shape. }; using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index c4cfed01450b7..af596ddc293cd 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -31,9 +31,9 @@ namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 880e55b64d170..6651647f8d262 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -19,8 +19,8 @@ #include #include -#include #include +#include #include #include @@ -29,11 +29,11 @@ #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using tier4_autoware_utils::generateUUID; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; struct ShiftLine diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp index f109bb2cbb213..67476a587decf 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp @@ -21,10 +21,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include @@ -35,11 +35,11 @@ namespace behavior_path_planner::utils { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_planning_msgs::msg::Path; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathWithLaneId; std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index 62fd87bbd91bc..84896cc72daa3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -33,8 +33,8 @@ namespace behavior_path_planner::utils::traffic_light { -using autoware_perception_msgs::msg::TrafficSignal; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; using geometry_msgs::msg::Pose; /** diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index b61ef0579cab0..f4296073eb413 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -21,16 +21,16 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include +#include +#include #include @@ -41,19 +41,19 @@ namespace behavior_path_planner::utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct PolygonPoint { diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index cc6bb451719c8..ce0f2e716ace3 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -43,9 +43,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs freespace_planning_algorithms geometry_msgs interpolation diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 787af681bf69b..8565d737d5762 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -41,7 +41,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 } MultiPolygon2d create_object_footprints( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { MultiPolygon2d footprints; diff --git a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index df48b48d51241..7dae0fb32c79f 100644 --- a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -200,8 +200,7 @@ bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( } bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const bool check_out_of_range) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const { for (const auto & p : path.points) { const auto pose_local = global2local(costmap_, p.point.pose); diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 9400abd20b984..90790f61db1a0 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -31,7 +31,6 @@ #include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; @@ -40,6 +39,7 @@ using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPoint; using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::transformPose; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index cdb4b778d3ab3..3abd9b6eb0c05 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -398,7 +398,7 @@ TargetObjectsOnLane createTargetObjectsOnLane( bool isTargetObjectType( const PredictedObject & object, const ObjectTypesToCheck & target_object_types) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; const auto t = utils::getHighestProbLabel(object.classification); return ( (t == ObjectClassification::CAR && target_object_types.check_car) || diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp index d11bc5a68e9b5..ed583a464634c 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -67,7 +67,7 @@ PathWithLaneId resamplePathWithSpline( return path; } - std::vector transformed_path(path.points.size()); + std::vector transformed_path(path.points.size()); for (size_t i = 0; i < path.points.size(); ++i) { transformed_path.at(i) = path.points.at(i).point; } diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index d674f7770ac67..ea5d07b1697eb 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -40,7 +40,7 @@ namespace { double calcInterpolatedZ( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const tier4_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { const double closest_to_target_dist = motion_utils::calcSignedArcLength( @@ -58,7 +58,7 @@ double calcInterpolatedZ( } double calcInterpolatedVelocity( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); @@ -71,8 +71,8 @@ double calcInterpolatedVelocity( namespace behavior_path_planner::utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; @@ -272,9 +272,8 @@ bool exists(std::vector vec, T element) } std::optional findIndexOutOfGoalSearchRange( - const std::vector & points, - const Pose & goal, const int64_t goal_lane_id, - const double max_dist = std::numeric_limits::max()) + const std::vector & points, const Pose & goal, + const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) { if (points.empty()) { return std::nullopt; diff --git a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 77728cc87604d..f244845679cea 100644 --- a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -176,7 +176,7 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) { drivable_area_expansion::DrivableAreaExpansionParameters params; drivable_area_expansion::PredictedObjects dynamic_objects; - autoware_auto_mapping_msgs::msg::HADMapBin map; + autoware_map_msgs::msg::LaneletMapBin map; lanelet::LaneletMapPtr empty_lanelet_map_ptr = std::make_shared(); lanelet::utils::conversion::toBinMsg(empty_lanelet_map_ptr, &map); route_handler::RouteHandler route_handler(map); diff --git a/planning/behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner_common/test/test_safety_check.cpp index f85f52bae53ad..e2dd1ade110d0 100644 --- a/planning/behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner_common/test/test_safety_check.cpp @@ -27,7 +27,7 @@ constexpr double epsilon = 1e-6; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -143,7 +143,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.resize(5); shape.footprint.points.at(0).x = 3.0; shape.footprint.points.at(0).y = 0.0; diff --git a/planning/behavior_path_planner_common/test/test_turn_signal.cpp b/planning/behavior_path_planner_common/test/test_turn_signal.cpp index 89101ca5c1ed7..2a8ea55f8d3c3 100644 --- a/planning/behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner_common/test/test_turn_signal.cpp @@ -16,16 +16,15 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" #include #include #include -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_planning_msgs::msg::PathPoint; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using behavior_path_planner::PathWithLaneId; using behavior_path_planner::Pose; using behavior_path_planner::TurnSignalDecider; @@ -34,6 +33,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Twist; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; +using tier4_planning_msgs::msg::PathPointWithLaneId; constexpr double nearest_dist_threshold = 5.0; constexpr double nearest_yaw_threshold = M_PI / 3.0; diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 6fac6216eb740..1683d4926c71b 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -41,8 +41,8 @@ #include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -58,7 +58,7 @@ #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; struct SamplingPlannerData { // input diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index c3c5624f0d182..c09b1e237d3cf 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -13,11 +13,10 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_msgs behavior_path_planner_common bezier_sampler frenet_planner diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 08c2345df5814..c4e1c0f3c811b 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -21,8 +21,8 @@ #include -#include #include +#include #include #include @@ -32,10 +32,10 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; class SideShiftModule : public SceneModuleInterface { diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp index 210dd4e8a8d2a..f53bd42363b55 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp @@ -15,17 +15,17 @@ #ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ -#include #include #include #include +#include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; +using tier4_planning_msgs::msg::PathWithLaneId; void setOrientation(PathWithLaneId * path); diff --git a/planning/behavior_path_side_shift_module/package.xml b/planning/behavior_path_side_shift_module/package.xml index 5c421b04c0e39..c1f3b3f3cb921 100644 --- a/planning/behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_side_shift_module/package.xml @@ -18,7 +18,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_path_planner behavior_path_planner_common geometry_msgs diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 04248ee7bd5fb..4e6177cf1e1aa 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -29,7 +29,7 @@ namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp index 75e55d6eab410..561ef337fa68c 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 138aaaddd6981..68c25a7f5d39a 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -21,7 +21,7 @@ #include -#include +#include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp index 68fcb25cac88e..741ad12efe967 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp @@ -17,14 +17,14 @@ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 9a2749759d03e..e49440894f901 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -21,16 +21,16 @@ #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/util.hpp" -#include #include +#include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; enum class PlannerType { NONE = 0, diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index f831eeda9778d..3bded7ee99fbb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index ea5afb133d57f..a89b1e5e31c56 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -31,7 +31,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 5d55ce1519217..bac772627a849 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -23,11 +23,11 @@ #include -#include -#include -#include +#include +#include #include #include +#include #include @@ -36,13 +36,13 @@ namespace behavior_path_planner::start_planner_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; +using tier4_planning_msgs::msg::PathWithLaneId; PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index 1148379c26f42..ebf760405a256 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp index d3e439b3663f8..6e4135c50b253 100644 --- a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp @@ -24,7 +24,7 @@ namespace behavior_velocity_planner */ template void BlindSpotModule::setRTCStatusByDecision( - const T &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const T &, const tier4_planning_msgs::msg::PathWithLaneId & path) { static_assert("Unsupported type passed to setRTCStatus"); return; @@ -33,7 +33,7 @@ void BlindSpotModule::setRTCStatusByDecision( template void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const T & decision, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { static_assert("Unsupported type passed to reactRTCApprovalByDecision"); @@ -45,7 +45,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( template <> void BlindSpotModule::setRTCStatusByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) { return; } @@ -53,7 +53,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { return; @@ -65,7 +65,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( template <> void BlindSpotModule::setRTCStatusByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) { return; } @@ -73,7 +73,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { return; @@ -84,7 +84,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( */ template <> void BlindSpotModule::setRTCStatusByDecision( - const Unsafe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const Unsafe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) { setSafe(false); const auto & current_pose = planner_data_->current_odometry->pose; @@ -95,7 +95,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Unsafe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { if (!isActivated()) { @@ -120,7 +120,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( */ template <> void BlindSpotModule::setRTCStatusByDecision( - const Safe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const Safe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) { setSafe(true); const auto & current_pose = planner_data_->current_odometry->pose; @@ -131,8 +131,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Safe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason) + const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { if (!isActivated()) { constexpr double stop_vel = 0.0; diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 07742a1217ab3..f1b0a56018d9b 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -52,8 +52,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); } -void BlindSpotModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -86,7 +85,7 @@ void BlindSpotModuleManager::launchNewModules( std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp index 9aeaa0abfc4b7..7d6f936cc9d5b 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC private: BlindSpotModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class BlindSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 6c7c12b556581..14ebe70216d5a 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -153,7 +153,7 @@ template VisitorSwitch(Ts...) -> VisitorSwitch; void BlindSpotModule::setRTCStatus( - const BlindSpotDecision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) { std::visit( VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }}, @@ -185,7 +185,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } static bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) { for (const auto & pid : p.lane_ids) { if (pid == id) { @@ -196,7 +196,7 @@ static bool hasLaneIds( } static std::optional> findLaneIdInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) + const tier4_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) { bool found = false; size_t start = 0; @@ -223,7 +223,7 @@ static std::optional> findLaneIdInterval( } std::optional BlindSpotModule::generateInterpolatedPathInfo( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const + const tier4_planning_msgs::msg::PathWithLaneId & input_path) const { constexpr double ds = 0.2; InterpolatedPathInfo interpolated_path_info; @@ -276,8 +276,7 @@ static std::optional getFirstPointIntersectsLineByFootprint( } static std::optional getDuplicatedPointIdx( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & point) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { for (size_t i = 0; i < path.points.size(); i++) { const auto & p = path.points.at(i).point.pose.position; @@ -292,8 +291,7 @@ static std::optional getDuplicatedPointIdx( } static std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); @@ -306,8 +304,7 @@ static std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) size_t insert_idx = closest_idx; - autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point = - inout_path->points.at(closest_idx); + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -326,7 +323,7 @@ static std::optional insertPointIndex( std::optional> BlindSpotModule::generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) const + tier4_planning_msgs::msg::PathWithLaneId * path) const { // NOTE: this is optionally int for later subtraction const int margin_idx_dist = @@ -388,9 +385,9 @@ std::optional> BlindSpotModule::generateStopLine( return std::make_pair(stopline_idx_default_opt.value(), stopline_idx_critical_opt.value()); } -autoware_auto_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWithDuration( +autoware_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWithDuration( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & object_original, + const autoware_perception_msgs::msg::PredictedObject & object_original, const double time_thr) const { auto object = object_original; @@ -413,7 +410,7 @@ autoware_auto_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictP } std::optional BlindSpotModule::isOverPassJudge( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const geometry_msgs::msg::Pose & stop_point_pose) const { const auto & current_pose = planner_data_->current_odometry->pose; @@ -459,8 +456,7 @@ double BlindSpotModule::computeTimeToPassStopLine( planner_data_->current_velocity->twist.linear.x); } -std::optional -BlindSpotModule::isCollisionDetected( +std::optional BlindSpotModule::isCollisionDetected( const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, const double ego_time_to_reach_stop_line) @@ -596,7 +592,7 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) } lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const + const tier4_planning_msgs::msg::PathWithLaneId & path) const { /* get lanelet map */ const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); @@ -681,7 +677,7 @@ lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( } std::optional BlindSpotModule::generateBlindSpotPolygons( - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose) const { @@ -694,15 +690,15 @@ std::optional BlindSpotModule::generateBlindSpotPoly } bool BlindSpotModule::isTargetObjectType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { if ( object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE || + autoware_perception_msgs::msg::ObjectClassification::BICYCLE || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 709942794ec1e..b3bf90f5b91d1 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -20,10 +20,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -42,7 +42,7 @@ namespace behavior_velocity_planner struct InterpolatedPathInfo { /** the interpolated path */ - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ @@ -67,7 +67,7 @@ struct OverPassJudge struct Unsafe { const size_t stop_line_idx; - const std::optional collision_obstacle; + const std::optional collision_obstacle; }; struct Safe @@ -86,7 +86,7 @@ class BlindSpotModule : public SceneModuleInterface { std::optional virtual_wall_pose{std::nullopt}; std::optional detection_area; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_perception_msgs::msg::PredictedObjects conflicting_targets; }; public: @@ -135,21 +135,20 @@ class BlindSpotModule : public SceneModuleInterface BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); // setSafe(), setDistance() void setRTCStatus( - const BlindSpotDecision & decision, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); template void setRTCStatusByDecision( - const Decision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); // stop/GO void reactRTCApproval( const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason); template void reactRTCApprovalByDecision( - const Decision & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); std::optional generateInterpolatedPathInfo( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const; + const tier4_planning_msgs::msg::PathWithLaneId & input_path) const; std::optional getSiblingStraightLanelet( const std::shared_ptr planner_data) const; @@ -165,10 +164,10 @@ class BlindSpotModule : public SceneModuleInterface */ std::optional> generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; + tier4_planning_msgs::msg::PathWithLaneId * path) const; std::optional isOverPassJudge( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & stop_point_pose) const; double computeTimeToPassStopLine( @@ -185,7 +184,7 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return true when an object is detected in blind spot */ - std::optional isCollisionDetected( + std::optional isCollisionDetected( const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, const double ego_time_to_reach_stop_line); @@ -203,7 +202,7 @@ class BlindSpotModule : public SceneModuleInterface const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; lanelet::ConstLanelets generateBlindSpotLanelets( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const; + const tier4_planning_msgs::msg::PathWithLaneId & path) const; /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. @@ -213,7 +212,7 @@ class BlindSpotModule : public SceneModuleInterface * @return Blind spot polygons */ std::optional generateBlindSpotPolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & pose) const; @@ -222,17 +221,16 @@ class BlindSpotModule : public SceneModuleInterface * @param object Dynamic object * @return True when object belong to targeted classes */ - bool isTargetObjectType(const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + bool isTargetObjectType(const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. * @param objects_ptr target objects * @param time_thr time threshold to cut path */ - autoware_auto_perception_msgs::msg::PredictedObject cutPredictPathWithDuration( + autoware_perception_msgs::msg::PredictedObject cutPredictPathWithDuration( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double time_thr) const; + const autoware_perception_msgs::msg::PredictedObject & object, const double time_thr) const; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index b82ea7887dc98..23222d2841bad 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -221,21 +221,21 @@ To inflate the masking behind objects, their footprint can be made bigger using ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600} -| Parameter | Unit | Type | Description | -| ---------------------------------------------- | ----- | ----------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | -| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | -| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | -| `slow_down_velocity` | [m/s] | double | slow down velocity | -| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | -| `min_size` | [m] | double | minimum size of an occlusion (square side size) | -| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | -| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | -| `ignore_with_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | -| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | -| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | -| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_auto_perception_msgs::msg::ObjectClassification` for all the labels) | -| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | -| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | +| Parameter | Unit | Type | Description | +| ---------------------------------------------- | ----- | ----------- | ------------------------------------------------------------------------------------------------------------------------------------------ | +| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | +| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | +| `slow_down_velocity` | [m/s] | double | slow down velocity | +| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | +| `min_size` | [m] | double | minimum size of an occlusion (square side size) | +| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | +| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | +| `ignore_with_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | +| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | +| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | +| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_perception_msgs::msg::ObjectClassification` for all the labels) | +| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | +| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | ### Others diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 67b923a8d4cd8..e0d25c56b1610 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -83,6 +83,6 @@ ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored ignore_velocity_thresholds: default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity - custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels) + custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_perception_msgs::msg::ObjectClassification for all the labels) custom_thresholds: [0.0] # velocities of the custom labels extra_predicted_objects_size: 0.5 # [m] extra size added to the objects for masking the occlusions diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index d287d7e705542..690eca196d536 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -35,14 +35,14 @@ #include #include -#include +#include #include namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; @@ -87,14 +87,12 @@ struct DebugData std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr); diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 90b5592bb3ddb..2fd5e2bb514c3 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,9 +21,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 5110ff9993a62..fe4282893674c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -143,19 +143,17 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); cp.occlusion_ignore_velocity_thresholds.resize( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); const auto get_label = [](const std::string & s) { - if (s == "CAR") return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; - if (s == "TRUCK") return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; - if (s == "BUS") return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; - if (s == "TRAILER") return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; - if (s == "MOTORCYCLE") - return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; - if (s == "BICYCLE") return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; - if (s == "PEDESTRIAN") - return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (s == "CAR") return autoware_perception_msgs::msg::ObjectClassification::CAR; + if (s == "TRUCK") return autoware_perception_msgs::msg::ObjectClassification::TRUCK; + if (s == "BUS") return autoware_perception_msgs::msg::ObjectClassification::BUS; + if (s == "TRAILER") return autoware_perception_msgs::msg::ObjectClassification::TRAILER; + if (s == "MOTORCYCLE") return autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + if (s == "BICYCLE") return autoware_perception_msgs::msg::ObjectClassification::BICYCLE; + if (s == "PEDESTRIAN") return autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; }; const auto custom_labels = getOrDeclareParameter>( node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index 38bb63c121699..ba7e75193deab 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -23,8 +23,8 @@ #include #include -#include #include +#include #include #include @@ -35,7 +35,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC { diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 2455b36d5883f..b6adb32f642eb 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -84,11 +84,11 @@ std::vector calculate_detection_areas( return detection_areas; } -std::vector select_and_inflate_objects( - const std::vector & objects, +std::vector select_and_inflate_objects( + const std::vector & objects, const std::vector velocity_thresholds, const double inflate_size) { - std::vector selected_objects; + std::vector selected_objects; for (const auto & o : objects) { const auto vel_threshold = velocity_thresholds[o.classification.front().label]; if (o.kinematics.initial_twist_with_covariance.twist.linear.x >= vel_threshold) { @@ -103,7 +103,7 @@ std::vector select_and_infl void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, - const std::vector & objects) + const std::vector & objects) { const auto angle_cmp = [&](const auto & p1, const auto & p2) { const auto d1 = p1 - grid_map.getPosition(); @@ -139,7 +139,7 @@ bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, - const std::vector & dynamic_objects, + const std::vector & dynamic_objects, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index a76fdeb549b88..4aab9d3bfc888 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -61,7 +61,7 @@ bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, - const std::vector & dynamic_objects, + const std::vector & dynamic_objects, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief calculate the distance away from the crosswalk that should be checked for occlusions @@ -78,8 +78,8 @@ double calculate_detection_range( /// @param skip_pedestrians if true, pedestrians are not selected regardless of their velocities /// @param inflate_size [m] size by which the shape of the selected objects are inflated /// @return selected and inflated objects -std::vector select_and_inflate_objects( - const std::vector & objects, +std::vector select_and_inflate_objects( + const std::vector & objects, const double velocity_threshold, const bool skip_pedestrians, const double inflate_size); /// @brief clear occlusions behind the given objects @@ -88,7 +88,7 @@ std::vector select_and_infl /// @param objects objects void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, - const std::vector & objects); + const std::vector & objects); } // namespace behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 721af25e75ece..191eea8feabed 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -674,7 +674,7 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( } std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( - const autoware_auto_perception_msgs::msg::PredictedPath & path) const + const autoware_perception_msgs::msg::PredictedPath & path) const { using tier4_autoware_utils::Segment2d; @@ -1127,8 +1127,7 @@ bool CrosswalkModule::isRedSignalForPedestrians() const } for (const auto & element : lights) { - if ( - element.color == TrafficSignalElement::RED && element.shape == TrafficSignalElement::CIRCLE) + if (element.color == TrafficLightElement::RED && element.shape == TrafficLightElement::CIRCLE) return true; } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index faba8730aa6d9..107a904dd076b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -49,15 +49,15 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; namespace { @@ -353,7 +353,7 @@ class CrosswalkModule : public SceneModuleInterface std::optional findEgoPassageDirectionAlongPath( const PathWithLaneId & sparse_resample_path) const; std::optional findObjectPassageDirectionAlongVehicleLane( - const autoware_auto_perception_msgs::msg::PredictedPath & path) const; + const autoware_perception_msgs::msg::PredictedPath & path) const; std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index ee1b6347c9e73..d276ae95e06b6 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -55,8 +55,7 @@ using tier4_autoware_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::vector> crosswalks; @@ -90,8 +89,7 @@ std::vector> getCrosswalksOnPath( std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::set crosswalk_id_set; diff --git a/planning/behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_detection_area_module/package.xml index 66b8a62e83a2c..6bcca43edc0cb 100644 --- a/planning/behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_detection_area_module/package.xml @@ -17,7 +17,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 8e9a6b6a58a96..feb5bf6bb50ef 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -51,7 +51,7 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) } void DetectionAreaModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & detection_area_with_lane_id : planning_utils::getRegElemMapOnPath( @@ -74,7 +74,7 @@ void DetectionAreaModuleManager::launchNewModules( std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp index 10fca7182d09a..71cfa0a5eef96 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: DetectionAreaModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DetectionAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_detection_area_module/src/scene.hpp index c3f3f81cd6dfd..f135c3b2660cb 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.hpp @@ -36,7 +36,7 @@ namespace behavior_velocity_planner using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class DetectionAreaModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml index 7e9599c49bc2d..84d01d04a09a4 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs libboost-dev diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp index d57789d574ad9..8497369917232 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -62,7 +62,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, - const std::vector & objects, + const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & object_forward_footprints) { std::vector collisions; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp index b65baaeb46aa3..27a48afa032b1 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -38,7 +38,7 @@ std::optional find_closest_collision_point( /// @return the point of earliest collision along the ego path std::vector find_collisions( const EgoData & ego_data, - const std::vector & objects, + const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints); } // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp index b134ad3c39628..7397f63ca079c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 6a0c61963ac81..a58e4e9b88c75 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -29,7 +29,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop { tier4_autoware_utils::MultiPolygon2d make_forward_footprints( - const std::vector & obstacles, + const std::vector & obstacles, const PlannerParam & params, const double hysteresis) { tier4_autoware_utils::MultiPolygon2d forward_footprints; @@ -41,7 +41,7 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( } tier4_autoware_utils::Polygon2d make_forward_footprint( - const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis) { const auto & shape = obstacle.shape.dimensions; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index c28e89ac6c9f6..0050a4e2c9259 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -30,7 +30,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param [in] hysteresis [m] extra lateral distance to add to the footprints /// @return forward footprint of the obstacle tier4_autoware_utils::MultiPolygon2d make_forward_footprints( - const std::vector & obstacles, + const std::vector & obstacles, const PlannerParam & params, const double hysteresis); /// @brief create the footprint of the given obstacle and its projection over a fixed time horizon /// @param [in] obstacle obstacle @@ -38,7 +38,7 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( /// @param [in] hysteresis [m] extra lateral distance to add to the footprint /// @return forward footprint of the obstacle tier4_autoware_utils::Polygon2d make_forward_footprint( - const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index 92004b4b9f249..63f1f3025f94a 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -50,7 +50,7 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node } void DynamicObstacleStopModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -62,7 +62,7 @@ void DynamicObstacleStopModuleManager::launchNewModules( std::function &)> DynamicObstacleStopModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp index e461cc9c16445..eb7bf4c6dbc98 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -43,10 +43,10 @@ class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DynamicObstacleStopModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index c95db485781f9..205e110298f67 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -30,20 +30,19 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param params parameters /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects -std::vector filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, +std::vector filter_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { - std::vector filtered_objects; + std::vector filtered_objects; const auto is_vehicle = [](const auto & o) { return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - c.label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + return c.label == autoware_perception_msgs::msg::ObjectClassification::CAR || + c.label == autoware_perception_msgs::msg::ObjectClassification::BUS || + c.label == autoware_perception_msgs::msg::ObjectClassification::TRUCK || + c.label == autoware_perception_msgs::msg::ObjectClassification::TRAILER || + c.label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + c.label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE; }) != o.classification.end(); }; const auto is_in_range = [&](const auto & o) { diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index 22857f6db1bda..5daa0cda44203 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include @@ -30,8 +30,8 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param params parameters /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects -std::vector filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, +std::vector filter_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp index 55eaf079afee6..1df8a1ed6a4a3 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp @@ -23,7 +23,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop void update_object_map( ObjectStopDecisionMap & object_map, const std::vector & collisions, const rclcpp::Time & now, - const std::vector & path_points, + const std::vector & path_points, const PlannerParam & params) { for (auto & [object, decision] : object_map) decision.collision_detected = false; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp index cddff65da36d3..5756c8589fb73 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp @@ -61,7 +61,7 @@ using ObjectStopDecisionMap = std::unordered_map & collisions, const rclcpp::Time & now, - const std::vector & path_points, + const std::vector & path_points, const PlannerParam & params); /// @brief find the earliest collision requiring a stop along the ego path diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp index 21fca83565ee6..79911c40a195d 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp index 8575e4729d7a3..532d770608dd3 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -18,9 +18,9 @@ #include #include -#include -#include +#include #include +#include #include @@ -52,7 +52,7 @@ struct PlannerParam struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double longitudinal_offset_to_first_path_idx; // [m] geometry_msgs::msg::Pose pose; diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0bed7d32f412f..3694a395b3a53 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -18,9 +18,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common fmt geometry_msgs diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index bd2ad1935406b..252d7c2a9f61e 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -406,8 +406,8 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } if (debug_data_.traffic_light_observation) { - const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN; - const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER; + const auto GREEN = autoware_perception_msgs::msg::TrafficLightElement::GREEN; + const auto YELLOW = autoware_perception_msgs::msg::TrafficLightElement::AMBER; const auto [ego, tl_point, id, color] = debug_data_.traffic_light_observation.value(); geometry_msgs::msg::Point tl_point_point; diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index 9002c88354d68..b6ae84aa8488b 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -15,7 +15,7 @@ #ifndef INTERPOLATED_PATH_INFO_HPP_ #define INTERPOLATED_PATH_INFO_HPP_ -#include +#include #include @@ -32,7 +32,7 @@ namespace behavior_velocity_planner::intersection struct InterpolatedPathInfo { /** the interpolated path */ - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 424fb6841eca9..ac32ad553d2b7 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -297,12 +297,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) decision_state_pub_ = node.create_publisher("~/debug/intersection/decision_state", 1); - tl_observation_pub_ = node.create_publisher( + tl_observation_pub_ = node.create_publisher( "~/debug/intersection_traffic_signal", 1); } void IntersectionModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -356,7 +356,7 @@ void IntersectionModuleManager::launchNewModules( std::function &)> IntersectionModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -446,7 +446,7 @@ void IntersectionModuleManager::setActivation() } void IntersectionModuleManager::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -483,7 +483,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node } void MergeFromPrivateModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -550,7 +550,7 @@ void MergeFromPrivateModuleManager::launchNewModules( std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index 46281df2f29c7..7bbc8d51bbe9e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -23,9 +23,9 @@ #include #include -#include #include #include +#include #include #include @@ -46,10 +46,10 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC // additional for INTERSECTION_OCCLUSION RTCInterface occlusion_rtc_interface_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; @@ -57,10 +57,11 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void sendRTC(const Time & stamp) override; void setActivation() override; /* called from SceneModuleInterface::updateSceneModuleInstances */ - void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; rclcpp::Publisher::SharedPtr decision_state_pub_; - rclcpp::Publisher::SharedPtr tl_observation_pub_; + rclcpp::Publisher::SharedPtr + tl_observation_pub_; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -73,10 +74,10 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface private: MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index 420031e4df1cf..d2c905673cee9 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -40,7 +40,7 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) tier4_autoware_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) + const autoware_perception_msgs::msg::Shape & shape) { namespace bg = boost::geometry; const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); @@ -73,7 +73,7 @@ ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_st } void ObjectInfo::initialize( - const autoware_auto_perception_msgs::msg::PredictedObject & object, + const autoware_perception_msgs::msg::PredictedObject & object, std::optional attention_lanelet_opt_, std::optional stopline_opt_) { @@ -250,9 +250,8 @@ std::vector> ObjectInfoManager::allObjects() const } std::optional findPassageInterval( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, - const lanelet::BasicPolygon2d & ego_lane_poly, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, const std::optional & second_attention_lane_opt) { diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index 77e39637523a9..180496bd6b18d 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -104,7 +104,7 @@ class ObjectInfo public: explicit ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid); - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object() const + const autoware_perception_msgs::msg::PredictedObject & predicted_object() const { return predicted_object_; }; @@ -126,7 +126,7 @@ class ObjectInfo * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline */ void initialize( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + const autoware_perception_msgs::msg::PredictedObject & predicted_object, std::optional attention_lanelet_opt, std::optional stopline_opt); @@ -193,7 +193,7 @@ class ObjectInfo const std::string uuid_str; private: - autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; + autoware_perception_msgs::msg::PredictedObject predicted_object_; //! null if the object in intersection_area but not in attention_area std::optional attention_lanelet_opt{std::nullopt}; @@ -283,9 +283,8 @@ class ObjectInfoManager * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically */ std::optional findPassageInterval( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, - const lanelet::BasicPolygon2d & ego_lane_poly, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, const std::optional & second_attention_lane_opt); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 08e2d38a5905f..c27483ea1fa51 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -496,9 +496,8 @@ VisitorSwitch(Ts...) -> VisitorSwitch; template void prepareRTCByDecisionResult( - const T & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const T & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { static_assert("Unsupported type passed to prepareRTCByDecisionResult"); return; @@ -507,7 +506,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const intersection::InternalError & result, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -517,7 +516,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const intersection::OverPassJudge & result, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -526,9 +525,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::StuckStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const intersection::StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "StuckStop"); const auto closest_idx = result.closest_idx; @@ -547,7 +546,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::YieldStuckStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop"); @@ -562,7 +561,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::NonOccludedCollisionStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); @@ -581,7 +580,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::FirstWaitBeforeOcclusion & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); @@ -600,7 +599,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::PeekingTowardOcclusion & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); @@ -619,7 +618,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::OccludedAbsenceTrafficLight & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); @@ -636,7 +635,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::OccludedCollisionStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); @@ -654,7 +653,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -674,7 +673,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::FullyPrioritized & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FullyPrioritized"); @@ -692,7 +691,7 @@ void prepareRTCByDecisionResult( void IntersectionModule::prepareRTCStatus( const intersection::DecisionResult & decision_result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; double default_distance = std::numeric_limits::lowest(); @@ -713,7 +712,7 @@ template void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -727,7 +726,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const intersection::InternalError & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) @@ -742,7 +741,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const intersection::OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) @@ -755,7 +754,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -802,7 +801,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -835,7 +834,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -879,7 +878,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -930,7 +929,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -986,7 +985,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1034,7 +1033,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1088,7 +1087,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1131,7 +1130,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1172,7 +1171,7 @@ void reactRTCApprovalByDecisionResult( void IntersectionModule::reactRTCApproval( const intersection::DecisionResult & decision_result, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( @@ -1187,7 +1186,7 @@ void IntersectionModule::reactRTCApproval( bool IntersectionModule::isGreenSolidOn() const { - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; if (!last_tl_valid_observation_) { return false; @@ -1205,7 +1204,7 @@ bool IntersectionModule::isGreenSolidOn() const IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel() const { - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; auto corresponding_arrow = [&](const TrafficSignalElement & element) { if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) { @@ -1293,7 +1292,7 @@ void IntersectionModule::updateTrafficSignalObservation() } IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines) { const auto & current_pose = planner_data_->current_odometry->pose; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8c8874156f07b..b86fd77491f54 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -27,8 +27,8 @@ #include #include -#include #include +#include #include #include @@ -225,13 +225,13 @@ class IntersectionModule : public SceneModuleInterface std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; - autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; - autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; - autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects parked_targets; + autoware_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; + autoware_perception_msgs::msg::PredictedObjects unsafe_targets; + autoware_perception_msgs::msg::PredictedObjects misjudge_targets; + autoware_perception_msgs::msg::PredictedObjects too_late_detect_targets; + autoware_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_perception_msgs::msg::PredictedObjects parked_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; @@ -513,15 +513,14 @@ class IntersectionModule : public SceneModuleInterface * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const intersection::DecisionResult &, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const intersection::DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); /** * @brief act based on current RTC approval */ void reactRTCApproval( const intersection::DecisionResult & decision_result, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); /** @}*/ private: @@ -569,7 +568,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const; + tier4_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets @@ -639,15 +638,15 @@ class IntersectionModule : public SceneModuleInterface * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() */ std::optional isStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::IntersectionStopLines & intersection_stoplines, const intersection::PathLanelets & path_lanelets) const; bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; bool isTargetYieldStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief check stuck @@ -669,7 +668,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline */ std::optional isYieldStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, const intersection::IntersectionStopLines & intersection_stoplines) const; @@ -726,7 +725,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_stopline */ PassJudgeStatus isOverPassJudgeLinesStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines); /** @} */ @@ -739,7 +738,7 @@ class IntersectionModule : public SceneModuleInterface * @{ */ bool isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief find the objects on attention_area/intersection_area and update positional information @@ -759,7 +758,7 @@ class IntersectionModule : public SceneModuleInterface void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, - autoware_auto_perception_msgs::msg::PredictedPath * predicted_path) const; + autoware_perception_msgs::msg::PredictedPath * predicted_path) const; /** * @brief check if there are any objects around the stoplines on the attention areas when ego @@ -790,7 +789,7 @@ class IntersectionModule : public SceneModuleInterface * situation */ std::string generateEgoRiskEvasiveDiagnosis( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const TimeDistanceArray & ego_time_distance_array, const std::vector< std::pair>> & @@ -818,7 +817,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; /** @} */ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 8388bc15492a3..131081c5e8ca0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -35,31 +35,30 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; bool IntersectionModule::isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.collision_detection.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; @@ -208,8 +207,8 @@ void IntersectionModule::updateObjectInfoManagerCollision( bool safe_under_traffic_control = false; const auto label = predicted_object.classification.at(0).label; const auto expected_deceleration = - (label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) + (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE) ? planner_param_.collision_detection.ignore_on_amber_traffic_light .object_expected_deceleration.bike : planner_param_.collision_detection.ignore_on_amber_traffic_light @@ -232,7 +231,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( // check the PredictedPath in the ascending order of its confidence to save the safe/unsafe // CollisionKnowledge for most probable path // ========================================================================================== - std::list sorted_predicted_paths; + std::list sorted_predicted_paths; for (unsigned i = 0; i < predicted_object.kinematics.predicted_paths.size(); ++i) { sorted_predicted_paths.push_back(&predicted_object.kinematics.predicted_paths.at(i)); } @@ -400,7 +399,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( void IntersectionModule::cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, - autoware_auto_perception_msgs::msg::PredictedPath * path) const + autoware_perception_msgs::msg::PredictedPath * path) const { const rclcpp::Time current_time = clock_->now(); const auto original_path = path->path; @@ -597,7 +596,7 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( } std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const IntersectionModule::TimeDistanceArray & ego_time_distance_array, const std::vector>> & @@ -811,7 +810,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( } IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 97d05aef26137..9ea5360c3a176 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -65,7 +65,7 @@ lanelet::ConstLanelets getPrevLanelets( // end inclusive lanelet::ConstLanelet generatePathLanelet( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, const double interval) { lanelet::Points3d lefts; @@ -96,7 +96,7 @@ lanelet::ConstLanelet generatePathLanelet( } std::optional> getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const std::vector & polygons, const bool search_forward = true) { @@ -356,7 +356,7 @@ IntersectionModule::generateIntersectionStopLines( const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const + tier4_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; const double stopline_margin = planner_param_.common.default_stopline_margin; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index b26f960ec28f9..498a902c032db 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -119,7 +119,7 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; std::optional IntersectionModule::isStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::IntersectionStopLines & intersection_stoplines, const intersection::PathLanelets & path_lanelets) const { @@ -171,62 +171,60 @@ std::optional IntersectionModule::isStuckStatus( } bool IntersectionModule::isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.stuck_vehicle.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; } bool IntersectionModule::isTargetYieldStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.yield_stuck.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; @@ -309,7 +307,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection( } std::optional IntersectionModule::isYieldStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, const intersection::IntersectionStopLines & intersection_stoplines) const { diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index a44b99c97457d..0b783cf2a7ebd 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -19,10 +19,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index dfdfb01fb2234..9c492e7a64cde 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -45,8 +45,7 @@ namespace behavior_velocity_planner::util namespace bg = boost::geometry; static std::optional getDuplicatedPointIdx( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & point) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { for (size_t i = 0; i < path.points.size(); i++) { const auto & p = path.points.at(i).point.pose.position; @@ -61,8 +60,7 @@ static std::optional getDuplicatedPointIdx( } std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); @@ -75,8 +73,7 @@ std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) int insert_idx = closest_idx; - autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point = - inout_path->points.at(closest_idx); + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -94,8 +91,7 @@ std::optional insertPointIndex( } bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, - const std::set & ids) + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) { for (const auto & pid : p.lane_ids) { if (ids.find(pid) != ids.end()) { @@ -106,7 +102,7 @@ bool hasLaneIds( } std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) + const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) { bool found = false; size_t start = 0; @@ -183,7 +179,7 @@ getFirstPointInsidePolygonsByFootprint( } std::optional getFirstPointInsidePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward) { @@ -316,7 +312,7 @@ mergeLaneletsByTopologicalSort( } bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { @@ -327,7 +323,7 @@ bool isOverTargetIndex( } bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { @@ -362,7 +358,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { intersection::InterpolatedPathInfo interpolated_path_info; @@ -378,7 +374,7 @@ std::optional generateInterpolatedPath( } geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( - const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state) { if (obj_state.initial_twist_with_covariance.twist.linear.x >= 0) { return obj_state.initial_pose_with_covariance.pose; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index e37bb3ee88cc1..878253e6943a7 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -38,16 +38,14 @@ namespace behavior_velocity_planner::util * @return if insertion was successful return the inserted point index */ std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, - const std::set & ids); + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); /** * @brief find the first contiguous interval of the path points that contains the specified lane ids @@ -55,7 +53,7 @@ bool hasLaneIds( * found, returns the pair (start-1, end) */ std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); + const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** * @brief return the index of the first point which is inside the given polygon @@ -63,7 +61,7 @@ std::optional> findLaneIdsInterval( * @param[in] search_forward flag for search direction */ std::optional getFirstPointInsidePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); @@ -75,7 +73,7 @@ std::optional getFirstPointInsidePolygon( * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); /** @@ -86,7 +84,7 @@ bool isOverTargetIndex( * @return true if ego is over the target_idx */ bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); std::optional getIntersectionArea( @@ -102,11 +100,11 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( - const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state); /** * @brief this function sorts the set of lanelets topologically using topological sort and merges diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_no_drivable_lane_module/package.xml index 061cf8d367ba9..79eea68142cc8 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/package.xml @@ -19,7 +19,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index f1fa4c47e951b..0b7a8a8c28dd5 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -32,7 +32,7 @@ NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) } void NoDrivableLaneModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -56,7 +56,7 @@ void NoDrivableLaneModuleManager::launchNewModules( std::function &)> NoDrivableLaneModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp index 2b3b80510c9e4..90455bd4b1c62 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoDrivableLaneModuleManager : public SceneModuleManagerInterface private: NoDrivableLaneModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoDrivableLaneModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp index 51d3339339051..fb90e023fedcd 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class NoDrivableLaneModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp index 1cd527d93ef74..11953fd5dd177 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -15,7 +15,7 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include +#include #include #include @@ -33,8 +33,8 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and no drivable lane polygon struct PathWithNoDrivableLanePolygonIntersection diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index a9c21f1aba189..0991b37120a6f 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index 46cc224ea0f6b..faa265afe1559 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -52,7 +52,7 @@ NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) } void NoStoppingAreaModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -76,7 +76,7 @@ void NoStoppingAreaModuleManager::launchNewModules( std::function &)> NoStoppingAreaModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index f7a9a5433e900..baf5901ccc124 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: NoStoppingAreaModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoStoppingAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 7b886fea09b34..3f2e7581a4d14 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -56,8 +56,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( } boost::optional NoStoppingAreaModule::getStopLineGeometry2d( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double stop_line_margin) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const { // get stop line from map { @@ -216,8 +215,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & - predicted_obj_arr_ptr) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr) { // stuck points by predicted objects for (const auto & object : predicted_obj_arr_ptr->objects) { @@ -250,7 +248,7 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( return false; } bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) { const double stop_vel = std::numeric_limits::min(); // stuck points by stop line @@ -278,14 +276,14 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( } Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const double margin, const double extra_dist) const { Polygon2d ego_area; // open polygon double dist_from_start_sum = 0.0; const double interpolation_interval = 0.5; bool is_in_area = false; - autoware_auto_planning_msgs::msg::PathWithLaneId interpolated_path; + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger_)) { return ego_area; } @@ -350,19 +348,19 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } bool NoStoppingAreaModule::isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { if ( object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + autoware_perception_msgs::msg::ObjectClassification::CAR || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + autoware_perception_msgs::msg::ObjectClassification::BUS || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + autoware_perception_msgs::msg::ObjectClassification::TRUCK || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + autoware_perception_msgs::msg::ObjectClassification::TRAILER || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; @@ -404,13 +402,13 @@ bool NoStoppingAreaModule::isStoppable( } void NoStoppingAreaModule::insertStopPoint( - autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) { size_t insert_idx = static_cast(stop_point.first + 1); const auto stop_pose = stop_point.second; // To PathPointWithLaneId - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(insert_idx); stop_point_with_lane_id.point.pose = stop_pose; stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 84f260e5e2c2b..62ec0b88b328e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -24,10 +24,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -101,7 +101,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return true if the object has a target type */ bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief Check if there is a stopped vehicle in stuck vehicle detect area. @@ -111,8 +111,7 @@ class NoStoppingAreaModule : public SceneModuleInterface */ bool checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & - predicted_obj_arr_ptr); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr); /** * @brief Check if there is a stop line in "stop line detect area". @@ -121,7 +120,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return true if exists */ bool checkStopLinesInNoStoppingArea( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); /** * @brief Calculate the polygon of the path from the ego-car position to the end of the @@ -133,7 +132,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return generated polygon */ Polygon2d generateEgoNoStoppingAreaLanePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const; /** @@ -144,8 +143,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return generated stop line */ boost::optional getStopLineGeometry2d( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double stop_line_margin) const; + const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const; /** * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit @@ -162,7 +160,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @param stop_point stop line point on the lane */ void insertStopPoint( - autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); // Key Feature const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem_; diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml index cb5bd744edfa8..5817c2203cc5d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs grid_map_ros diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index c9dbe13474b06..710f671df609f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -55,8 +55,8 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; namespace grid_utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index f257012c05519..64adc112aab2f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -117,7 +117,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } void OcclusionSpotModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -130,7 +130,7 @@ void OcclusionSpotModuleManager::launchNewModules( std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index c153a727b8a7e..0955e4ae9aab5 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -23,11 +23,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include #include @@ -53,10 +53,10 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class OcclusionSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index f22cedbe67c8f..a650a6fa5b39f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -94,8 +94,8 @@ bool buildDetectionAreaPolygon( } void calcSlowDownPointsForPossibleCollision( - const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double offset, std::vector & possible_collisions) + const int closest_idx, const tier4_planning_msgs::msg::PathWithLaneId & path, const double offset, + std::vector & possible_collisions) { if (possible_collisions.empty()) { return; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 7f495f042d7f8..2b6d89680cd37 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -23,11 +23,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -48,19 +48,19 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::BasicLineString2d; using lanelet::BasicPoint2d; using lanelet::BasicPolygon2d; using lanelet::LaneletMapPtr; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 9a0278bfad5e5..8d7e9d2fdedd5 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -17,7 +17,7 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 8ac3fe26c1c33..1f5ca1bab904e 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -42,7 +42,7 @@ namespace { namespace utils = behavior_velocity_planner::occlusion_spot_utils; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; std::vector extractStuckVehicle( const std::vector & vehicles, const double stop_velocity) { diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 2cd7ea3582452..b7da7c073cbd9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -22,11 +22,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 85ae85f1895d9..05e73855f2642 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -24,11 +24,11 @@ using Point = geometry_msgs::msg::Point; using Vector3 = geometry_msgs::msg::Vector3; -using DynamicObjects = autoware_auto_perception_msgs::msg::PredictedObjects; -using DynamicObject = autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using DynamicObjects = autoware_perception_msgs::msg::PredictedObjects; +using DynamicObject = autoware_perception_msgs::msg::PredictedObject; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using tier4_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { @@ -41,8 +41,7 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) std::vector possible_collisions; size_t num = 2000; // make a path with 2000 points from x=0 to x=4 - autoware_auto_planning_msgs::msg::PathWithLaneId path = - test::generatePath(0.0, 3.0, 4.0, 3.0, num); + tier4_planning_msgs::msg::PathWithLaneId path = test::generatePath(0.0, 3.0, 4.0, 3.0, num); // make 2000 possible collision from x=0 to x=10 test::generatePossibleCollisions(possible_collisions, 0.0, 3.0, 4.0, 3.0, num); diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp index cf24be4cc0b5c..f7bb3d4b1ad6f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -19,22 +19,22 @@ #include -#include -#include +#include +#include #include namespace test { -inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( +inline tier4_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + tier4_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; @@ -88,7 +88,7 @@ inline void generatePossibleCollisions( intersection_pose.position.x = y0 + y_step * i; // collision path point - autoware_auto_planning_msgs::msg::PathPoint collision_with_margin{}; + autoware_planning_msgs::msg::PathPoint collision_with_margin{}; collision_with_margin.pose.position.x = x0 + x_step * i + lon; collision_with_margin.pose.position.y = y0 + y_step * i; diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index b61441132969c..d3742ea7d305f 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -14,8 +14,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 489bb3f5abc78..862ca351a118a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -113,8 +113,8 @@ void add_lanelet_markers( void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, - const double z, const size_t prev_nb) + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, const double z, + const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = "ranges"; diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp index 2b6b65638ec40..0802ae78d7a26 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -62,7 +62,7 @@ void add_lanelet_markers( /// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, const double z, const size_t prev_nb); } // namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index e948bd74eba45..126c75c2f80b9 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -61,7 +61,7 @@ bool object_is_incoming( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger) { @@ -155,7 +155,7 @@ std::optional> object_time_to_range( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger) { const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 0612cc041c1ef..4f2b0a6dad89b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -53,7 +53,7 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit @@ -67,7 +67,7 @@ std::optional> object_time_to_range( /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index de12334b91a19..80cd106bf52ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -27,7 +27,7 @@ namespace behavior_velocity_planner::out_of_lane { void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) { auto stop_line_idx = 0UL; @@ -57,7 +57,7 @@ void cut_predicted_path_beyond_line( } std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) + const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) { lanelet::ConstLanelets lanelets; lanelet::BasicLineString2d query_line; @@ -81,8 +81,8 @@ std::optional find_next_stop_line( } void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const PlannerData & planner_data, const double object_front_overhang) + autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, + const double object_front_overhang) { const auto stop_line = find_next_stop_line(predicted_path, planner_data); if (stop_line) { @@ -95,15 +95,15 @@ void cut_predicted_path_beyond_red_lights( } } -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + autoware_perception_msgs::msg::PredictedObjects filtered_objects; filtered_objects.header = planner_data.predicted_objects->header; for (const auto & object : planner_data.predicted_objects->objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; }) != object.classification.end(); if (is_pedestrian) continue; diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index bb6b5f4d00005..be3e8809d2e3d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -28,7 +28,7 @@ namespace behavior_velocity_planner::out_of_lane /// @param [in] stop_line stop line used for cutting /// @param [in] object_front_overhang extra distance to cut ahead of the stop line void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); /// @brief find the next red light stop line along the given path @@ -36,21 +36,21 @@ void cut_predicted_path_beyond_line( /// @param [in] planner_data planner data with stop line information /// @return the first red light stop line found along the path (if any) std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); + const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); /// @brief cut predicted path beyond stop lines of red lights /// @param [inout] predicted_path predicted path to cut /// @param [in] planner_data planner data to get the map and traffic light information void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const PlannerData & planner_data, const double object_front_overhang); + autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, + const double object_front_overhang); /// @brief filter predicted objects and their predicted paths /// @param [in] planner_data planner data /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 7d764722405af..840054d92252f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -82,8 +82,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.right_offset = vehicle_info.min_lateral_offset_m; } -void OutOfLaneModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void OutOfLaneModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -95,7 +94,7 @@ void OutOfLaneModuleManager::launchNewModules( std::function &)> OutOfLaneModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp index c8ef397913c8f..9da1751576a7f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -22,11 +22,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include #include @@ -51,10 +51,10 @@ class OutOfLaneModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class OutOfLaneModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 6133bb1ea0d6e..1d51c45c6afd1 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index ff690699ee638..cb2945f8b32b8 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -17,9 +17,9 @@ #include -#include -#include +#include #include +#include #include @@ -99,7 +99,7 @@ struct Slowdown struct SlowdownToInsert { Slowdown slowdown; - autoware_auto_planning_msgs::msg::PathWithLaneId::_points_type::value_type point; + tier4_planning_msgs::msg::PathWithLaneId::_points_type::value_type point; }; /// @brief bound of an overlap range (either the first, or last bound) @@ -135,7 +135,7 @@ struct OverlapRange std::vector overlaps; std::optional decision; RangeTimes times; - std::optional object{}; + std::optional object{}; } debug; }; using OverlapRanges = std::vector; @@ -172,7 +172,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] @@ -184,7 +184,7 @@ struct DecisionInputs { OverlapRanges ranges{}; EgoData ego_data; - autoware_auto_perception_msgs::msg::PredictedObjects objects{}; + autoware_perception_msgs::msg::PredictedObjects objects{}; std::shared_ptr route_handler{}; lanelet::ConstLanelets lanelets{}; }; @@ -201,7 +201,7 @@ struct DebugData lanelet::ConstLanelets path_lanelets; lanelet::ConstLanelets ignored_lanelets; lanelet::ConstLanelets other_lanelets; - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; size_t first_path_idx; size_t prev_footprints = 0; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index c85da1e551391..51511b94f3e33 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -66,7 +66,7 @@ struct PlannerData geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; static constexpr double velocity_buffer_time_sec = 10.0; std::deque velocity_buffer; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp index 624da6b170d06..dcdb4a7052cc0 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -30,10 +30,10 @@ class PluginInterface public: virtual ~PluginInterface() = default; virtual void init(rclcpp::Node & node) = 0; - virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) = 0; virtual void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::optional getFirstStopPathPointIndex() = 0; virtual const char * getModuleName() = 0; }; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp index 7aa3e6bfef9ab..abb14dd8b2356 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -28,13 +28,13 @@ class PluginWrapper : public PluginInterface { public: void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } - void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override { scene_manager_->plan(path); }; void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override + const tier4_planning_msgs::msg::PathWithLaneId & path) override { scene_manager_->updateSceneModuleInstances(planner_data, path); } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 3a2b474e3d730..3e7992207f3f1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -26,9 +26,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -48,7 +48,6 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; using motion_utils::PlanningBehavior; using motion_utils::VelocityFactor; @@ -58,6 +57,7 @@ using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_rtc_msgs::msg::Module; @@ -67,10 +67,10 @@ using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest { geometry_msgs::msg::Pose pose; - autoware_auto_perception_msgs::msg::Shape shape; + autoware_perception_msgs::msg::Shape shape; ColorName color; ObjectOfInterest( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name) : pose(pose), shape(shape), color(color_name) { @@ -144,14 +144,14 @@ class SceneModuleInterface void syncActivation() { setActivation(isSafe()); } void setObjectsOfInterestData( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name) { objects_of_interest_.emplace_back(pose, shape, color_name); } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; }; class SceneModuleManagerInterface @@ -167,22 +167,19 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path); - virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) - { - modifyPathVelocity(path); - } + virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } protected: - virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path); + virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); - virtual void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::function &)> - getModuleExpiredFunction(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path); bool isModuleRegistered(const int64_t module_id) { @@ -194,7 +191,7 @@ class SceneModuleManagerInterface void unregisterModule(const std::shared_ptr & scene_module); size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; std::set> scene_modules_; std::set registered_module_id_set_; @@ -210,7 +207,7 @@ class SceneModuleManagerInterface rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; - rclcpp::Publisher::SharedPtr pub_debug_path_; + rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; @@ -226,7 +223,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; protected: RTCInterface rtc_interface_; @@ -261,7 +258,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishObjectsOfInterestMarker(); - void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) { diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 98cf5114fb0e7..c9d292536ac13 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -159,8 +159,7 @@ std::optional findOffsetSegment( } std::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, - const double offset); + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset); template geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) @@ -192,7 +191,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse } std::optional createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const size_t lane_id, const double margin, const double vehicle_offset); } // namespace arc_lane_utils diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 0f3cc7203ef1f..bf238ecad55cb 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -17,12 +17,12 @@ #include -#include -#include -#include +#include +#include #include #include #include +#include #include #include @@ -37,13 +37,13 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( geometry_msgs::msg::PoseWithCovarianceStamped, double, cs::cartesian, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, - pose.position.y, pose.position.z) + autoware_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, pose.position.y, + pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, - point.pose.position.x, point.pose.position.y, point.pose.position.z) + tier4_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, point.pose.position.x, + point.pose.position.y, point.pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, + autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index 98df29d1c44c0..ab44af265fbaa 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -17,10 +17,10 @@ #include -#include -#include +#include #include #include +#include #include #include @@ -35,11 +35,11 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPathMarkerArray( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPointsMarkerArray( diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 667c915ac1d7f..55a82db1ae390 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -17,22 +17,22 @@ #include -#include -#include +#include +#include #include namespace behavior_velocity_planner { bool splineInterpolate( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); -autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval); -autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( - const autoware_auto_planning_msgs::msg::Path & path); -autoware_auto_planning_msgs::msg::Path filterStopPathPoint( - const autoware_auto_planning_msgs::msg::Path & path); + const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, + tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval); +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path); +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path); } // namespace behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 2184be4fbd2fe..2aadb7883a857 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -28,9 +28,9 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index e042338c53485..4ef4bb91a295d 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -53,7 +53,7 @@ struct DetectionRange struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; - autoware_perception_msgs::msg::TrafficSignal signal; + autoware_perception_msgs::msg::TrafficLightGroup signal; }; using Pose = geometry_msgs::msg::Pose; @@ -62,16 +62,16 @@ using LineString2d = tier4_autoware_utils::LineString2d; using Polygon2d = tier4_autoware_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; namespace planning_utils { size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx); // create detection area from given range return false if creation failure bool createDetectionAreaPolygons( @@ -96,7 +96,7 @@ inline int64_t bitShift(int64_t original_id) bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path); Polygon2d generatePathPolygon( const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); double calcJudgeLineDistWithAccLimit( @@ -210,9 +210,8 @@ std::set getLaneIdSetOnPath( const geometry_msgs::msg::Pose & current_pose); bool isOverLine( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, - const double offset = 0.0); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const double offset = 0.0); std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 30916709c82f7..ba847d8b1f853 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -20,9 +20,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_perception_msgs + autoware_planning_msgs autoware_velocity_smoother diagnostic_msgs eigen diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index eb43e45d55711..3092d33418c8b 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -38,7 +38,7 @@ SceneModuleInterface::SceneModuleInterface( } size_t SceneModuleInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -57,7 +57,7 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); } if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( + pub_debug_path_ = node.create_publisher( std::string("~/debug/path_with_lane_id/") + module_name, 1); } pub_virtual_wall_ = node.create_publisher( @@ -74,7 +74,7 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( } size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -83,7 +83,7 @@ size_t SceneModuleManagerInterface::findEgoSegmentIndex( void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { planner_data_ = planner_data; @@ -92,7 +92,7 @@ void SceneModuleManagerInterface::updateSceneModuleInstances( } void SceneModuleManagerInterface::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) + tier4_planning_msgs::msg::PathWithLaneId * path) { StopWatch stop_watch; stop_watch.tic("Total"); @@ -145,7 +145,7 @@ void SceneModuleManagerInterface::modifyPathVelocity( pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { - autoware_auto_planning_msgs::msg::PathWithLaneId debug_path; + tier4_planning_msgs::msg::PathWithLaneId debug_path; debug_path.header = path->header; debug_path.points = path->points; pub_debug_path_->publish(debug_path); @@ -156,7 +156,7 @@ void SceneModuleManagerInterface::modifyPathVelocity( } void SceneModuleManagerInterface::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -198,8 +198,7 @@ SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( { } -void SceneModuleManagerInterfaceWithRTC::plan( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) +void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) { setActivation(); modifyPathVelocity(path); @@ -262,7 +261,7 @@ void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() } void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 9409dddb678db..e5705b1367e0e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -98,8 +98,7 @@ std::optional checkCollision( } std::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, - const double offset) + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset) { if (offset >= 0) { return findForwardOffsetSegment(path, index, offset); @@ -109,7 +108,7 @@ std::optional findOffsetSegment( } std::optional createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const size_t lane_id, const double margin, const double vehicle_offset) { // Find collision segment diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp index 6ff2ef53176c4..00d746c56db85 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -54,7 +54,7 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( } visualization_msgs::msg::MarkerArray createPathMarkerArray( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b) { @@ -83,7 +83,7 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp index 59c9e4861fbfd..fe956e9be9512 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -26,8 +26,8 @@ constexpr double DOUBLE_EPSILON = 1e-6; namespace behavior_velocity_planner { bool splineInterpolate( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) + const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, + tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) { if (input.points.size() < 2) { RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1."); @@ -47,8 +47,8 @@ bool splineInterpolate( * is the velocity of the closest point for the input "sub-path" which consists of the points before * the interpolated point. */ -autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval) +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval) { const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")}; @@ -125,10 +125,10 @@ autoware_auto_planning_msgs::msg::Path interpolatePath( return motion_utils::resamplePath(path, s_out); } -autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( - const autoware_auto_planning_msgs::msg::Path & path) +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path) { - autoware_auto_planning_msgs::msg::Path filtered_path; + autoware_planning_msgs::msg::Path filtered_path; const double epsilon = 0.01; size_t latest_id = 0; @@ -153,10 +153,10 @@ autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( return filtered_path; } -autoware_auto_planning_msgs::msg::Path filterStopPathPoint( - const autoware_auto_planning_msgs::msg::Path & path) +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path) { - autoware_auto_planning_msgs::msg::Path filtered_path = path; + autoware_planning_msgs::msg::Path filtered_path = path; bool found_stop = false; for (size_t i = 0; i < filtered_path.points.size(); ++i) { if (std::fabs(filtered_path.points.at(i).longitudinal_velocity_mps) < 0.01) { diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 725c717082620..ecc314bca2009 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -20,8 +20,8 @@ #include #include -#include #include +#include #include @@ -35,10 +35,10 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; @@ -55,8 +55,7 @@ bool smoothPath( const auto & smoother = planner_data->velocity_smoother_; auto trajectory = - motion_utils::convertToTrajectoryPoints( - in_path); + motion_utils::convertToTrajectoryPoints(in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index a3d1aab4f9b9f..b724d01346f1e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include @@ -39,7 +39,7 @@ namespace { size_t calcPointIndexFromSegmentIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) { const size_t prev_point_idx = seg_idx; @@ -54,7 +54,7 @@ size_t calcPointIndexFromSegmentIndex( return next_point_idx; } -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) { @@ -93,7 +93,7 @@ namespace behavior_velocity_planner { namespace planning_utils { -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; using motion_utils::validateNonEmpty; @@ -105,7 +105,7 @@ using tier4_autoware_utils::createQuaternionFromYaw; using tier4_autoware_utils::getPoint; size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) { if (idx == 0) { @@ -304,7 +304,7 @@ bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.size() == 0) { return geometry_msgs::msg::Pose{}; @@ -605,9 +605,8 @@ std::vector getSubsequentLaneIdsSetOnPath( // TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex bool isOverLine( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, - const double offset) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const double offset) { return motion_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) + offset < diff --git a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp index f43450800b46e..fbc5f5d709c5c 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -82,8 +82,8 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity) TEST(specialInterpolation, specialInterpolation) { - using autoware_auto_planning_msgs::msg::Path; - using autoware_auto_planning_msgs::msg::PathPoint; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; using behavior_velocity_planner::interpolatePath; using motion_utils::calcSignedArcLength; using motion_utils::searchZeroVelocityIndex; diff --git a/planning/behavior_velocity_planner_common/test/src/utils.hpp b/planning/behavior_velocity_planner_common/test/src/utils.hpp index 1328edb6f6027..4dbf4c73fcd82 100644 --- a/planning/behavior_velocity_planner_common/test/src/utils.hpp +++ b/planning/behavior_velocity_planner_common/test/src/utils.hpp @@ -15,23 +15,23 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include -#include #include +#include +#include #include namespace test { -inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( +inline tier4_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + tier4_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index 7c79d1c288a0d..d3e2d813c3799 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -71,14 +71,14 @@ This module can handle multiple types of obstacles by creating abstracted dynami Abstracted obstacle data has following information. -| Name | Type | Description | -| ---------------- | ----------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------- | -| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | -| classifications | `std::vector` | classifications with probability | -| shape | `autoware_auto_perception_msgs::msg::Shape` | shape of the obstacle | -| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | -| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | -| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | +| Name | Type | Description | +| ---------------- | ------------------------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------- | +| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | +| classifications | `std::vector` | classifications with probability | +| shape | `autoware_perception_msgs::msg::Shape` | shape of the obstacle | +| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | +| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | +| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | Enter the maximum/minimum velocity of the object as a parameter, adding enough margin to the expected velocity. This parameter is used to create polygons for [collision detection](.#Collision-detection). diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index a0b15570f48f2..40e55b3de1a17 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -19,8 +19,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_crosswalk_module behavior_velocity_planner_common eigen diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 9b94c8b681942..edf1d8cf48b8e 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -321,7 +321,7 @@ double convertDurationToDouble(const builtin_interfaces::msg::Duration & duratio // Create a path leading up to a specified prediction time std::vector createPathToPredictionTime( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) + const autoware_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) { // Calculate the number of poses to include based on the prediction time and the time step between // poses diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index f020da2abd045..c7d8ef674aafd 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -44,17 +44,17 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using run_out_utils::DynamicObstacle; using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; using run_out_utils::PlannerParam; using run_out_utils::PredictedPath; -using PathPointsWithLaneId = std::vector; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using PathPointsWithLaneId = std::vector; /** * @brief base class for creating dynamic obstacles from multiple types of input diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 1c85a22f57bf6..fb6a70070749a 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -146,8 +146,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) setDynamicObstacleCreator(node, debug_ptr_); } -void RunOutModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) { return; @@ -162,8 +161,7 @@ void RunOutModuleManager::launchNewModules( } std::function &)> -RunOutModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +RunOutModuleManager::getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) { return [&path]([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { diff --git a/planning/behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_run_out_module/src/manager.hpp index 4dd66ad45898b..ad35caf98149a 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_run_out_module/src/manager.hpp @@ -37,10 +37,10 @@ class RunOutModuleManager : public SceneModuleManagerInterface std::shared_ptr debug_ptr_; std::unique_ptr dynamic_obstacle_creator_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_run_out_module/src/path_utils.cpp index 1cec20297683c..be7db13cd3ca3 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.cpp @@ -18,7 +18,7 @@ namespace behavior_velocity_planner::run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points) { diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp index 92aca946c13ef..8867778520bc7 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -15,8 +15,8 @@ #ifndef PATH_UTILS_HPP_ #define PATH_UTILS_HPP_ -#include #include +#include #include #include @@ -31,7 +31,7 @@ namespace run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points); diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index da826e1cf0cf6..de50b3161ea6f 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -743,7 +743,7 @@ std::optional RunOutModule::calcStopPoint( bool RunOutModule::insertStopPoint( const std::optional stop_point, - autoware_auto_planning_msgs::msg::PathWithLaneId & path) + tier4_planning_msgs::msg::PathWithLaneId & path) { // no stop point if (!stop_point) { @@ -764,7 +764,7 @@ bool RunOutModule::insertStopPoint( } // to PathPointWithLaneId - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(nearest_seg_idx); stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); @@ -880,7 +880,7 @@ void RunOutModule::insertApproachingVelocity( // to PathPointWithLaneId // use lane id of point behind inserted point - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); stop_point_with_lane_id.point.pose = *stop_point; diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 3db6051ab36e7..2f797a44d06f7 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -32,12 +32,12 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; class RunOutModule : public SceneModuleInterface @@ -124,7 +124,7 @@ class RunOutModule : public SceneModuleInterface bool insertStopPoint( const std::optional stop_point, - autoware_auto_planning_msgs::msg::PathWithLaneId & path); + tier4_planning_msgs::msg::PathWithLaneId & path); void insertVelocityForState( const std::optional & dynamic_obstacle, const PlannerData planner_data, diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index f17f4c7251ea4..c6eda901a1424 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -54,7 +54,7 @@ Polygon2d createBoostPolyFromMsg(const std::vector & std::uint8_t getHighestProbLabel(const std::vector & classification) { - std::uint8_t label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + std::uint8_t label = autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; float highest_prob = 0.0; for (const auto & _class : classification) { if (highest_prob < _class.probability) { diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 51b460482458f..9208e57dcdc7f 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -34,18 +34,18 @@ namespace behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::PathPoint; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::PathWithLaneId; using vehicle_info_util::VehicleInfo; -using PathPointsWithLaneId = std::vector; +using PathPointsWithLaneId = std::vector; struct CommonParam { double normal_min_jerk; // min jerk limit for mild stop [m/sss] diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml index 3dd16f2fd792a..9b1de66c95c92 100644 --- a/planning/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -15,7 +15,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index 5949357c90ff3..fcd000699b7ff 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -53,8 +53,7 @@ SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) static_cast(getOrDeclareParameter(node, ns + ".max_speed")); } -void SpeedBumpModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & speed_bump_with_lane_id : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -71,7 +70,7 @@ void SpeedBumpModuleManager::launchNewModules( std::function &)> SpeedBumpModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto speed_bump_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp index 480208067d87e..40fcfdd081c2e 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface private: SpeedBumpModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class SpeedBumpModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp index 34de722055d3d..f227366127046 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class SpeedBumpModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_speed_bump_module/src/util.hpp index fd114772bda60..2cd408735fd61 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.hpp @@ -30,8 +30,8 @@ #include #include -#include #include +#include #include @@ -40,9 +40,9 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point32; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and speed bump struct PathPolygonIntersectionStatus diff --git a/planning/behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_stop_line_module/package.xml index ee79d64312161..c71ecb83098fb 100644 --- a/planning/behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -18,7 +18,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 5c9f0fd39c644..b37b3b119178c 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -43,8 +43,7 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::vector stop_lines_with_lane_id; @@ -66,8 +65,7 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP } std::set StopLineModuleManager::getStopLineIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::set stop_line_id_set; @@ -78,8 +76,7 @@ std::set StopLineModuleManager::getStopLineIdSetOnPath( return stop_line_id_set; } -void StopLineModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { @@ -95,7 +92,7 @@ void StopLineModuleManager::launchNewModules( std::function &)> StopLineModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); diff --git a/planning/behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp index 4b665a3a536f7..af9dbaa083c33 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -45,17 +45,17 @@ class StopLineModuleManager : public SceneModuleManagerInterface StopLineModule::PlannerParam planner_param_; std::vector getStopLinesWithLaneIdOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); std::set getStopLineIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class StopLineModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index 544e14f130a7e..ff7b5a269db32 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -19,8 +19,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index edddef5cef4d8..01c3acf84cd63 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,17 +41,16 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); - pub_tl_state_ = node.create_publisher( + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } -void TrafficLightModuleManager::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) +void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) { visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; - autoware_perception_msgs::msg::TrafficSignal tl_state; + autoware_perception_msgs::msg::TrafficLightGroup tl_state; autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; velocity_factor_array.header.frame_id = "map"; @@ -110,7 +109,7 @@ void TrafficLightModuleManager::modifyPathVelocity( } void TrafficLightModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & traffic_light_reg_elem : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -140,7 +139,7 @@ void TrafficLightModuleManager::launchNewModules( std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 959ef2f91d36c..97340f8592a7d 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -40,12 +40,12 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC private: TrafficLightModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; - void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; bool isModuleRegisteredFromRegElement(const lanelet::Id & id, const size_t module_id) const; @@ -56,7 +56,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC const lanelet::TrafficLightConstPtr registered_element) const; // Debug - rclcpp::Publisher::SharedPtr pub_tl_state_; + rclcpp::Publisher::SharedPtr pub_tl_state_; std::optional nearest_ref_stop_path_point_index_; }; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 0158251bb42b4..c886578dc65e4 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -75,7 +75,7 @@ std::optional findNearestCollisionPoint( } bool createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, const double offset, size_t & target_point_idx, Eigen::Vector2d & target_point) { if (input.points.size() < 2) { @@ -140,7 +140,7 @@ bool createTargetPoint( } bool calcStopPointAndInsertIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, const double & stop_line_extend_length, Eigen::Vector2d & stop_line_point, size_t & stop_line_point_idx) @@ -394,12 +394,11 @@ bool TrafficLightModule::isTrafficSignalTimedOut() const return false; } -autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, - const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, - tier4_planning_msgs::msg::StopReason * stop_reason) +tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, + const Eigen::Vector2d & target_point, tier4_planning_msgs::msg::StopReason * stop_reason) { - autoware_auto_planning_msgs::msg::PathWithLaneId modified_path; + tier4_planning_msgs::msg::PathWithLaneId modified_path; modified_path = input; // Create stop pose diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index d6a409ca7034a..8385a1210d421 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -36,8 +36,8 @@ namespace behavior_velocity_planner class TrafficLightModule : public SceneModuleInterface { public: - using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; using Time = rclcpp::Time; enum class State { APPROACH, GO_OUT }; @@ -45,7 +45,8 @@ class TrafficLightModule : public SceneModuleInterface { double base_link2front; std::vector, autoware_perception_msgs::msg::TrafficSignal>> + std::shared_ptr, + autoware_perception_msgs::msg::TrafficLightGroup>> tl_state; std::vector stop_poses; geometry_msgs::msg::Pose first_stop_pose; @@ -87,10 +88,9 @@ class TrafficLightModule : public SceneModuleInterface private: bool isStopSignal(); - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, - const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, - tier4_planning_msgs::msg::StopReason * stop_reason); + tier4_planning_msgs::msg::PathWithLaneId insertStopPose( + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, + const Eigen::Vector2d & target_point, tier4_planning_msgs::msg::StopReason * stop_reason); bool isPassthrough(const double & signed_arc_length) const; diff --git a/planning/behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_virtual_traffic_light_module/package.xml index 56d958d29bdec..2cf3847c66935 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_virtual_traffic_light_module/package.xml @@ -17,7 +17,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 5310fae78c294..cd8047cff24e0 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -49,7 +49,7 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node } void VirtualTrafficLightModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -67,7 +67,7 @@ void VirtualTrafficLightModuleManager::launchNewModules( std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp index 15bd6f468132e..fa41e50d96e3f 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -38,10 +38,10 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface private: VirtualTrafficLightModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class VirtualTrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 09e2219646e82..746d35b3ecd7b 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -145,7 +145,7 @@ std::optional findLastCollisionBeforeEndLine( return {}; } -void insertStopVelocityFromStart(autoware_auto_planning_msgs::msg::PathWithLaneId * path) +void insertStopVelocityFromStart(tier4_planning_msgs::msg::PathWithLaneId * path) { for (auto & p : path->points) { p.point.longitudinal_velocity_mps = 0.0; @@ -154,7 +154,7 @@ void insertStopVelocityFromStart(autoware_auto_planning_msgs::msg::PathWithLaneI std::optional insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) + tier4_planning_msgs::msg::PathWithLaneId * path) { const auto collision_offset = motion_utils::calcLongitudinalOffsetToSegment(path->points, collision.index, collision.point); @@ -523,7 +523,7 @@ bool VirtualTrafficLightModule::hasRightOfWay( } void VirtualTrafficLightModule::insertStopVelocityAtStopLine( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) { const auto collision = @@ -586,7 +586,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } void VirtualTrafficLightModule::insertStopVelocityAtEndLine( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) { const auto collision = diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp index c8541555a6e3a..7715bb127962f 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -57,7 +57,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface struct ModuleData { geometry_msgs::msg::Pose head_pose{}; - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; std::optional stop_head_pose_at_stop_line; std::optional stop_head_pose_at_end_line; }; @@ -119,11 +119,11 @@ class VirtualTrafficLightModule : public SceneModuleInterface bool hasRightOfWay(const tier4_v2x_msgs::msg::VirtualTrafficLightState & state); void insertStopVelocityAtStopLine( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); void insertStopVelocityAtEndLine( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_walkway_module/package.xml index 2c72959d6b1d4..ea6c7803717f9 100644 --- a/planning/behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_walkway_module/package.xml @@ -17,7 +17,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_crosswalk_module behavior_velocity_planner_common geometry_msgs diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp index 453fcdb40f0db..5228445bb4e03 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface { diff --git a/planning/costmap_generator/README.md b/planning/costmap_generator/README.md index d30d8b5b58706..cb298195a82a1 100644 --- a/planning/costmap_generator/README.md +++ b/planning/costmap_generator/README.md @@ -6,12 +6,12 @@ This node reads `PointCloud` and/or `DynamicObjectArray` and creates an `Occupan ### Input topics -| Name | Type | Description | -| ------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------- | -| `~input/objects` | autoware_auto_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | -| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | -| `~input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| Name | Type | Description | +| ------------------------- | ------------------------------------------ | ---------------------------------------------------------------------------- | +| `~input/objects` | autoware_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | +| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | +| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map, for drivable areas | +| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp index 87fded0c01891..31c8a05cc3c60 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp @@ -53,8 +53,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -84,7 +84,7 @@ class CostmapGenerator : public rclcpp::Node bool use_parkinglot_; lanelet::LaneletMapPtr lanelet_map_; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; sensor_msgs::msg::PointCloud2::ConstSharedPtr points_; std::string costmap_frame_; @@ -114,9 +114,8 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_points_; - rclcpp::Subscription::SharedPtr - sub_objects_; - rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; rclcpp::Subscription::SharedPtr sub_scenario_; rclcpp::TimerBase::SharedPtr timer_; @@ -143,12 +142,12 @@ class CostmapGenerator : public rclcpp::Node void initLaneletMap(); /// \brief callback for loading lanelet2 map - void onLaneletMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onLaneletMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); /// \brief callback for DynamicObjectArray /// \param[in] in_objects input DynamicObjectArray usually from prediction or perception /// component - void onObjects(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void onObjects(const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); /// \brief callback for sensor_msgs::PointCloud2 /// \param[in] in_points input sensor_msgs::PointCloud2. Assuming ground-filtered pointcloud @@ -190,7 +189,7 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost from DynamicObjectArray /// \param[in] in_objects: subscribed DynamicObjectArray grid_map::Matrix generateObjectsCostmap( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); /// \brief calculate cost from lanelet2 map grid_map::Matrix generatePrimitivesCostmap(); diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp index 84a7807e432cd..a88bb97a623e2 100644 --- a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp +++ b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp @@ -53,7 +53,7 @@ #include #endif -#include +#include #include @@ -71,7 +71,7 @@ class ObjectsToCostmap grid_map::Matrix makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, const double size_of_expansion_kernel, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); private: const int NUMBER_OF_POINTS; @@ -84,7 +84,7 @@ class ObjectsToCostmap /// \param[in] expand_rectangle_size: expanding 4 points /// \param[out] 4 rectangle points Eigen::MatrixXd makeRectanglePoints( - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size); /// \brief make polygon(grid_map::Polygon) from 4 rectangle's points @@ -93,7 +93,7 @@ class ObjectsToCostmap /// \param[out] polygon with 4 rectangle points grid_map::Polygon makePolygonFromObjectBox( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size); /// \brief make expanded point from convex hull's point @@ -111,7 +111,7 @@ class ObjectsToCostmap /// \param[out] polygon object with convex hull points grid_map::Polygon makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size); /// \brief set cost in polygon by using DynamicObject's score diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 8200db93ae2fb..777dfc7a50a6e 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -196,12 +196,12 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) // Subscribers using std::placeholders::_1; - sub_objects_ = this->create_subscription( + sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1)); sub_points_ = this->create_subscription( "~/input/points_no_ground", rclcpp::SensorDataQoS(), std::bind(&CostmapGenerator::onPoints, this, _1)); - sub_lanelet_bin_map_ = this->create_subscription( + sub_lanelet_bin_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&CostmapGenerator::onLaneletMapBin, this, _1)); sub_scenario_ = this->create_subscription( @@ -263,7 +263,7 @@ void CostmapGenerator::loadParkingAreasFromLaneletMap( } void CostmapGenerator::onLaneletMapBin( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_); @@ -278,7 +278,7 @@ void CostmapGenerator::onLaneletMapBin( } void CostmapGenerator::onObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { objects_ = msg; } @@ -388,12 +388,12 @@ grid_map::Matrix CostmapGenerator::generatePointsCostmap( return points_costmap; } -autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( +autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( const tf2_ros::Buffer & tf_buffer, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, const std::string & target_frame_id, const std::string & src_frame_id) { - auto objects = new autoware_auto_perception_msgs::msg::PredictedObjects(); + auto objects = new autoware_perception_msgs::msg::PredictedObjects(); *objects = *in_objects; objects->header.frame_id = target_frame_id; @@ -411,11 +411,11 @@ autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformOb object.kinematics.initial_pose_with_covariance.pose = output_stamped.pose; } - return autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); + return autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); } grid_map::Matrix CostmapGenerator::generateObjectsCostmap( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { const auto object_frame = in_objects->header.frame_id; const auto transformed_objects = diff --git a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp index 79ab4a3bf66c1..f6f024fb92a4e 100644 --- a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp @@ -59,7 +59,7 @@ ObjectsToCostmap::ObjectsToCostmap() } Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size) { double length = in_object.shape.dimensions.x + expand_rectangle_size; @@ -85,7 +85,7 @@ Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectBox( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size) { grid_map::Polygon polygon; @@ -122,7 +122,7 @@ geometry_msgs::msg::Point ObjectsToCostmap::makeExpandedPoint( grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size) { grid_map::Polygon polygon; @@ -157,7 +157,7 @@ void ObjectsToCostmap::setCostInPolygon( grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, const double size_of_expansion_kernel, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { grid_map::GridMap objects_costmap = costmap; objects_costmap.add(OBJECTS_COSTMAP_LAYER_, 0); @@ -165,11 +165,11 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( for (const auto & object : in_objects->objects) { grid_map::Polygon polygon; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { polygon = makePolygonFromObjectConvexHull(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); } diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index ae124a1774881..dc94d74dce8ae 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -15,8 +15,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_map_msgs + autoware_perception_msgs grid_map_ros lanelet2_extension libpcl-all-dev diff --git a/planning/costmap_generator/test/test_objects_to_costmap.cpp b/planning/costmap_generator/test/test_objects_to_costmap.cpp index 667ee70ef53ff..15b853d0782aa 100644 --- a/planning/costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/costmap_generator/test/test_objects_to_costmap.cpp @@ -17,7 +17,7 @@ #include -using LABEL = autoware_auto_perception_msgs::msg::ObjectClassification; +using LABEL = autoware_perception_msgs::msg::ObjectClassification; class ObjectsToCostMapTest : public ::testing::Test { @@ -69,10 +69,10 @@ grid_y */ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) { - auto objs = std::make_shared(); - autoware_auto_perception_msgs::msg::PredictedObject object; + auto objs = std::make_shared(); + autoware_perception_msgs::msg::PredictedObject object; - object.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); + object.classification.push_back(autoware_perception_msgs::msg::ObjectClassification{}); object.classification.at(0).label = LABEL::CAR; object.classification.at(0).probability = 0.8; @@ -85,7 +85,7 @@ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0; object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1; - object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; object.shape.dimensions.x = 5; object.shape.dimensions.y = 3; object.shape.dimensions.z = 2; diff --git a/planning/freespace_planner/README.md b/planning/freespace_planner/README.md index b40f2681dce51..6ed0ab605ea05 100644 --- a/planning/freespace_planner/README.md +++ b/planning/freespace_planner/README.md @@ -13,19 +13,19 @@ In other words, the output trajectory doesn't include both forward and backward ### Input topics -| Name | Type | Description | -| ----------------------- | ---------------------------------- | --------------------------------------------------------- | -| `~input/route` | autoware_auto_planning_msgs::Route | route and goal pose | -| `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas | -| `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| Name | Type | Description | +| ----------------------- | ----------------------------- | --------------------------------------------------------- | +| `~input/route` | autoware_planning_msgs::Route | route and goal pose | +| `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas | +| `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped | +| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics -| Name | Type | Description | -| -------------------- | --------------------------------------- | ------------------------------------------ | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | -| `is_completed` | bool (implemented as rosparam) | whether all split trajectory are published | +| Name | Type | Description | +| -------------------- | ---------------------------------- | ------------------------------------------ | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | +| `is_completed` | bool (implemented as rosparam) | whether all split trajectory are published | ### Output TFs diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp index 2c10b1b491ace..25f3bca215063 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -38,8 +38,8 @@ #include #include -#include #include +#include #include #include #include @@ -65,8 +65,8 @@ namespace freespace_planner { -using autoware_auto_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; using freespace_planning_algorithms::AbstractPlanningAlgorithm; using freespace_planning_algorithms::AstarParam; using freespace_planning_algorithms::AstarSearch; diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index 8ebb22fdb5c2f..8445bd2355b25 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -15,7 +15,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager freespace_planning_algorithms geometry_msgs diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 1c2b4513e9c45..b0d9e9cf3edcd 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -44,9 +44,9 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; using freespace_planning_algorithms::AstarSearch; using freespace_planning_algorithms::PlannerWaypoint; using freespace_planning_algorithms::PlannerWaypoints; diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index a9eb488c71377..2f912e30b8246 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -47,10 +47,10 @@ It distributes route requests and planning results according to current MRM oper ### Subscriptions -| Name | Type | Description | -| ----------------------- | ------------------------------------ | ---------------------- | -| `~/input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2 | -| `~/input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| Name | Type | Description | +| --------------------- | ----------------------------------- | ---------------------- | +| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | +| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | ### Publications @@ -70,7 +70,7 @@ It distributes route requests and planning results according to current MRM oper ![route_sections](./media/route_sections.svg) Route section, whose type is `autoware_planning_msgs/LaneletSegment`, is a "slice" of a road that bundles lane changeable lanes. -Note that the most atomic unit of route is `autoware_auto_mapping_msgs/LaneletPrimitive`, which has the unique id of a lane in a vector map and its type. +Note that the most atomic unit of route is `autoware_planning_msgs/LaneletPrimitive`, which has the unique id of a lane in a vector map and its type. Therefore, route message does not contain geometric information about the lane since we did not want to have planning module’s message to have dependency on map data structure. The ROS message of route section contains following three elements for each route section. diff --git a/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp b/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp index 96de30ddab592..80e197e2a8ef9 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -32,12 +32,12 @@ class PlannerPlugin public: using RoutePoints = std::vector; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using MarkerArray = visualization_msgs::msg::MarkerArray; virtual ~PlannerPlugin() = default; virtual void initialize(rclcpp::Node * node) = 0; - virtual void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) = 0; + virtual void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) = 0; virtual bool ready() const = 0; virtual LaneletRoute plan(const RoutePoints & points) = 0; virtual MarkerArray visualize(const LaneletRoute & route) const = 0; diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index b8ad634c9a0d4..f878953d50420 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -18,7 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_mapping_msgs + autoware_map_msgs autoware_planning_msgs geometry_msgs lanelet2_extension diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index d4f12316052d9..5ba152af6611d 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -163,12 +163,12 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) void DefaultPlanner::initialize(rclcpp::Node * node) { initialize_common(node); - map_subscriber_ = node_->create_subscription( + map_subscriber_ = node_->create_subscription( "~/input/vector_map", rclcpp::QoS{10}.transient_local(), std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); } -void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) +void DefaultPlanner::initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) { initialize_common(node); map_callback(msg); @@ -179,7 +179,7 @@ bool DefaultPlanner::ready() const return is_graph_ready_; } -void DefaultPlanner::map_callback(const HADMapBin::ConstSharedPtr msg) +void DefaultPlanner::map_callback(const LaneletMapBin::ConstSharedPtr msg) { route_handler_.setMap(*msg); is_graph_ready_ = true; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 8c0fbf3b33955..60c232162991d 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -45,7 +45,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin { public: void initialize(rclcpp::Node * node) override; - void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) override; + void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override; bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; void updateRoute(const PlannerPlugin::LaneletRoute & route) override; @@ -63,11 +63,11 @@ class DefaultPlanner : public mission_planner::PlannerPlugin DefaultPlannerParameters param_; rclcpp::Node * node_; - rclcpp::Subscription::SharedPtr map_subscriber_; + rclcpp::Subscription::SharedPtr map_subscriber_; rclcpp::Publisher::SharedPtr pub_goal_footprint_marker_; void initialize_common(rclcpp::Node * node); - void map_callback(const HADMapBin::ConstSharedPtr msg); + void map_callback(const LaneletMapBin::ConstSharedPtr msg); /** * @brief check if the goal_footprint is within the combined lanelet of route_lanelets plus the diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index af363bcf23da7..d95bcb8caefae 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -58,7 +58,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); - sub_vector_map_ = create_subscription( + sub_vector_map_ = create_subscription( "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); sub_reroute_availability_ = create_subscription( "~/input/reroute_availability", rclcpp::QoS(1), @@ -141,7 +141,7 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha reroute_availability_ = msg; } -void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) +void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg) { map_ptr_ = msg; lanelet_map_ptr_ = std::make_shared(); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 6752ceb4eaa8b..3d712a351d94a 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -44,7 +44,7 @@ namespace mission_planner { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; @@ -84,19 +84,19 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; rclcpp::Publisher::SharedPtr pub_marker_; Odometry::ConstSharedPtr odometry_; - HADMapBin::ConstSharedPtr map_ptr_; + LaneletMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; RouteState state_; LaneletRoute::ConstSharedPtr current_route_; lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; void on_odometry(const Odometry::ConstSharedPtr msg); - void on_map(const HADMapBin::ConstSharedPtr msg); + void on_map(const LaneletMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 5ba740773d5a5..051415e9856ed 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -14,9 +14,9 @@ autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_motion_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs geometry_msgs lanelet2_extension libboost-dev diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index 585f4760290ef..efb957ea85691 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -118,7 +118,7 @@ void add_lanelet_markers( void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const std::vector & trajectory_points, + const std::vector & trajectory_points, const size_t first_ego_idx, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp index 56047e459bf51..81610f750e753 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp @@ -63,7 +63,7 @@ bool object_is_incoming( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger) { @@ -157,7 +157,7 @@ std::optional> object_time_to_range( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger) { const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp index 2aa3a8b490bf6..e691f2893dab9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -54,7 +54,7 @@ double time_along_trajectory(const EgoData & ego_data, const size_t target_idx); /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit @@ -68,7 +68,7 @@ std::optional> object_time_to_range( /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index 31631fb577b09..ce993dc1cedb0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -27,7 +27,7 @@ namespace autoware::motion_velocity_planner::out_of_lane { void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) { if (predicted_path.path.empty() || stop_line.size() < 2) return; @@ -59,7 +59,7 @@ void cut_predicted_path_beyond_line( } std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, + const autoware_perception_msgs::msg::PredictedPath & path, const std::shared_ptr planner_data) { lanelet::ConstLanelets lanelets; @@ -84,7 +84,7 @@ std::optional find_next_stop_line( } void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const std::shared_ptr planner_data, const double object_front_overhang) { const auto stop_line = find_next_stop_line(predicted_path, planner_data); @@ -98,16 +98,16 @@ void cut_predicted_path_beyond_red_lights( } } -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const std::shared_ptr planner_data, const EgoData & ego_data, const PlannerParam & params) { - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + autoware_perception_msgs::msg::PredictedObjects filtered_objects; filtered_objects.header = planner_data->predicted_objects->header; for (const auto & object : planner_data->predicted_objects->objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; }) != object.classification.end(); if (is_pedestrian) continue; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 0770fbc0dcff1..3083d85df42ab 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::out_of_lane /// @param [in] stop_line stop line used for cutting /// @param [in] object_front_overhang extra distance to cut ahead of the stop line void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); /// @brief find the next red light stop line along the given path @@ -37,14 +37,14 @@ void cut_predicted_path_beyond_line( /// @param [in] planner_data planner data with stop line information /// @return the first red light stop line found along the path (if any) std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, + const autoware_perception_msgs::msg::PredictedPath & path, const std::shared_ptr planner_data); /// @brief cut predicted path beyond stop lines of red lights /// @param [inout] predicted_path predicted path to cut /// @param [in] planner_data planner data to get the map and traffic light information void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const std::shared_ptr planner_data, const double object_front_overhang); /// @brief filter predicted objects and their predicted paths @@ -52,7 +52,7 @@ void cut_predicted_path_beyond_red_lights( /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const std::shared_ptr planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index b662186049342..2cd050d15dd9b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -147,7 +147,7 @@ void OutOfLaneModule::update_parameters(const std::vector & p } VelocityPlanningResult OutOfLaneModule::plan( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { VelocityPlanningResult result; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index 8a20d5c850a09..94f1c41fe97a6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -22,9 +22,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include @@ -40,7 +40,7 @@ class OutOfLaneModule : public PluginModuleInterface void init(rclcpp::Node & node, const std::string & module_name) override; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) override; std::string get_module_name() const override { return module_name_; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index 04f9f1ccac4c2..4fce13582aa1e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -17,9 +17,9 @@ #include -#include -#include -#include +#include +#include +#include #include #include @@ -34,7 +34,7 @@ namespace autoware::motion_velocity_planner::out_of_lane { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; /// @brief parameters for the "out of lane" module struct PlannerParam @@ -100,7 +100,7 @@ struct Slowdown struct SlowdownToInsert { Slowdown slowdown{}; - autoware_auto_planning_msgs::msg::TrajectoryPoint point{}; + autoware_planning_msgs::msg::TrajectoryPoint point{}; }; /// @brief bound of an overlap range (either the first, or last bound) @@ -136,7 +136,7 @@ struct OverlapRange std::vector overlaps{}; std::optional decision{}; RangeTimes times{}; - std::optional object{}; + std::optional object{}; } debug; }; using OverlapRanges = std::vector; @@ -173,7 +173,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - std::vector trajectory_points{}; + std::vector trajectory_points{}; size_t first_trajectory_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] @@ -185,7 +185,7 @@ struct DecisionInputs { OverlapRanges ranges{}; EgoData ego_data{}; - autoware_auto_perception_msgs::msg::PredictedObjects objects{}; + autoware_perception_msgs::msg::PredictedObjects objects{}; std::shared_ptr route_handler{}; lanelet::ConstLanelets lanelets{}; }; @@ -202,7 +202,7 @@ struct DebugData lanelet::ConstLanelets trajectory_lanelets; lanelet::ConstLanelets ignored_lanelets; lanelet::ConstLanelets other_lanelets; - std::vector trajectory_points; + std::vector trajectory_points; size_t first_trajectory_idx; size_t prev_footprints = 0; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp index 4961cd064efaf..6d1ba8084b821 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp @@ -14,9 +14,9 @@ #include "../src/filter_predicted_objects.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -24,7 +24,7 @@ TEST(TestCollisionDistance, CutPredictedPathBeyondLine) { using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line; - autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + autoware_perception_msgs::msg::PredictedPath predicted_path; lanelet::BasicLineString2d stop_line; double object_front_overhang = 0.0; const auto eps = 1e-9; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index c8d0ac54ca969..add6b5ef392ea 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -19,10 +19,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -50,7 +50,7 @@ namespace autoware::motion_velocity_planner struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; - autoware_perception_msgs::msg::TrafficSignal signal; + autoware_perception_msgs::msg::TrafficLightGroup signal; }; struct PlannerData { @@ -63,7 +63,7 @@ struct PlannerData geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; std::shared_ptr route_handler; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp index 9011d3fb7e079..dd2b89e3261d6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -37,7 +37,7 @@ class PluginModuleInterface virtual void init(rclcpp::Node & node, const std::string & module_name) = 0; virtual void update_parameters(const std::vector & parameters) = 0; virtual VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; motion_utils::VelocityFactorInterface velocity_factor_interface_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index b3eaaccd11d13..2f4e7241d60ee 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -15,8 +15,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_velocity_smoother behavior_velocity_planner_common eigen diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md index 9190adf1ff67e..494446828e134 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -17,23 +17,23 @@ This means that to stop before a wall, a stop point is inserted in the trajector ## Input topics -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------- | ----------------------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | input trajectory | -| `~/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | -| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | -| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | -| `~/input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | -| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | -| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | +| Name | Type | Description | +| -------------------------------------- | ----------------------------------------------------- | ----------------------------- | +| `~/input/trajectory` | autoware_planning_msgs::msg::Trajectory | input trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | +| `~/input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | +| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | ## Output topics | Name | Type | Description | | --------------------------- | ------------------------------------------------- | -------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | | `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | ## Services diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 0cbf220cd0955..e45935b36b13a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -17,10 +17,10 @@ rosidl_default_generators - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_motion_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs autoware_velocity_smoother diagnostic_msgs eigen diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 7c2c56c332865..4d152afa250b7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -60,11 +60,11 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & using std::placeholders::_2; // Subscribers - sub_trajectory_ = this->create_subscription( + sub_trajectory_ = this->create_subscription( "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1), create_subscription_options(this)); sub_predicted_objects_ = - this->create_subscription( + this->create_subscription( "~/input/dynamic_objects", 1, std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1), create_subscription_options(this)); @@ -78,12 +78,12 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), create_subscription_options(this)); - sub_lanelet_map_ = this->create_subscription( + sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1), create_subscription_options(this)); sub_traffic_signals_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1), create_subscription_options(this)); @@ -104,7 +104,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Publishers trajectory_pub_ = - this->create_publisher("~/output/trajectory", 1); + this->create_publisher("~/output/trajectory", 1); velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); @@ -183,7 +183,7 @@ void MotionVelocityPlannerNode::on_occupancy_grid( } void MotionVelocityPlannerNode::on_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.predicted_objects = msg; @@ -245,7 +245,7 @@ void MotionVelocityPlannerNode::set_velocity_smoother_params() } void MotionVelocityPlannerNode::on_lanelet_map( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -254,7 +254,7 @@ void MotionVelocityPlannerNode::on_lanelet_map( } void MotionVelocityPlannerNode::on_traffic_signals( - const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -263,27 +263,29 @@ void MotionVelocityPlannerNode::on_traffic_signals( const auto traffic_light_id_map_last_observed_old = planner_data_.traffic_light_id_map_last_observed_; planner_data_.traffic_light_id_map_last_observed_.clear(); - for (const auto & signal : msg->signals) { + for (const auto & signal : msg->traffic_light_groups) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_light_group_id] = traffic_signal; const bool is_unknown_observation = std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { - return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + return element.color == autoware_perception_msgs::msg::TrafficLightElement::UNKNOWN; }); // if the observation is UNKNOWN and past observation is available, only update the timestamp // and keep the body of the info - const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + const auto old_data = + traffic_light_id_map_last_observed_old.find(signal.traffic_light_group_id); if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { // copy last observation - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = old_data->second; // update timestamp - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id].stamp = msg->stamp; } else { - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = + traffic_signal; } } } @@ -296,7 +298,7 @@ void MotionVelocityPlannerNode::on_virtual_traffic_light_states( } void MotionVelocityPlannerNode::on_trajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) { std::unique_lock lk(mutex_); @@ -328,7 +330,7 @@ void MotionVelocityPlannerNode::on_trajectory( } void MotionVelocityPlannerNode::insert_stop( - autoware_auto_planning_msgs::msg::Trajectory & trajectory, + autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const { const auto seg_idx = motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); @@ -342,7 +344,7 @@ void MotionVelocityPlannerNode::insert_stop( } void MotionVelocityPlannerNode::insert_slowdown( - autoware_auto_planning_msgs::msg::Trajectory & trajectory, + autoware_planning_msgs::msg::Trajectory & trajectory, const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const { const auto from_seg_idx = @@ -403,10 +405,10 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s return traj_smoothed; } -autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( +autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) { - autoware_auto_planning_msgs::msg::Trajectory output_trajectory_msg; + autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; if (smooth_velocity_before_planning_) input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index ecf128bf9a012..4b943dcbb67f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -24,9 +24,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -42,11 +42,11 @@ namespace autoware::motion_velocity_planner { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_motion_velocity_planner_node::srv::LoadPlugin; using autoware_motion_velocity_planner_node::srv::UnloadPlugin; -using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoints = std::vector; class MotionVelocityPlannerNode : public rclcpp::Node { @@ -59,35 +59,35 @@ class MotionVelocityPlannerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // subscriber - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr sub_predicted_objects_; rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; rclcpp::Subscription::SharedPtr sub_acceleration_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr sub_traffic_signals_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; rclcpp::Subscription::SharedPtr sub_occupancy_grid_; void on_trajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); void on_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); - void on_lanelet_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_lanelet_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); void on_traffic_signals( - const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); void on_virtual_traffic_light_states( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); // publishers - rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; @@ -101,7 +101,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node // members PlannerData planner_data_; MotionVelocityPlannerManager planner_manager_; - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; bool has_received_map_ = false; rclcpp::Service::SharedPtr srv_load_plugin_; @@ -120,15 +120,15 @@ class MotionVelocityPlannerNode : public rclcpp::Node // function bool is_data_ready() const; void insert_stop( - autoware_auto_planning_msgs::msg::Trajectory & trajectory, + autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const; void insert_slowdown( - autoware_auto_planning_msgs::msg::Trajectory & trajectory, + autoware_planning_msgs::msg::Trajectory & trajectory, const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const; autoware::motion_velocity_planner::TrajectoryPoints smooth_trajectory( const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, const autoware::motion_velocity_planner::PlannerData & planner_data) const; - autoware_auto_planning_msgs::msg::Trajectory generate_trajectory( + autoware_planning_msgs::msg::Trajectory generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); std::unique_ptr logger_configure_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index fa3a3b1ae5dcb..66063fcbaebca 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -72,7 +72,7 @@ void MotionVelocityPlannerManager::update_module_parameters( } std::vector MotionVelocityPlannerManager::plan_velocities( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { std::vector results; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp index a5e330535fc73..70af1041ec222 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -20,9 +20,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -45,7 +45,7 @@ class MotionVelocityPlannerManager void unload_module_plugin(rclcpp::Node & node, const std::string & name); void update_module_parameters(const std::vector & parameters); std::vector plan_velocities( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data); private: diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp index 32262ab7eec35..6a6bd2c0dcf19 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp @@ -15,7 +15,7 @@ #ifndef OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ #define OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ -#include +#include #include #include @@ -24,7 +24,7 @@ namespace objects_of_interest_marker_interface struct ObjectMarkerData { geometry_msgs::msg::Pose pose{}; - autoware_auto_perception_msgs::msg::Shape shape{}; + autoware_perception_msgs::msg::Shape shape{}; std_msgs::msg::ColorRGBA color; }; diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp index c1fae5ecc4bec..5a094e349c31a 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp index d5a19fede4fc2..7138ddd49d4c7 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include #include @@ -47,7 +47,7 @@ class ObjectsOfInterestMarkerInterface * @param color_name Color name */ void insertObjectData( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name); /** @@ -57,7 +57,7 @@ class ObjectsOfInterestMarkerInterface * @param color Color data with alpha */ void insertObjectDataWithCustomColor( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const std_msgs::msg::ColorRGBA & color); /** diff --git a/planning/objects_of_interest_marker_interface/package.xml b/planning/objects_of_interest_marker_interface/package.xml index 2cdb0fe2c9a46..28d00e2fdcd95 100644 --- a/planning/objects_of_interest_marker_interface/package.xml +++ b/planning/objects_of_interest_marker_interface/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs rclcpp std_msgs diff --git a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index 03e71193cc5a5..f12731aba5df2 100644 --- a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -20,7 +20,7 @@ namespace objects_of_interest_marker_interface { -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; using visualization_msgs::msg::Marker; diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 8016107d96911..23be6908cbcfe 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -15,17 +15,17 @@ The `obstacle_cruise_planner` package has following modules. ### Input topics -| Name | Type | Description | -| -------------------- | ----------------------------------------------- | ---------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | -| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | +| Name | Type | Description | +| -------------------- | ------------------------------------------ | ---------------- | +| `~/input/trajectory` | autoware_planning_msgs::Trajectory | input trajectory | +| `~/input/objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | ### Output topics | Name | Type | Description | | ------------------------------- | ---------------------------------------------- | ------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | +| `~/output/trajectory` | autoware_planning_msgs::Trajectory | output trajectory | | `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | | `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | | `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | @@ -46,7 +46,7 @@ This planner data is created first, and then sent to the planning algorithm. struct PlannerData { rclcpp::Time current_time; - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; geometry_msgs::msg::Pose current_pose; double ego_vel; double current_acc; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index d46062dd8f85c..979cef8610cbd 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -19,9 +19,9 @@ #include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp" #include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,13 +38,13 @@ using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::AccelStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index eaba45a31869a..e394add65b05d 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -18,8 +18,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py index 6a11c916124e9..e3ede845a842d 100755 --- a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py +++ b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py @@ -15,7 +15,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory from geometry_msgs.msg import Pose from geometry_msgs.msg import Twist from geometry_msgs.msg import TwistStamped diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index ef5b0facd236e..c922874594f54 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -13,21 +13,21 @@ ### Input topics -| Name | Type | Description | -| --------------------------- | ----------------------------------------------- | ------------------- | -| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | -| `~/input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map | -| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | -| `~/input/dynamic_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | +| Name | Type | Description | +| --------------------------- | ------------------------------------------ | ------------------- | +| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | +| `~/input/trajectory` | autoware_planning_msgs::Trajectory | trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | +| `~/input/dynamic_objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | ### Output topics -| Name | Type | Description | -| ---------------------- | --------------------------------------- | -------------------------------------- | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | -| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | +| Name | Type | Description | +| ---------------------- | ------------------------------------ | -------------------------------------- | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | +| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | ### Common Parameter diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 2c7308e576129..55501cf596b07 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include #include @@ -31,9 +31,9 @@ namespace motion_planning { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; class AdaptiveCruiseController { public: @@ -45,7 +45,7 @@ class AdaptiveCruiseController const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); @@ -192,7 +192,7 @@ class AdaptiveCruiseController const std_msgs::msg::Header & trajectory_header); double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx); std::optional estimatePointVelocityFromObject( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const double traj_yaw, const pcl::PointXYZ & nearest_collision_point); std::optional estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index d894544a67fe1..723d7e8d12e56 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -29,8 +29,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -77,9 +77,9 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; @@ -94,7 +94,7 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; struct ObstacleWithDetectionTime { explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 6b8e2b8b3ec9d..1d1e8a3a9844d 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include #include @@ -33,7 +33,7 @@ namespace motion_planning using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::Pose; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; struct StopPoint { @@ -256,7 +256,7 @@ struct PlannerData pcl::PointXYZ lateral_nearest_slow_down_point; - autoware_auto_perception_msgs::msg::Shape slow_down_object_shape{}; + autoware_perception_msgs::msg::Shape slow_down_object_shape{}; rclcpp::Time nearest_collision_point_time{}; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index d4a7d92d8f161..b43e51873c09e 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -45,15 +45,15 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using std_msgs::msg::Header; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using PointVariant = std::variant; std::optional> calcFeasibleMarginAndVelocity( @@ -111,10 +111,10 @@ void getLateralNearestPointForPredictedObject( Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertBoundingBoxObjectToGeometryPolygon( const Pose & current_pose, const double & base_to_front, const double & base_to_rear, @@ -145,7 +145,7 @@ std::optional getObstacleFromUuid( bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr); diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 1a6f8433875ce..fcf09b96c1345 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -21,8 +21,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager diagnostic_msgs motion_utils diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 965d52a882568..2931da5830a60 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -198,7 +198,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header) { @@ -306,7 +306,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( calcUpperVelocity(col_point_distance, point_velocity, current_velocity); pub_debug_->publish(debug_values_); - if (target_object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (target_object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // if the target object is obstacle return stop true RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), @@ -399,7 +399,7 @@ double AdaptiveCruiseController::calcTrajYaw( } std::optional AdaptiveCruiseController::estimatePointVelocityFromObject( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const double traj_yaw, const pcl::PointXYZ & nearest_collision_point) { geometry_msgs::msg::Point nearest_collision_p_ros; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 8f57b3e0f051a..31c044c93af22 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -40,7 +40,7 @@ namespace motion_planning { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; @@ -628,7 +628,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_slow_down_range; bool found_slow_down_object = false; Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -636,7 +636,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( slow_down_param_.pedestrian_lateral_margin); found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.vehicle_lateral_margin); @@ -647,7 +647,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.unknown_lateral_margin); @@ -722,15 +722,15 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_slow_down_vehicle_polygon; const auto & obj = filtered_objects.objects.at(nearest_slow_down_object_index); - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.pedestrian_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.vehicle_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.unknown_lateral_margin); @@ -763,7 +763,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_collision_polygon; bool found_collision_object = false; Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); @@ -772,7 +772,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.pedestrian_lateral_margin); found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = convertBoundingBoxObjectToGeometryPolygon( @@ -782,7 +782,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.vehicle_lateral_margin); found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -846,7 +846,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d object_polygon{}; Polygon2d one_step_move_vehicle_polygon; bool found_collision_object = false; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); @@ -855,7 +855,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.pedestrian_lateral_margin); found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = convertBoundingBoxObjectToGeometryPolygon( @@ -865,7 +865,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.vehicle_lateral_margin); found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -1128,7 +1128,7 @@ void ObstacleStopPlannerNode::insertVelocity( if (node_param_.use_predicted_objects) { if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + autoware_perception_msgs::msg::Shape::CYLINDER) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * @@ -1136,7 +1136,7 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_param_.pedestrian_lateral_margin; } else if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * @@ -1144,7 +1144,7 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_param_.vehicle_lateral_margin; } else if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::POLYGON) { + autoware_perception_msgs::msg::Shape::POLYGON) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 31fb843c234d9..3e2ecca8ec4d7 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -32,8 +32,8 @@ namespace motion_planning { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -632,7 +632,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( } Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; @@ -655,7 +655,7 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; tf2::Transform tf_map2obj; @@ -698,13 +698,13 @@ bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & ob return base_pose_vec.dot(obstacle_vec) >= 0; } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index 36aa35bb31156..9afe338cc17e0 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -146,22 +146,22 @@ For example a value of `1` means all trajectory points will be evaluated while a ### Inputs -| Name | Type | Description | -| ----------------------------- | ------------------------------------------------ | -------------------------------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | -| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | -| `~/input/dynamic_obstacles` | `autoware_auto_perception_msgs/PredictedObjects` | Dynamic objects | -| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | -| `~/input/map` | `autoware_auto_mapping_msgs/HADMapBin` | Vector map used to retrieve static obstacles | +| Name | Type | Description | +| ----------------------------- | ------------------------------------------- | -------------------------------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | +| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | +| `~/input/dynamic_obstacles` | `autoware_perception_msgs/PredictedObjects` | Dynamic objects | +| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | +| `~/input/map` | `autoware_map_msgs/LaneletMapBin` | Vector map used to retrieve static obstacles | ### Outputs -| Name | Type | Description | -| ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | -| `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | -| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | +| Name | Type | Description | +| ------------------------------- | ----------------------------------- | -------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Trajectory with adjusted velocities | +| `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | +| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | ## Parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp index 3f5f8cd814ea3..d3074e3fb1b45 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp @@ -18,7 +18,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -#include +#include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp index 4d89d4b440c21..0beced62bd7d6 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -19,7 +19,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -#include +#include #include #include @@ -63,8 +63,8 @@ void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, - const Float buffer, const Float min_vel); + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, + const Float min_vel); /// @brief create footprint polygons from projection lines /// @details A footprint is create for each group of lines. Each group of lines is assumed to share diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 63eec0f2cf378..f0b9d34413c9a 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -69,7 +69,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node sub_pointcloud_; //!< @brief subscriber for obstacle pointcloud rclcpp::Subscription::SharedPtr sub_odom_; //!< @brief subscriber for the current velocity - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; // cached inputs PredictedObjects::ConstSharedPtr dynamic_obstacles_ptr_; diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp index 8620c254968fe..618437edc981a 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include @@ -163,7 +163,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp index e44011e1f0ee3..6bd1b39b02f73 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -17,17 +17,17 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include +#include #include #include namespace obstacle_velocity_limiter { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using nav_msgs::msg::OccupancyGrid; using PointCloud = sensor_msgs::msg::PointCloud2; using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml index 8c9c89094f5f0..49e424bfbc9ef 100644 --- a/planning/obstacle_velocity_limiter/package.xml +++ b/planning/obstacle_velocity_limiter/package.xml @@ -11,8 +11,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager eigen grid_map_msgs diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index a54b238ad56e1..2e56e8034555a 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory import matplotlib.pyplot as plt from nav_msgs.msg import Odometry import numpy as np diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp index ac1b4bde65211..7c050b078c3aa 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp @@ -35,8 +35,8 @@ Float calculateSafeVelocity( } multi_polygon_t createPolygonMasks( - const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, - const Float buffer, const Float min_vel) + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, + const Float min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); } diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index ea08c2f7cf552..18cf534da8c7e 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -57,9 +57,9 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio sub_odom_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) { current_odometry_ptr_ = msg; }); - map_sub_ = create_subscription( + map_sub_ = create_subscription( "~/input/map", rclcpp::QoS{1}.transient_local(), - [this](const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { + [this](const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); static_map_obstacles_ = extractStaticObstacles(*lanelet_map_ptr_, obstacle_params_.static_map_tags); diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp index 603e573e68f17..5d4c1d29665df 100644 --- a/planning/obstacle_velocity_limiter/src/obstacles.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -59,7 +59,7 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp index c5e69e5fd8fb9..d4f44e7a6a775 100644 --- a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -17,8 +17,8 @@ #include "obstacle_velocity_limiter/types.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -384,8 +384,8 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { - using autoware_auto_perception_msgs::msg::PredictedObject; - using autoware_auto_perception_msgs::msg::PredictedObjects; + using autoware_perception_msgs::msg::PredictedObject; + using autoware_perception_msgs::msg::PredictedObjects; using obstacle_velocity_limiter::createObjectPolygons; PredictedObjects objects; diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index c8f6d6da98dcd..21cbcf8e7eefc 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -15,10 +15,10 @@ #ifndef PATH_SMOOTHER__TYPE_ALIAS_HPP_ #define PATH_SMOOTHER__TYPE_ALIAS_HPP_ -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" @@ -30,10 +30,10 @@ namespace path_smoother // std_msgs using std_msgs::msg::Header; // planning -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // debug diff --git a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp index 067f5d2c57cc4..55a7829a5bea3 100644 --- a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp +++ b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp @@ -24,8 +24,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/path_smoother/package.xml b/planning/path_smoother/package.xml index 0cba14254e492..a0e83fdf8a091 100644 --- a/planning/path_smoother/package.xml +++ b/planning/path_smoother/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index 6409c61700f34..b4d0950a593d5 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -18,8 +18,8 @@ #include "motion_utils/trajectory/conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index 43c3e029d4437..9d787070a9dc5 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -26,19 +26,19 @@ #include #include -#include -#include -#include -#include +#include #include #include #include +#include #include #include #include #include #include #include +#include +#include #include #include @@ -57,14 +57,14 @@ namespace test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using RouteSections = std::vector; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -137,18 +137,18 @@ LaneletSegment createLaneletSegment(int id); lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename); /** - * @brief Converts a Lanelet map to a HADMapBin message. + * @brief Converts a Lanelet map to a LaneletMapBin message. * - * This function converts a given Lanelet map to a HADMapBin message. It also + * This function converts a given Lanelet map to a LaneletMapBin message. It also * parses the format and map versions from the specified filename and includes * them in the message. The timestamp for the message is set to the provided time. * * @param map The Lanelet map to convert. * @param lanelet2_filename The filename of the Lanelet2 map, used to parse format and map versions. * @param now The current time to set in the message header. - * @return A HADMapBin message containing the converted map data. + * @return A LaneletMapBin message containing the converted map data. */ -HADMapBin convertToMapBinMsg( +LaneletMapBin convertToMapBinMsg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -175,28 +175,28 @@ LaneletRoute makeNormalRoute(); OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2); /** - * @brief Creates a HADMapBin message from a Lanelet map file. + * @brief Creates a LaneletMapBin message from a Lanelet map file. * * This function loads a Lanelet map from the given file, overwrites the - * centerline with the specified resolution, and converts the map to a HADMapBin message. + * centerline with the specified resolution, and converts the map to a LaneletMapBin message. * * @param absolute_path The absolute path to the Lanelet2 map file. * @param center_line_resolution The resolution for the centerline. - * @return A HADMapBin message containing the map data. + * @return A LaneletMapBin message containing the map data. */ -HADMapBin make_map_bin_msg( +LaneletMapBin make_map_bin_msg( const std::string & absolute_path, const double center_line_resolution = 5.0); /** - * @brief Creates a HADMapBin message using a predefined Lanelet2 map file. + * @brief Creates a LaneletMapBin message using a predefined Lanelet2 map file. * * This function loads a lanelet2_map.osm from the test_map folder in the * planning_test_utils package, overwrites the centerline with a resolution of 5.0, - * and converts the map to a HADMapBin message. + * and converts the map to a LaneletMapBin message. * - * @return A HADMapBin message containing the map data. + * @return A LaneletMapBin message containing the map data. */ -HADMapBin makeMapBinMsg(); +LaneletMapBin makeMapBinMsg(); /** * @brief Creates an Odometry message with a specified shift. @@ -366,7 +366,7 @@ void createPublisherWithQoS( std::shared_ptr> & publisher) { if constexpr ( - std::is_same_v || std::is_same_v || + std::is_same_v || std::is_same_v || std::is_same_v) { rclcpp::QoS qos(rclcpp::KeepLast(1)); qos.reliable(); diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index b236a64f48da0..8f3c3259c33d0 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -15,12 +15,11 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils lanelet2_extension diff --git a/planning/planning_test_utils/src/planning_test_utils.cpp b/planning/planning_test_utils/src/planning_test_utils.cpp index efed22855cf0f..3ba97552ef250 100644 --- a/planning/planning_test_utils/src/planning_test_utils.cpp +++ b/planning/planning_test_utils/src/planning_test_utils.cpp @@ -56,7 +56,7 @@ lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) return nullptr; } -HADMapBin convertToMapBinMsg( +LaneletMapBin convertToMapBinMsg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { std::string format_version{}; @@ -64,11 +64,11 @@ HADMapBin convertToMapBinMsg( lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); - HADMapBin map_bin_msg; + LaneletMapBin map_bin_msg; map_bin_msg.header.stamp = now; map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); return map_bin_msg; @@ -103,11 +103,12 @@ OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution) return costmap_msg; } -HADMapBin make_map_bin_msg(const std::string & absolute_path, const double center_line_resolution) +LaneletMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution) { const auto map = loadMap(absolute_path); if (!map) { - return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; + return autoware_map_msgs::msg::LaneletMapBin_>{}; } // overwrite centerline @@ -119,7 +120,7 @@ HADMapBin make_map_bin_msg(const std::string & absolute_path, const double cente return map_bin_msg; } -HADMapBin makeMapBinMsg() +LaneletMapBin makeMapBinMsg() { const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); diff --git a/planning/planning_topic_converter/README.md b/planning/planning_topic_converter/README.md index 7be6979e62281..74137cda0197d 100644 --- a/planning/planning_topic_converter/README.md +++ b/planning/planning_topic_converter/README.md @@ -2,7 +2,7 @@ ## Purpose -This package provides tools that convert topic type among types are defined in . +This package provides tools that convert topic type among types are defined in . ## Inner-workings / Algorithms diff --git a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp b/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp index f214f2a4d5e2a..708f624e97898 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp +++ b/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp @@ -18,18 +18,18 @@ #include "planning_topic_converter/converter_base.hpp" #include "rclcpp/rclcpp.hpp" -#include -#include +#include +#include #include namespace planning_topic_converter { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; class PathToTrajectory : public ConverterBase { diff --git a/planning/planning_topic_converter/package.xml b/planning/planning_topic_converter/package.xml index d07d59f3d2c89..3699f3e53900c 100644 --- a/planning/planning_topic_converter/package.xml +++ b/planning/planning_topic_converter/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto - autoware_auto_planning_msgs + autoware_planning_msgs motion_utils rclcpp rclcpp_components diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt index 709e00c5a9a25..455dded2d7e32 100644 --- a/planning/planning_validator/CMakeLists.txt +++ b/planning/planning_validator/CMakeLists.txt @@ -56,7 +56,7 @@ if(BUILD_TESTING) ) ament_target_dependencies(test_planning_validator rclcpp - autoware_auto_planning_msgs + autoware_planning_msgs ) target_link_libraries(test_planning_validator planning_validator_component diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index a260254159947..9d70e7f78a5bb 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -31,10 +31,10 @@ The following features are to be implemented. The `planning_validator` takes in the following inputs: -| Name | Type | Description | -| -------------------- | -------------------------------------- | ---------------------------------------------- | -| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | -| `~/input/trajectory` | autoware_auto_planning_msgs/Trajectory | target trajectory to be validated in this node | +| Name | Type | Description | +| -------------------- | --------------------------------- | ---------------------------------------------- | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/trajectory` | autoware_planning_msgs/Trajectory | target trajectory to be validated in this node | ### Outputs @@ -42,7 +42,7 @@ It outputs the following: | Name | Type | Description | | ---------------------------- | ------------------------------------------ | ------------------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/Trajectory | validated trajectory | +| `~/output/trajectory` | autoware_planning_msgs/Trajectory | validated trajectory | | `~/output/validation_status` | planning_validator/PlanningValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid | | `/diagnostics` | diagnostic_msgs/DiagnosticStatus | diagnostics to report errors | diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/planning_validator/debug_marker.hpp index 4c7f6907a97b2..4671a0b0d7297 100644 --- a/planning/planning_validator/include/planning_validator/debug_marker.hpp +++ b/planning/planning_validator/include/planning_validator/debug_marker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -32,8 +32,7 @@ class PlanningValidatorDebugMarkerPublisher explicit PlanningValidatorDebugMarkerPublisher(rclcpp::Node * node); void pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, - int id = 0); + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); void pushVirtualWall(const geometry_msgs::msg::Pose & pose); void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 36954c267b2a9..fed6e161c4d90 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -35,8 +35,8 @@ namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 5831993d748b6..11eaf4f37f400 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -25,8 +25,8 @@ namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; std::pair getMaxValAndIdx(const std::vector & v); diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 9ecc760efd7e3..5a413f27dc7a3 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -15,7 +15,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager diagnostic_updater geometry_msgs diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 1a0793957402a..82117d199c36e 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -40,7 +40,7 @@ void PlanningValidatorDebugMarkerPublisher::clearMarkers() } void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) { pushPoseMarker(p.pose, ns, id); } diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp index 421b60d0ab756..1d1e47ad10407 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp @@ -17,14 +17,14 @@ #include -#include +#include #include namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; class InvalidTrajectoryPublisherNode : public rclcpp::Node { diff --git a/planning/planning_validator/test/src/test_planning_validator_functions.cpp b/planning/planning_validator/test/src/test_planning_validator_functions.cpp index 482996c4c040d..e406b6c5d7da2 100644 --- a/planning/planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -21,7 +21,7 @@ #include -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using planning_validator::PlanningValidator; TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.cpp b/planning/planning_validator/test/src/test_planning_validator_helper.cpp index 19e0d5a7f73ee..de7d8293286d7 100644 --- a/planning/planning_validator/test/src/test_planning_validator_helper.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_helper.cpp @@ -19,8 +19,8 @@ #include -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createQuaternionFromYaw; Trajectory generateTrajectoryWithConstantAcceleration( diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.hpp b/planning/planning_validator/test/src/test_planning_validator_helper.hpp index d2e6472fb04bc..23b634c3f56ed 100644 --- a/planning/planning_validator/test/src/test_planning_validator_helper.hpp +++ b/planning/planning_validator/test/src/test_planning_validator_helper.hpp @@ -17,11 +17,11 @@ #include -#include -#include +#include +#include #include -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using nav_msgs::msg::Odometry; Trajectory generateTrajectoryWithConstantAcceleration( diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index a7b90be4471cd..2329a3b9bb6c8 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -29,7 +29,7 @@ * This test checks the diagnostics message published from the planning_validator node */ -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 73bf99962ea82..24a20f86ed718 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -17,11 +17,11 @@ #include -#include -#include +#include #include #include #include +#include #include #include @@ -37,13 +37,13 @@ namespace route_handler { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using std_msgs::msg::Header; +using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; using RouteSections = std::vector; @@ -55,10 +55,10 @@ class RouteHandler { public: RouteHandler() = default; - explicit RouteHandler(const HADMapBin & map_msg); + explicit RouteHandler(const LaneletMapBin & map_msg); // non-const methods - void setMap(const HADMapBin & map_msg); + void setMap(const LaneletMapBin & map_msg); void setRoute(const LaneletRoute & route_msg); void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); void clearRoute(); diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 097bc62f8bf39..8be6707e933d7 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -22,8 +22,7 @@ ament_lint_auto autoware_lint_common - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_planning_msgs geometry_msgs lanelet2_extension diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 4dd559b9bbc98..d410cb2668175 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -22,9 +22,9 @@ #include #include -#include -#include #include +#include +#include #include #include @@ -43,12 +43,12 @@ namespace { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; using lanelet::utils::to2D; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; bool exists(const std::vector & primitives, const int64_t & id) { @@ -133,13 +133,13 @@ std::string toString(const geometry_msgs::msg::Pose & pose) namespace route_handler { -RouteHandler::RouteHandler(const HADMapBin & map_msg) +RouteHandler::RouteHandler(const LaneletMapBin & map_msg) { setMap(map_msg); route_ptr_ = nullptr; } -void RouteHandler::setMap(const HADMapBin & map_msg) +void RouteHandler::setMap(const LaneletMapBin & map_msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index 3c6893da3f9d1..9f9fba717a4aa 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include @@ -37,7 +37,7 @@ namespace route_handler::test { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; class TestRouteHandler : public ::testing::Test { diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp index 5b395883dd65d..6f0bef882743c 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -22,7 +22,7 @@ #include #include -#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path.hpp" #include #include diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index b82896f4fd93f..27009719c230c 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -19,17 +19,17 @@ Note that the velocity is just taken over from the input path. ### input -| Name | Type | Description | -| ------------------ | -------------------------------------------------- | -------------------------------------------------- | -| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | -| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | -| `~/input/objects` | autoware_auto_perception_msgs/msg/PredictedObjects | objects to avoid | +| Name | Type | Description | +| ------------------ | --------------------------------------------- | -------------------------------------------------- | +| `~/input/path` | autoware_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | +| `~/input/objects` | autoware_perception_msgs/msg/PredictedObjects | objects to avoid | ### output -| Name | Type | Description | -| --------------------- | ------------------------------------------ | ----------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/msg/Trajectory | generated trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs/msg/Trajectory | generated trajectory that is feasible to drive and collision-free | ## Algorithm diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp index 41493b74659fc..448400236c9f8 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp @@ -22,7 +22,7 @@ #include "sampler_common/structures.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include +#include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp index 29cfddfbf8632..a9a2728b27c49 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp @@ -20,11 +20,11 @@ #include "path_sampler/type_alias.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp index 8a792eba2f237..90e655c086444 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp @@ -15,11 +15,11 @@ #ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ #define PATH_SAMPLER__TYPE_ALIAS_HPP_ -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -33,12 +33,12 @@ namespace path_sampler // std_msgs using std_msgs::msg::Header; // planning -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; // perception -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; // navigation using nav_msgs::msg::Odometry; // visualization diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp index 3e5afdbdad696..875b81b9cd49a 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp @@ -24,8 +24,8 @@ #include "path_sampler/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp index d848e9b209d28..a036dcfe638c2 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp @@ -24,8 +24,8 @@ #include "path_sampler/type_alias.hpp" #include "sampler_common/structures.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include #include diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/path_sampler/package.xml index 22303a021ff63..a57ff6e6033f6 100644 --- a/planning/sampling_based_planner/path_sampler/package.xml +++ b/planning/sampling_based_planner/path_sampler/package.xml @@ -13,7 +13,8 @@ autoware_cmake - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs bezier_sampler frenet_planner geometry_msgs diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp index 4ae6382b82383..2d1587c7b45a3 100644 --- a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp +++ b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index ad516faa717ff..07bd4f32787de 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -19,8 +19,8 @@ #include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/scenario_selector/README.md b/planning/scenario_selector/README.md index 0aea2c876cf35..373ac164e5057 100644 --- a/planning/scenario_selector/README.md +++ b/planning/scenario_selector/README.md @@ -6,21 +6,21 @@ ### Input topics -| Name | Type | Description | -| -------------------------------- | --------------------------------------- | ----------------------------------------------------- | -| `~input/lane_driving/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of LaneDriving scenario | -| `~input/parking/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of Parking scenario | -| `~input/lanelet_map` | autoware_auto_mapping_msgs::HADMapBin | | -| `~input/route` | autoware_planning_msgs::LaneletRoute | route and goal pose | -| `~input/odometry` | nav_msgs::Odometry | for checking whether vehicle is stopped | -| `is_parking_completed` | bool (implemented as rosparam) | whether all split trajectory of Parking are published | +| Name | Type | Description | +| -------------------------------- | ------------------------------------- | ----------------------------------------------------- | +| `~input/lane_driving/trajectory` | autoware_planning_msgs::Trajectory | trajectory of LaneDriving scenario | +| `~input/parking/trajectory` | autoware_planning_msgs::Trajectory | trajectory of Parking scenario | +| `~input/lanelet_map` | autoware_map_msgs::msg::LaneletMapBin | | +| `~input/route` | autoware_planning_msgs::LaneletRoute | route and goal pose | +| `~input/odometry` | nav_msgs::Odometry | for checking whether vehicle is stopped | +| `is_parking_completed` | bool (implemented as rosparam) | whether all split trajectory of Parking are published | ### Output topics -| Name | Type | Description | -| -------------------- | --------------------------------------- | ---------------------------------------------- | -| `~output/scenario` | tier4_planning_msgs::Scenario | current scenario and scenarios to be activated | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | +| Name | Type | Description | +| -------------------- | ---------------------------------- | ---------------------------------------------- | +| `~output/scenario` | tier4_planning_msgs::Scenario | current scenario and scenarios to be activated | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | ### Output TFs diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp index 3651f6c76b8bc..7162a435d8852 100644 --- a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp +++ b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp @@ -18,9 +18,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -48,39 +48,37 @@ class ScenarioSelectorNode : public rclcpp::Node public: explicit ScenarioSelectorNode(const rclcpp::NodeOptions & node_options); - void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onParkingState(const std_msgs::msg::Bool::ConstSharedPtr msg); bool isDataReady(); void onTimer(); - void onLaneDrivingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void onParkingTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void publishTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onLaneDrivingTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onParkingTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void publishTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void updateCurrentScenario(); std::string selectScenarioByPosition(); - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory( + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory( const std::string & scenario); private: rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_lane_driving_trajectory_; - rclcpp::Subscription::SharedPtr - sub_parking_trajectory_; + rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_state_; - rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_; autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_; nav_msgs::msg::Odometry::ConstSharedPtr current_pose_; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml index 341076505d5b8..f910c09dbb070 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/scenario_selector/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs + autoware_planning_msgs autoware_planning_test_manager lanelet2_extension nav_msgs diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 7cc251a03296c..2ba56a911bbeb 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -91,7 +91,7 @@ bool isInParkingLot( } bool isNearTrajectoryEnd( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory, + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory, const geometry_msgs::msg::Pose & current_pose, const double th_dist) { if (!trajectory || trajectory->points.empty()) { @@ -120,8 +120,8 @@ bool isStopped( } // namespace -autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr -ScenarioSelectorNode::getScenarioTrajectory(const std::string & scenario) +autoware_planning_msgs::msg::Trajectory::ConstSharedPtr ScenarioSelectorNode::getScenarioTrajectory( + const std::string & scenario) { if (scenario == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { return lane_driving_trajectory_; @@ -185,8 +185,7 @@ void ScenarioSelectorNode::updateCurrentScenario() } } -void ScenarioSelectorNode::onMap( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +void ScenarioSelectorNode::onMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -290,7 +289,7 @@ void ScenarioSelectorNode::onTimer() } void ScenarioSelectorNode::onLaneDrivingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { lane_driving_trajectory_ = msg; @@ -302,7 +301,7 @@ void ScenarioSelectorNode::onLaneDrivingTrajectory( } void ScenarioSelectorNode::onParkingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { parking_trajectory_ = msg; @@ -314,7 +313,7 @@ void ScenarioSelectorNode::onParkingTrajectory( } void ScenarioSelectorNode::publishTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { const auto now = this->now(); const auto delay_sec = (now - msg->header.stamp).seconds(); @@ -340,16 +339,15 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti is_parking_completed_(false) { // Input - sub_lane_driving_trajectory_ = - this->create_subscription( - "input/lane_driving/trajectory", rclcpp::QoS{1}, - std::bind(&ScenarioSelectorNode::onLaneDrivingTrajectory, this, std::placeholders::_1)); + sub_lane_driving_trajectory_ = this->create_subscription( + "input/lane_driving/trajectory", rclcpp::QoS{1}, + std::bind(&ScenarioSelectorNode::onLaneDrivingTrajectory, this, std::placeholders::_1)); - sub_parking_trajectory_ = this->create_subscription( + sub_parking_trajectory_ = this->create_subscription( "input/parking/trajectory", rclcpp::QoS{1}, std::bind(&ScenarioSelectorNode::onParkingTrajectory, this, std::placeholders::_1)); - sub_lanelet_map_ = this->create_subscription( + sub_lanelet_map_ = this->create_subscription( "input/lanelet_map", rclcpp::QoS{1}.transient_local(), std::bind(&ScenarioSelectorNode::onMap, this, std::placeholders::_1)); sub_route_ = this->create_subscription( @@ -365,7 +363,7 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti // Output pub_scenario_ = this->create_publisher("output/scenario", rclcpp::QoS{1}); - pub_trajectory_ = this->create_publisher( + pub_trajectory_ = this->create_publisher( "output/trajectory", rclcpp::QoS{1}); // Timer Callback diff --git a/planning/surround_obstacle_checker/README.md b/planning/surround_obstacle_checker/README.md index 071701c024685..5cf7c8a419b61 100644 --- a/planning/surround_obstacle_checker/README.md +++ b/planning/surround_obstacle_checker/README.md @@ -78,13 +78,13 @@ As mentioned in stop condition section, it prevents chattering by changing thres ### Input -| Name | Type | Description | -| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index a720de68acbcd..e009c77d3ea1b 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -45,8 +45,8 @@ namespace surround_obstacle_checker { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using motion_utils::VehicleStopChecker; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 49165f5475ad2..d9b5d4615e148 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,8 +14,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs diagnostic_msgs eigen libpcl-all-dev diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index e499d6b43dea3..c19a04bbfeac0 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -51,7 +51,7 @@ namespace surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::pose2transform; diff --git a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md index 8a72c976621f8..f8a5f29e064f7 100644 --- a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md +++ b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -78,13 +78,13 @@ Stop condition の項で述べたように、状態によって障害物判定 ### Input -| Name | Type | Description | -| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output