Skip to content

Commit b5417a9

Browse files
isamu-takagih-ohta
isamu-takagi
and
h-ohta
authored
feat: change planning factor behavior constants (#5590)
* replace module type Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * support compatibility Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
1 parent 118e9b6 commit b5417a9

File tree

36 files changed

+166
-148
lines changed

36 files changed

+166
-148
lines changed

‎common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp

+4-86
Original file line numberDiff line numberDiff line change
@@ -107,62 +107,9 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray::
107107
for (std::size_t i = 0; i < msg->factors.size(); i++) {
108108
const auto & e = msg->factors.at(i);
109109

110-
// type
110+
// behavior
111111
{
112-
auto label = new QLabel();
113-
switch (e.type) {
114-
case VelocityFactor::SURROUNDING_OBSTACLE:
115-
label->setText("SURROUNDING_OBSTACLE");
116-
break;
117-
case VelocityFactor::ROUTE_OBSTACLE:
118-
label->setText("ROUTE_OBSTACLE");
119-
break;
120-
case VelocityFactor::INTERSECTION:
121-
label->setText("INTERSECTION");
122-
break;
123-
case VelocityFactor::CROSSWALK:
124-
label->setText("CROSSWALK");
125-
break;
126-
case VelocityFactor::REAR_CHECK:
127-
label->setText("REAR_CHECK");
128-
break;
129-
case VelocityFactor::USER_DEFINED_DETECTION_AREA:
130-
label->setText("USER_DEFINED_DETECTION_AREA");
131-
break;
132-
case VelocityFactor::NO_STOPPING_AREA:
133-
label->setText("NO_STOPPING_AREA");
134-
break;
135-
case VelocityFactor::STOP_SIGN:
136-
label->setText("STOP_SIGN");
137-
break;
138-
case VelocityFactor::TRAFFIC_SIGNAL:
139-
label->setText("TRAFFIC_SIGNAL");
140-
break;
141-
case VelocityFactor::V2I_GATE_CONTROL_ENTER:
142-
label->setText("V2I_GATE_CONTROL_ENTER");
143-
break;
144-
case VelocityFactor::V2I_GATE_CONTROL_LEAVE:
145-
label->setText("V2I_GATE_CONTROL_LEAVE");
146-
break;
147-
case VelocityFactor::MERGE:
148-
label->setText("MERGE");
149-
break;
150-
case VelocityFactor::SIDEWALK:
151-
label->setText("SIDEWALK");
152-
break;
153-
case VelocityFactor::LANE_CHANGE:
154-
label->setText("LANE_CHANGE");
155-
break;
156-
case VelocityFactor::AVOIDANCE:
157-
label->setText("AVOIDANCE");
158-
break;
159-
case VelocityFactor::EMERGENCY_STOP_OPERATION:
160-
label->setText("EMERGENCY_STOP_OPERATION");
161-
break;
162-
default:
163-
label->setText("UNKNOWN");
164-
break;
165-
}
112+
auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str());
166113
label->setAlignment(Qt::AlignCenter);
167114
velocity_factors_table_->setCellWidget(i, 0, label);
168115
}
@@ -213,38 +160,9 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray::
213160
for (std::size_t i = 0; i < msg->factors.size(); i++) {
214161
const auto & e = msg->factors.at(i);
215162

216-
// type
163+
// behavior
217164
{
218-
auto label = new QLabel();
219-
switch (e.type) {
220-
case SteeringFactor::INTERSECTION:
221-
label->setText("INTERSECTION");
222-
break;
223-
case SteeringFactor::LANE_CHANGE:
224-
label->setText("LANE_CHANGE");
225-
break;
226-
case SteeringFactor::AVOIDANCE_PATH_CHANGE:
227-
label->setText("AVOIDANCE_PATH_CHANGE");
228-
break;
229-
case SteeringFactor::AVOIDANCE_PATH_RETURN:
230-
label->setText("AVOIDANCE_PATH_RETURN");
231-
break;
232-
case SteeringFactor::STATION:
233-
label->setText("STATION");
234-
break;
235-
case SteeringFactor::START_PLANNER:
236-
label->setText("START_PLANNER");
237-
break;
238-
case SteeringFactor::GOAL_PLANNER:
239-
label->setText("GOAL_PLANNER");
240-
break;
241-
case SteeringFactor::EMERGENCY_OPERATION:
242-
label->setText("EMERGENCY_OPERATION");
243-
break;
244-
default:
245-
label->setText("UNKNOWN");
246-
break;
247-
}
165+
auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str());
248166
label->setAlignment(Qt::AlignCenter);
249167
steering_factors_table_->setCellWidget(i, 0, label);
250168
}

‎common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include <rclcpp/rclcpp.hpp>
2727
#include <rviz_common/panel.hpp>
2828

29+
#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
2930
#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
3031
#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
3132

@@ -35,6 +36,7 @@ namespace rviz_plugins
3536
{
3637
class VelocitySteeringFactorsPanel : public rviz_common::Panel
3738
{
39+
using PlanningBehavior = autoware_adapi_v1_msgs::msg::PlanningBehavior;
3840
using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray;
3941
using VelocityFactor = autoware_adapi_v1_msgs::msg::VelocityFactor;
4042
using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray;

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

+2-3
Original file line numberDiff line numberDiff line change
@@ -123,7 +123,7 @@ class AvoidanceModule : public SceneModuleInterface
123123
if (finish_distance > -1.0e-03) {
124124
steering_factor_interface_ptr_->updateSteeringFactor(
125125
{left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
126-
SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::LEFT, SteeringFactor::TURNING, "");
126+
PlanningBehavior::AVOIDANCE, SteeringFactor::LEFT, SteeringFactor::TURNING, "");
127127
}
128128
}
129129

@@ -137,8 +137,7 @@ class AvoidanceModule : public SceneModuleInterface
137137
if (finish_distance > -1.0e-03) {
138138
steering_factor_interface_ptr_->updateSteeringFactor(
139139
{right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},
140-
SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::RIGHT, SteeringFactor::TURNING,
141-
"");
140+
PlanningBehavior::AVOIDANCE, SteeringFactor::RIGHT, SteeringFactor::TURNING, "");
142141
}
143142
}
144143
}

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
#include <tier4_autoware_utils/ros/marker_helper.hpp>
3535
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
3636

37+
#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
3738
#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
3839
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
3940
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
@@ -55,6 +56,7 @@
5556

5657
namespace behavior_path_planner
5758
{
59+
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
5860
using autoware_adapi_v1_msgs::msg::SteeringFactor;
5961
using autoware_auto_planning_msgs::msg::PathWithLaneId;
6062
using objects_of_interest_marker_interface::ColorName;

‎planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -35,8 +35,9 @@ class SteeringFactorInterface
3535
SteeringFactorInterface(rclcpp::Node * node, const std::string & name);
3636
void publishSteeringFactor(const rclcpp::Time & stamp);
3737
void updateSteeringFactor(
38-
const std::array<Pose, 2> & pose, const std::array<double, 2> distance, const uint16_t type,
39-
const uint16_t direction, const uint16_t status, const std::string detail);
38+
const std::array<Pose, 2> & poses, const std::array<double, 2> distances,
39+
const std::string & behavior, const uint16_t direction, const uint16_t status,
40+
const std::string detail);
4041
void clearSteeringFactors();
4142

4243
private:

‎planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -647,7 +647,7 @@ void BehaviorPathPlannerNode::publish_steering_factor(
647647

648648
steering_factor_interface_ptr_->updateSteeringFactor(
649649
{intersection_pose, intersection_pose}, {intersection_distance, intersection_distance},
650-
SteeringFactor::INTERSECTION, steering_factor_direction, steering_factor_state, "");
650+
PlanningBehavior::INTERSECTION, steering_factor_direction, steering_factor_state, "");
651651
} else {
652652
steering_factor_interface_ptr_->clearSteeringFactors();
653653
}

‎planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -982,8 +982,7 @@ CandidateOutput AvoidanceModule::planCandidate() const
982982
steering_factor_interface_ptr_->updateSteeringFactor(
983983
{sl_front.start, sl_back.end},
984984
{output.start_distance_to_path_change, output.finish_distance_to_path_change},
985-
SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING,
986-
"");
985+
PlanningBehavior::AVOIDANCE, steering_factor_direction, SteeringFactor::APPROACHING, "");
987986

988987
output.path_candidate = shifted_path.path;
989988
return output;

‎planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -853,7 +853,7 @@ void GoalPlannerModule::updateSteeringFactor(
853853

854854
// TODO(tkhmy) add handle status TRYING
855855
steering_factor_interface_ptr_->updateSteeringFactor(
856-
pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, "");
856+
pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, "");
857857
}
858858

859859
bool GoalPlannerModule::hasDecidedPath() const

‎planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -388,7 +388,7 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o
388388
// TODO(tkhmy) add handle status TRYING
389389
steering_factor_interface_ptr_->updateSteeringFactor(
390390
{status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end},
391-
{start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction,
391+
{start_distance, finish_distance}, PlanningBehavior::LANE_CHANGE, steering_factor_direction,
392392
SteeringFactor::TURNING, "");
393393
}
394394

@@ -405,7 +405,7 @@ void LaneChangeInterface::updateSteeringFactorPtr(
405405
steering_factor_interface_ptr_->updateSteeringFactor(
406406
{selected_path.info.shift_line.start, selected_path.info.shift_line.end},
407407
{output.start_distance_to_path_change, output.finish_distance_to_path_change},
408-
SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
408+
PlanningBehavior::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
409409
}
410410
void LaneChangeInterface::acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const
411411
{

‎planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -329,7 +329,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
329329
// TODO(tkhmy) add handle status TRYING
330330
steering_factor_interface_ptr_->updateSteeringFactor(
331331
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
332-
{start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction,
332+
{start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
333333
SteeringFactor::TURNING, "");
334334
} else {
335335
const double distance = motion_utils::calcSignedArcLength(
@@ -339,7 +339,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
339339
// TODO(tkhmy) add handle status TRYING
340340
steering_factor_interface_ptr_->updateSteeringFactor(
341341
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
342-
SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
342+
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
343343
}
344344

345345
setDebugData();
@@ -442,7 +442,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
442442
updateRTCStatus(start_distance, finish_distance);
443443
steering_factor_interface_ptr_->updateSteeringFactor(
444444
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
445-
{start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction,
445+
{start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
446446
SteeringFactor::APPROACHING, "");
447447
} else {
448448
const double distance = motion_utils::calcSignedArcLength(
@@ -451,7 +451,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
451451
updateRTCStatus(0.0, distance);
452452
steering_factor_interface_ptr_->updateSteeringFactor(
453453
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
454-
SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
454+
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
455455
}
456456

457457
setDebugData();

‎planning/behavior_path_planner/src/steering_factor_interface.cpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -32,16 +32,17 @@ void SteeringFactorInterface::publishSteeringFactor(const rclcpp::Time & stamp)
3232
}
3333

3434
void SteeringFactorInterface::updateSteeringFactor(
35-
const std::array<Pose, 2> & pose, const std::array<double, 2> distance, const uint16_t type,
36-
const uint16_t direction, const uint16_t status, const std::string detail)
35+
const std::array<Pose, 2> & pose, const std::array<double, 2> distance,
36+
const std::string & behavior, const uint16_t direction, const uint16_t status,
37+
const std::string detail)
3738
{
3839
std::lock_guard<std::mutex> lock(mutex_);
3940
SteeringFactor factor;
4041
factor.pose = pose;
4142
std::array<float, 2> converted_distance;
4243
for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast<float>(distance[i]);
4344
factor.distance = converted_distance;
44-
factor.type = type;
45+
factor.behavior = behavior;
4546
factor.direction = direction;
4647
factor.status = status;
4748
factor.detail = detail;

‎planning/behavior_velocity_blind_spot_module/src/scene.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ BlindSpotModule::BlindSpotModule(
6969
turn_direction_(TurnDirection::INVALID),
7070
is_over_pass_judge_line_(false)
7171
{
72-
velocity_factor_.init(VelocityFactor::REAR_CHECK);
72+
velocity_factor_.init(PlanningBehavior::REAR_CHECK);
7373
planner_param_ = planner_param;
7474

7575
const auto & assigned_lanelet =

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -184,7 +184,7 @@ CrosswalkModule::CrosswalkModule(
184184
planner_param_(planner_param),
185185
use_regulatory_element_(reg_elem_id)
186186
{
187-
velocity_factor_.init(VelocityFactor::CROSSWALK);
187+
velocity_factor_.init(PlanningBehavior::CROSSWALK);
188188
passed_safety_slow_point_ = false;
189189

190190
if (use_regulatory_element_) {

‎planning/behavior_velocity_detection_area_module/src/scene.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ DetectionAreaModule::DetectionAreaModule(
4747
state_(State::GO),
4848
planner_param_(planner_param)
4949
{
50-
velocity_factor_.init(VelocityFactor::USER_DEFINED_DETECTION_AREA);
50+
velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA);
5151
}
5252

5353
LineString2d DetectionAreaModule::getStopLineGeometry2d() const

0 commit comments

Comments
 (0)