Skip to content

Commit 2877834

Browse files
committed
feat(bpp): support new rtc cooperate status
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 6cc85b8 commit 2877834

File tree

4 files changed

+18
-8
lines changed

4 files changed

+18
-8
lines changed

planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,10 @@ void AvoidanceByLaneChangeInterface::updateRTCStatus(
5757
return (dir == Direction::LEFT) ? "left" : "right";
5858
});
5959

60+
const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING;
61+
6062
rtc_interface_ptr_map_.at(direction)->updateCooperateStatus(
61-
uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now());
63+
uuid_map_.at(direction), isExecutionReady(), state, start_distance, finish_distance,
64+
clock_->now());
6265
}
6366
} // namespace behavior_path_planner

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

+8-6
Original file line numberDiff line numberDiff line change
@@ -77,15 +77,17 @@ class AvoidanceModule : public SceneModuleInterface
7777
{
7878
if (candidate.lateral_shift > 0.0) {
7979
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
80-
uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change,
81-
candidate.finish_distance_to_path_change, clock_->now());
80+
uuid_map_.at("left"), isExecutionReady(), State::WAITING_FOR_EXECUTION,
81+
candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change,
82+
clock_->now());
8283
candidate_uuid_ = uuid_map_.at("left");
8384
return;
8485
}
8586
if (candidate.lateral_shift < 0.0) {
8687
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
87-
uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change,
88-
candidate.finish_distance_to_path_change, clock_->now());
88+
uuid_map_.at("right"), isExecutionReady(), State::WAITING_FOR_EXECUTION,
89+
candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change,
90+
clock_->now());
8991
candidate_uuid_ = uuid_map_.at("right");
9092
return;
9193
}
@@ -108,7 +110,7 @@ class AvoidanceModule : public SceneModuleInterface
108110
motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
109111
const double finish_distance = start_distance + left_shift.relative_longitudinal;
110112
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
111-
left_shift.uuid, true, start_distance, finish_distance, clock_->now());
113+
left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
112114
if (finish_distance > -1.0e-03) {
113115
steering_factor_interface_ptr_->updateSteeringFactor(
114116
{left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
@@ -121,7 +123,7 @@ class AvoidanceModule : public SceneModuleInterface
121123
motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
122124
const double finish_distance = start_distance + right_shift.relative_longitudinal;
123125
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
124-
right_shift.uuid, true, start_distance, finish_distance, clock_->now());
126+
right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
125127
if (finish_distance > -1.0e-03) {
126128
steering_factor_interface_ptr_->updateSteeringFactor(
127129
{right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
2929
#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
3030
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
31+
#include <tier4_rtc_msgs/msg/state.hpp>
3132
#include <visualization_msgs/msg/marker_array.hpp>
3233

3334
namespace behavior_path_planner
@@ -49,6 +50,7 @@ using visualization_msgs::msg::MarkerArray;
4950
// tier4 msgs
5051
using tier4_planning_msgs::msg::AvoidanceDebugMsg;
5152
using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
53+
using tier4_rtc_msgs::msg::State;
5254

5355
// tier4 utils functions
5456
using tier4_autoware_utils::appendMarkerArray;

planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
#include <tier4_planning_msgs/msg/stop_factor.hpp>
4242
#include <tier4_planning_msgs/msg/stop_reason.hpp>
4343
#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
44+
#include <tier4_rtc_msgs/msg/state.hpp>
4445
#include <unique_identifier_msgs/msg/uuid.hpp>
4546
#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
4647

@@ -67,6 +68,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
6768
using tier4_planning_msgs::msg::StopFactor;
6869
using tier4_planning_msgs::msg::StopReason;
6970
using tier4_planning_msgs::msg::StopReasonArray;
71+
using tier4_rtc_msgs::msg::State;
7072
using unique_identifier_msgs::msg::UUID;
7173
using visualization_msgs::msg::MarkerArray;
7274
using PlanResult = PathWithLaneId::SharedPtr;
@@ -490,8 +492,9 @@ class SceneModuleInterface
490492
{
491493
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
492494
if (ptr) {
495+
const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING;
493496
ptr->updateCooperateStatus(
494-
uuid_map_.at(module_name), isExecutionReady(), start_distance, finish_distance,
497+
uuid_map_.at(module_name), isExecutionReady(), state, start_distance, finish_distance,
495498
clock_->now());
496499
}
497500
}

0 commit comments

Comments
 (0)