Skip to content

Commit

Permalink
feat!: replace autoware_auto_msgs with autoware_msgs for planning mod…
Browse files Browse the repository at this point in the history
…ules (autowarefoundation#7246)

Signed-off-by: Ryohsuke Mitsudome <[email protected]>
Co-authored-by: Cynthia Liu <[email protected]>
Co-authored-by: NorahXiong <[email protected]>
Co-authored-by: beginningfan <[email protected]>
  • Loading branch information
4 people authored and a-maumau committed Jun 7, 2024
1 parent d3ecfbf commit 95b1fff
Show file tree
Hide file tree
Showing 360 changed files with 1,626 additions and 1,689 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,12 @@
#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_perception_msgs/msg/predicted_object.hpp>
#include <autoware_perception_msgs/msg/predicted_path.hpp>
#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>

#include <algorithm>
#include <memory>
Expand Down Expand Up @@ -54,9 +57,9 @@ std::vector<T> getAllKeys(const std::unordered_map<T, S> & 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
{
Expand Down Expand Up @@ -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<rclcpp::Time> latest_time_inside_ego_path{std::nullopt};
std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};
std::vector<autoware_perception_msgs::msg::PredictedPath> predicted_paths{};

// NOTE: Previous values of the following are used for low-pass filtering.
// Therefore, they has to be initialized as nullopt.
Expand Down Expand Up @@ -397,7 +400,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
const std::optional<DynamicAvoidanceObject> & 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(
Expand All @@ -407,15 +410,15 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & obj_path) const;
LatLonOffset getLateralLongitudinalOffset(
const std::vector<PathPointWithLaneId> & 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<PathPointWithLaneId> & 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<MinMaxValue> calcMinMaxLateralOffsetToAvoidRegulatedObject(
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,9 @@
<buildtool_depend>autoware_cmake</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend>

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
<depend>behavior_path_planner</depend>
<depend>behavior_path_planner_common</depend>
<depend>geometry_msgs</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -128,13 +128,13 @@ std::pair<double, double> 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);
Expand All @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -1183,7 +1183,7 @@ DynamicObstacleAvoidanceModule::getAdjacentLanes(
DynamicObstacleAvoidanceModule::LatLonOffset
DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset(
const std::vector<PathPointWithLaneId> & 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);
Expand Down Expand Up @@ -1218,7 +1218,7 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset(
MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid(
const std::vector<PathPointWithLaneId> & 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 =
Expand Down Expand Up @@ -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 =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,14 +19,14 @@
#include <autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp>
#include <rclcpp/node.hpp>

#include <autoware_auto_perception_msgs/msg/detail/object_classification__struct.hpp>
#include <autoware_perception_msgs/msg/detail/object_classification__struct.hpp>

#include <string>
#include <vector>

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)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,20 +23,20 @@
#include <tier4_autoware_utils/ros/marker_helper.hpp>
#include <tier4_autoware_utils/ros/uuid_helper.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
#include <autoware_perception_msgs/msg/predicted_object.hpp>
#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
#include <tier4_rtc_msgs/msg/state.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,6 @@
<buildtool_depend>autoware_cmake</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend>

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>behavior_path_planner</depend>
<depend>behavior_path_planner_common</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -42,7 +42,7 @@ void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node)
void StaticObstacleAvoidanceModuleManager::updateModuleParams(
const std::vector<rclcpp::Parameter> & parameters)
{
using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_perception_msgs::msg::ObjectClassification;
using tier4_autoware_utils::updateParam;

auto p = parameters_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
20 changes: 10 additions & 10 deletions planning/autoware_behavior_velocity_planner/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 3 additions & 3 deletions planning/autoware_behavior_velocity_planner/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,9 @@

<build_depend>rosidl_default_generators</build_depend>

<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_velocity_smoother</depend>
<depend>behavior_velocity_planner_common</depend>
<depend>diagnostic_msgs</depend>
Expand Down
Loading

0 comments on commit 95b1fff

Please sign in to comment.