Skip to content

Commit cb905cc

Browse files
rej55shmpwk
authored andcommitted
feat(behavior_path_planner_common): update RTC state to SUCCEEDED (autowarefoundation#7119)
Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
1 parent 52118ba commit cb905cc

File tree

1 file changed

+16
-0
lines changed

1 file changed

+16
-0
lines changed

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

+16
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@
4747

4848
#include <algorithm>
4949
#include <any>
50+
#include <limits>
5051
#include <memory>
5152
#include <string>
5253
#include <unordered_map>
@@ -186,6 +187,10 @@ class SceneModuleInterface
186187
{
187188
RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__);
188189

190+
if (getCurrentStatus() == ModuleStatus::SUCCESS) {
191+
updateRTCStatusForSuccess();
192+
}
193+
189194
clearWaitingApproval();
190195
removeRTCStatus();
191196
publishRTCStatus();
@@ -513,6 +518,17 @@ class SceneModuleInterface
513518
}
514519
}
515520

521+
void updateRTCStatusForSuccess()
522+
{
523+
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
524+
if (ptr) {
525+
ptr->updateCooperateStatus(
526+
uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits<double>::lowest(),
527+
std::numeric_limits<double>::lowest(), clock_->now());
528+
}
529+
}
530+
}
531+
516532
void setObjectsOfInterestData(
517533
const geometry_msgs::msg::Pose & obj_pose,
518534
const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name)

0 commit comments

Comments
 (0)