Skip to content

Commit bfb978b

Browse files
satoshi-otasatoshi-ota
committed
fix(default_adapi): subscribe planning factor
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent e6cf25f commit bfb978b

File tree

3 files changed

+176
-52
lines changed

3 files changed

+176
-52
lines changed

‎system/autoware_default_adapi/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
<depend>std_srvs</depend>
3333
<depend>tier4_api_msgs</depend>
3434
<depend>tier4_control_msgs</depend>
35+
<depend>tier4_planning_msgs</depend>
3536
<depend>tier4_system_msgs</depend>
3637

3738
<exec_depend>python3-flask</exec_depend>

‎system/autoware_default_adapi/src/planning.cpp‎

Lines changed: 133 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -49,16 +49,117 @@ std::vector<typename rclcpp::Subscription<T>::SharedPtr> init_factors(
4949
}
5050

5151
template <class T>
52-
T merge_factors(const rclcpp::Time stamp, const std::vector<typename T::ConstSharedPtr> & factors)
52+
std::vector<T> convert(const std::vector<PlanningFactor> & factors)
5353
{
54-
T message;
54+
static_assert(sizeof(T) == 0, "Only specializations of convert can be used.");
55+
throw std::logic_error("Only specializations of convert can be used.");
56+
}
57+
58+
template <>
59+
std::vector<VelocityFactor> convert(const std::vector<PlanningFactor> & factors)
60+
{
61+
std::vector<VelocityFactor> velocity_factors;
62+
63+
for (const auto & factor : factors) {
64+
if (factor.behavior != PlanningFactor::SLOW_DOWN && factor.behavior != PlanningFactor::STOP) {
65+
continue;
66+
}
67+
68+
if (factor.control_points.empty()) {
69+
continue;
70+
}
71+
72+
if (conversion_map.count(factor.module) == 0) {
73+
continue;
74+
}
75+
76+
VelocityFactor velocity_factor;
77+
velocity_factor.behavior = conversion_map.at(factor.module);
78+
velocity_factor.pose = factor.control_points.front().pose;
79+
velocity_factor.distance = factor.control_points.front().distance;
80+
81+
velocity_factors.push_back(velocity_factor);
82+
}
83+
84+
return velocity_factors;
85+
}
86+
87+
template <>
88+
std::vector<SteeringFactor> convert(const std::vector<PlanningFactor> & factors)
89+
{
90+
std::vector<SteeringFactor> steering_factors;
91+
92+
for (const auto & factor : factors) {
93+
if (
94+
factor.behavior != PlanningFactor::SHIFT_RIGHT &&
95+
factor.behavior != PlanningFactor::SHIFT_LEFT &&
96+
factor.behavior != PlanningFactor::TURN_RIGHT &&
97+
factor.behavior != PlanningFactor::TURN_LEFT) {
98+
continue;
99+
}
100+
101+
if (factor.control_points.size() < 2) {
102+
continue;
103+
}
104+
105+
if (conversion_map.count(factor.module) == 0) {
106+
continue;
107+
}
108+
109+
SteeringFactor steering_factor;
110+
steering_factor.behavior = conversion_map.at(factor.module);
111+
steering_factor.direction = direction_map.at(factor.behavior);
112+
steering_factor.pose = std::array<geometry_msgs::msg::Pose, 2>{
113+
factor.control_points.front().pose, factor.control_points.back().pose};
114+
steering_factor.distance = std::array<float, 2>{
115+
factor.control_points.front().distance, factor.control_points.back().distance};
116+
117+
steering_factors.push_back(steering_factor);
118+
}
119+
120+
return steering_factors;
121+
}
122+
123+
template <class T>
124+
T merge_factors(
125+
const rclcpp::Time stamp, const std::vector<PlanningFactorArray::ConstSharedPtr> & factors)
126+
{
127+
static_assert(sizeof(T) == 0, "Only specializations of merge_factors can be used.");
128+
throw std::logic_error("Only specializations of merge_factors can be used.");
129+
}
130+
131+
template <>
132+
VelocityFactorArray merge_factors(
133+
const rclcpp::Time stamp, const std::vector<PlanningFactorArray::ConstSharedPtr> & factors)
134+
{
135+
VelocityFactorArray message;
136+
message.header.stamp = stamp;
137+
message.header.frame_id = "map";
138+
139+
for (const auto & factor : factors) {
140+
if (!factor) {
141+
continue;
142+
}
143+
144+
concat<VelocityFactor>(message.factors, convert<VelocityFactor>(factor->factors));
145+
}
146+
return message;
147+
}
148+
149+
template <>
150+
SteeringFactorArray merge_factors(
151+
const rclcpp::Time stamp, const std::vector<PlanningFactorArray::ConstSharedPtr> & factors)
152+
{
153+
SteeringFactorArray message;
55154
message.header.stamp = stamp;
56155
message.header.frame_id = "map";
57156

58157
for (const auto & factor : factors) {
59-
if (factor) {
60-
concat(message.factors, factor->factors);
158+
if (!factor) {
159+
continue;
61160
}
161+
162+
concat<SteeringFactor>(message.factors, convert<SteeringFactor>(factor->factors));
62163
}
63164
return message;
64165
}
@@ -70,46 +171,32 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning
70171
stop_duration_ = declare_parameter<double>("stop_duration", 1.0);
71172
stop_checker_ = std::make_unique<VehicleStopChecker>(this, stop_duration_ + 1.0);
72173

73-
std::vector<std::string> velocity_factor_topics = {
74-
"/planning/velocity_factors/blind_spot",
75-
"/planning/velocity_factors/crosswalk",
76-
"/planning/velocity_factors/detection_area",
77-
"/planning/velocity_factors/dynamic_obstacle_stop",
78-
"/planning/velocity_factors/intersection",
79-
"/planning/velocity_factors/merge_from_private",
80-
"/planning/velocity_factors/no_stopping_area",
81-
"/planning/velocity_factors/obstacle_stop",
82-
"/planning/velocity_factors/obstacle_cruise",
83-
"/planning/velocity_factors/occlusion_spot",
84-
"/planning/velocity_factors/run_out",
85-
"/planning/velocity_factors/stop_line",
86-
"/planning/velocity_factors/surround_obstacle",
87-
"/planning/velocity_factors/traffic_light",
88-
"/planning/velocity_factors/virtual_traffic_light",
89-
"/planning/velocity_factors/walkway",
90-
"/planning/velocity_factors/motion_velocity_planner",
91-
"/planning/velocity_factors/static_obstacle_avoidance",
92-
"/planning/velocity_factors/dynamic_obstacle_avoidance",
93-
"/planning/velocity_factors/avoidance_by_lane_change",
94-
"/planning/velocity_factors/lane_change_left",
95-
"/planning/velocity_factors/lane_change_right",
96-
"/planning/velocity_factors/start_planner",
97-
"/planning/velocity_factors/goal_planner"};
98-
99-
std::vector<std::string> steering_factor_topics = {
100-
"/planning/steering_factor/static_obstacle_avoidance",
101-
"/planning/steering_factor/dynamic_obstacle_avoidance",
102-
"/planning/steering_factor/avoidance_by_lane_change",
103-
"/planning/steering_factor/intersection",
104-
"/planning/steering_factor/lane_change_left",
105-
"/planning/steering_factor/lane_change_right",
106-
"/planning/steering_factor/start_planner",
107-
"/planning/steering_factor/goal_planner"};
108-
109-
sub_velocity_factors_ =
110-
init_factors<VelocityFactorArray>(this, velocity_factors_, velocity_factor_topics);
111-
sub_steering_factors_ =
112-
init_factors<SteeringFactorArray>(this, steering_factors_, steering_factor_topics);
174+
std::vector<std::string> factor_topics = {
175+
"/planning/planning_factors/blind_spot",
176+
"/planning/planning_factors/crosswalk",
177+
"/planning/planning_factors/detection_area",
178+
"/planning/planning_factors/dynamic_obstacle_stop",
179+
"/planning/planning_factors/intersection",
180+
"/planning/planning_factors/merge_from_private",
181+
"/planning/planning_factors/no_stopping_area",
182+
"/planning/planning_factors/obstacle_cruise_planner",
183+
"/planning/planning_factors/occlusion_spot",
184+
"/planning/planning_factors/run_out",
185+
"/planning/planning_factors/stop_line",
186+
"/planning/planning_factors/surround_obstacle_checker",
187+
"/planning/planning_factors/traffic_light",
188+
"/planning/planning_factors/virtual_traffic_light",
189+
"/planning/planning_factors/walkway",
190+
"/planning/planning_factors/motion_velocity_planner",
191+
"/planning/planning_factors/static_obstacle_avoidance",
192+
"/planning/planning_factors/dynamic_obstacle_avoidance",
193+
"/planning/planning_factors/avoidance_by_lane_change",
194+
"/planning/planning_factors/lane_change_left",
195+
"/planning/planning_factors/lane_change_right",
196+
"/planning/planning_factors/start_planner",
197+
"/planning/planning_factors/goal_planner"};
198+
199+
sub_factors_ = init_factors<PlanningFactorArray>(this, factors_, factor_topics);
113200

114201
const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this);
115202
adaptor.init_pub(pub_velocity_factors_);
@@ -139,8 +226,8 @@ void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg)
139226
void PlanningNode::on_timer()
140227
{
141228
using autoware_adapi_v1_msgs::msg::VelocityFactor;
142-
auto velocity = merge_factors<VelocityFactorArray>(now(), velocity_factors_);
143-
auto steering = merge_factors<SteeringFactorArray>(now(), steering_factors_);
229+
auto velocity = merge_factors<VelocityFactorArray>(now(), factors_);
230+
auto steering = merge_factors<SteeringFactorArray>(now(), factors_);
144231

145232
// Set the distance if it is nan.
146233
if (trajectory_ && kinematic_state_) {

‎system/autoware_default_adapi/src/planning.hpp‎

Lines changed: 42 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,12 @@
2121
#include <autoware/motion_utils/vehicle/vehicle_state_checker.hpp>
2222
#include <rclcpp/rclcpp.hpp>
2323

24+
#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
25+
#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
26+
27+
#include <map>
2428
#include <memory>
29+
#include <string>
2530
#include <vector>
2631

2732
// This file should be included after messages.
@@ -30,22 +35,53 @@
3035
namespace autoware::default_adapi
3136
{
3237

38+
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
39+
using autoware_adapi_v1_msgs::msg::SteeringFactor;
40+
using autoware_adapi_v1_msgs::msg::SteeringFactorArray;
41+
using autoware_adapi_v1_msgs::msg::VelocityFactor;
42+
using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
43+
using tier4_planning_msgs::msg::PlanningFactor;
44+
using tier4_planning_msgs::msg::PlanningFactorArray;
45+
46+
const std::map<std::uint16_t, std::uint16_t> direction_map = {
47+
{PlanningFactor::SHIFT_RIGHT, SteeringFactor::RIGHT},
48+
{PlanningFactor::SHIFT_LEFT, SteeringFactor::LEFT},
49+
{PlanningFactor::TURN_RIGHT, SteeringFactor::RIGHT},
50+
{PlanningFactor::TURN_LEFT, SteeringFactor::LEFT}};
51+
52+
const std::map<std::string, std::string> conversion_map = {
53+
{"static_obstacle_avoidance", PlanningBehavior::AVOIDANCE},
54+
{"crosswalk", PlanningBehavior::CROSSWALK},
55+
{"goal_planner", PlanningBehavior::GOAL_PLANNER},
56+
{"intersection", PlanningBehavior::INTERSECTION},
57+
{"lane_change_left", PlanningBehavior::LANE_CHANGE},
58+
{"lane_change_right", PlanningBehavior::LANE_CHANGE},
59+
{"merge_from_private", PlanningBehavior::MERGE},
60+
{"no_stopping_area", PlanningBehavior::NO_STOPPING_AREA},
61+
{"blind_spot", PlanningBehavior::REAR_CHECK},
62+
{"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE},
63+
{"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE},
64+
{"walkway", PlanningBehavior::SIDEWALK},
65+
{"start_planner", PlanningBehavior::START_PLANNER},
66+
{"stop_line", PlanningBehavior::STOP_SIGN},
67+
{"surround_obstacle_checker", PlanningBehavior::SURROUNDING_OBSTACLE},
68+
{"traffic_light", PlanningBehavior::TRAFFIC_SIGNAL},
69+
{"detection_area", PlanningBehavior::USER_DEFINED_DETECTION_AREA},
70+
{"virtual_traffic_light", PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT},
71+
{"run_out", PlanningBehavior::RUN_OUT}};
72+
3373
class PlanningNode : public rclcpp::Node
3474
{
3575
public:
3676
explicit PlanningNode(const rclcpp::NodeOptions & options);
3777

3878
private:
39-
using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray;
40-
using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray;
4179
Pub<autoware::adapi_specs::planning::VelocityFactors> pub_velocity_factors_;
4280
Pub<autoware::adapi_specs::planning::SteeringFactors> pub_steering_factors_;
4381
Sub<autoware::component_interface_specs::planning::Trajectory> sub_trajectory_;
4482
Sub<autoware::component_interface_specs::localization::KinematicState> sub_kinematic_state_;
45-
std::vector<rclcpp::Subscription<VelocityFactorArray>::SharedPtr> sub_velocity_factors_;
46-
std::vector<rclcpp::Subscription<SteeringFactorArray>::SharedPtr> sub_steering_factors_;
47-
std::vector<VelocityFactorArray::ConstSharedPtr> velocity_factors_;
48-
std::vector<SteeringFactorArray::ConstSharedPtr> steering_factors_;
83+
std::vector<rclcpp::Subscription<PlanningFactorArray>::SharedPtr> sub_factors_;
84+
std::vector<PlanningFactorArray::ConstSharedPtr> factors_;
4985
rclcpp::TimerBase::SharedPtr timer_;
5086

5187
using VehicleStopChecker = autoware::motion_utils::VehicleStopCheckerBase;

0 commit comments

Comments
 (0)