Skip to content

Commit

Permalink
fix(default_adapi): subscribe planning factor
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Dec 24, 2024
1 parent e6cf25f commit bfb978b
Show file tree
Hide file tree
Showing 3 changed files with 176 additions and 52 deletions.
1 change: 1 addition & 0 deletions system/autoware_default_adapi/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
<depend>std_srvs</depend>
<depend>tier4_api_msgs</depend>
<depend>tier4_control_msgs</depend>
<depend>tier4_planning_msgs</depend>
<depend>tier4_system_msgs</depend>

<exec_depend>python3-flask</exec_depend>
Expand Down
179 changes: 133 additions & 46 deletions system/autoware_default_adapi/src/planning.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,16 +49,117 @@ std::vector<typename rclcpp::Subscription<T>::SharedPtr> init_factors(
}

template <class T>
T merge_factors(const rclcpp::Time stamp, const std::vector<typename T::ConstSharedPtr> & factors)
std::vector<T> convert(const std::vector<PlanningFactor> & factors)
{
T message;
static_assert(sizeof(T) == 0, "Only specializations of convert can be used.");
throw std::logic_error("Only specializations of convert can be used.");
}

template <>
std::vector<VelocityFactor> convert(const std::vector<PlanningFactor> & factors)
{
std::vector<VelocityFactor> velocity_factors;

for (const auto & factor : factors) {
if (factor.behavior != PlanningFactor::SLOW_DOWN && factor.behavior != PlanningFactor::STOP) {
continue;
}

if (factor.control_points.empty()) {
continue;
}

if (conversion_map.count(factor.module) == 0) {
continue;
}

VelocityFactor velocity_factor;
velocity_factor.behavior = conversion_map.at(factor.module);
velocity_factor.pose = factor.control_points.front().pose;
velocity_factor.distance = factor.control_points.front().distance;

velocity_factors.push_back(velocity_factor);
}

return velocity_factors;
}

template <>
std::vector<SteeringFactor> convert(const std::vector<PlanningFactor> & factors)
{
std::vector<SteeringFactor> steering_factors;

for (const auto & factor : factors) {
if (
factor.behavior != PlanningFactor::SHIFT_RIGHT &&
factor.behavior != PlanningFactor::SHIFT_LEFT &&
factor.behavior != PlanningFactor::TURN_RIGHT &&
factor.behavior != PlanningFactor::TURN_LEFT) {
continue;
}

if (factor.control_points.size() < 2) {
continue;
}

if (conversion_map.count(factor.module) == 0) {
continue;
}

SteeringFactor steering_factor;
steering_factor.behavior = conversion_map.at(factor.module);
steering_factor.direction = direction_map.at(factor.behavior);
steering_factor.pose = std::array<geometry_msgs::msg::Pose, 2>{
factor.control_points.front().pose, factor.control_points.back().pose};
steering_factor.distance = std::array<float, 2>{
factor.control_points.front().distance, factor.control_points.back().distance};

steering_factors.push_back(steering_factor);
}

return steering_factors;
}

template <class T>
T merge_factors(
const rclcpp::Time stamp, const std::vector<PlanningFactorArray::ConstSharedPtr> & factors)
{
static_assert(sizeof(T) == 0, "Only specializations of merge_factors can be used.");
throw std::logic_error("Only specializations of merge_factors can be used.");
}

template <>
VelocityFactorArray merge_factors(
const rclcpp::Time stamp, const std::vector<PlanningFactorArray::ConstSharedPtr> & factors)
{
VelocityFactorArray message;
message.header.stamp = stamp;
message.header.frame_id = "map";

for (const auto & factor : factors) {
if (!factor) {
continue;
}

concat<VelocityFactor>(message.factors, convert<VelocityFactor>(factor->factors));
}
return message;
}

template <>
SteeringFactorArray merge_factors(
const rclcpp::Time stamp, const std::vector<PlanningFactorArray::ConstSharedPtr> & factors)
{
SteeringFactorArray message;
message.header.stamp = stamp;
message.header.frame_id = "map";

for (const auto & factor : factors) {
if (factor) {
concat(message.factors, factor->factors);
if (!factor) {
continue;
}

concat<SteeringFactor>(message.factors, convert<SteeringFactor>(factor->factors));
}
return message;
}
Expand All @@ -70,46 +171,32 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning
stop_duration_ = declare_parameter<double>("stop_duration", 1.0);
stop_checker_ = std::make_unique<VehicleStopChecker>(this, stop_duration_ + 1.0);

std::vector<std::string> velocity_factor_topics = {
"/planning/velocity_factors/blind_spot",
"/planning/velocity_factors/crosswalk",
"/planning/velocity_factors/detection_area",
"/planning/velocity_factors/dynamic_obstacle_stop",
"/planning/velocity_factors/intersection",
"/planning/velocity_factors/merge_from_private",
"/planning/velocity_factors/no_stopping_area",
"/planning/velocity_factors/obstacle_stop",
"/planning/velocity_factors/obstacle_cruise",
"/planning/velocity_factors/occlusion_spot",
"/planning/velocity_factors/run_out",
"/planning/velocity_factors/stop_line",
"/planning/velocity_factors/surround_obstacle",
"/planning/velocity_factors/traffic_light",
"/planning/velocity_factors/virtual_traffic_light",
"/planning/velocity_factors/walkway",
"/planning/velocity_factors/motion_velocity_planner",
"/planning/velocity_factors/static_obstacle_avoidance",
"/planning/velocity_factors/dynamic_obstacle_avoidance",
"/planning/velocity_factors/avoidance_by_lane_change",
"/planning/velocity_factors/lane_change_left",
"/planning/velocity_factors/lane_change_right",
"/planning/velocity_factors/start_planner",
"/planning/velocity_factors/goal_planner"};

std::vector<std::string> steering_factor_topics = {
"/planning/steering_factor/static_obstacle_avoidance",
"/planning/steering_factor/dynamic_obstacle_avoidance",
"/planning/steering_factor/avoidance_by_lane_change",
"/planning/steering_factor/intersection",
"/planning/steering_factor/lane_change_left",
"/planning/steering_factor/lane_change_right",
"/planning/steering_factor/start_planner",
"/planning/steering_factor/goal_planner"};

sub_velocity_factors_ =
init_factors<VelocityFactorArray>(this, velocity_factors_, velocity_factor_topics);
sub_steering_factors_ =
init_factors<SteeringFactorArray>(this, steering_factors_, steering_factor_topics);
std::vector<std::string> factor_topics = {
"/planning/planning_factors/blind_spot",
"/planning/planning_factors/crosswalk",
"/planning/planning_factors/detection_area",
"/planning/planning_factors/dynamic_obstacle_stop",
"/planning/planning_factors/intersection",
"/planning/planning_factors/merge_from_private",
"/planning/planning_factors/no_stopping_area",
"/planning/planning_factors/obstacle_cruise_planner",
"/planning/planning_factors/occlusion_spot",
"/planning/planning_factors/run_out",
"/planning/planning_factors/stop_line",
"/planning/planning_factors/surround_obstacle_checker",
"/planning/planning_factors/traffic_light",
"/planning/planning_factors/virtual_traffic_light",
"/planning/planning_factors/walkway",
"/planning/planning_factors/motion_velocity_planner",
"/planning/planning_factors/static_obstacle_avoidance",
"/planning/planning_factors/dynamic_obstacle_avoidance",
"/planning/planning_factors/avoidance_by_lane_change",
"/planning/planning_factors/lane_change_left",
"/planning/planning_factors/lane_change_right",
"/planning/planning_factors/start_planner",
"/planning/planning_factors/goal_planner"};

sub_factors_ = init_factors<PlanningFactorArray>(this, factors_, factor_topics);

const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this);
adaptor.init_pub(pub_velocity_factors_);
Expand Down Expand Up @@ -139,8 +226,8 @@ void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg)
void PlanningNode::on_timer()
{
using autoware_adapi_v1_msgs::msg::VelocityFactor;
auto velocity = merge_factors<VelocityFactorArray>(now(), velocity_factors_);
auto steering = merge_factors<SteeringFactorArray>(now(), steering_factors_);
auto velocity = merge_factors<VelocityFactorArray>(now(), factors_);
auto steering = merge_factors<SteeringFactorArray>(now(), factors_);

// Set the distance if it is nan.
if (trajectory_ && kinematic_state_) {
Expand Down
48 changes: 42 additions & 6 deletions system/autoware_default_adapi/src/planning.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,12 @@
#include <autoware/motion_utils/vehicle/vehicle_state_checker.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
#include <tier4_planning_msgs/msg/planning_factor_array.hpp>

#include <map>
#include <memory>
#include <string>
#include <vector>

// This file should be included after messages.
Expand All @@ -30,22 +35,53 @@
namespace autoware::default_adapi
{

using autoware_adapi_v1_msgs::msg::PlanningBehavior;
using autoware_adapi_v1_msgs::msg::SteeringFactor;
using autoware_adapi_v1_msgs::msg::SteeringFactorArray;
using autoware_adapi_v1_msgs::msg::VelocityFactor;
using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
using tier4_planning_msgs::msg::PlanningFactor;
using tier4_planning_msgs::msg::PlanningFactorArray;

const std::map<std::uint16_t, std::uint16_t> direction_map = {
{PlanningFactor::SHIFT_RIGHT, SteeringFactor::RIGHT},
{PlanningFactor::SHIFT_LEFT, SteeringFactor::LEFT},
{PlanningFactor::TURN_RIGHT, SteeringFactor::RIGHT},
{PlanningFactor::TURN_LEFT, SteeringFactor::LEFT}};

const std::map<std::string, std::string> conversion_map = {
{"static_obstacle_avoidance", PlanningBehavior::AVOIDANCE},
{"crosswalk", PlanningBehavior::CROSSWALK},
{"goal_planner", PlanningBehavior::GOAL_PLANNER},
{"intersection", PlanningBehavior::INTERSECTION},
{"lane_change_left", PlanningBehavior::LANE_CHANGE},
{"lane_change_right", PlanningBehavior::LANE_CHANGE},
{"merge_from_private", PlanningBehavior::MERGE},
{"no_stopping_area", PlanningBehavior::NO_STOPPING_AREA},
{"blind_spot", PlanningBehavior::REAR_CHECK},
{"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE},
{"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE},
{"walkway", PlanningBehavior::SIDEWALK},
{"start_planner", PlanningBehavior::START_PLANNER},
{"stop_line", PlanningBehavior::STOP_SIGN},
{"surround_obstacle_checker", PlanningBehavior::SURROUNDING_OBSTACLE},
{"traffic_light", PlanningBehavior::TRAFFIC_SIGNAL},
{"detection_area", PlanningBehavior::USER_DEFINED_DETECTION_AREA},
{"virtual_traffic_light", PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT},
{"run_out", PlanningBehavior::RUN_OUT}};

class PlanningNode : public rclcpp::Node
{
public:
explicit PlanningNode(const rclcpp::NodeOptions & options);

private:
using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray;
using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray;
Pub<autoware::adapi_specs::planning::VelocityFactors> pub_velocity_factors_;
Pub<autoware::adapi_specs::planning::SteeringFactors> pub_steering_factors_;
Sub<autoware::component_interface_specs::planning::Trajectory> sub_trajectory_;
Sub<autoware::component_interface_specs::localization::KinematicState> sub_kinematic_state_;
std::vector<rclcpp::Subscription<VelocityFactorArray>::SharedPtr> sub_velocity_factors_;
std::vector<rclcpp::Subscription<SteeringFactorArray>::SharedPtr> sub_steering_factors_;
std::vector<VelocityFactorArray::ConstSharedPtr> velocity_factors_;
std::vector<SteeringFactorArray::ConstSharedPtr> steering_factors_;
std::vector<rclcpp::Subscription<PlanningFactorArray>::SharedPtr> sub_factors_;
std::vector<PlanningFactorArray::ConstSharedPtr> factors_;
rclcpp::TimerBase::SharedPtr timer_;

using VehicleStopChecker = autoware::motion_utils::VehicleStopCheckerBase;
Expand Down

0 comments on commit bfb978b

Please sign in to comment.