Skip to content

Commit ad24dc6

Browse files
rej55KhalilSelyan
authored and
KhalilSelyan
committed
feat(behavior_path_planner_common): update RTC state to SUCCEEDED (#7119)
Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
1 parent eef5919 commit ad24dc6

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
unlockNewModuleLaunch();
191196
unlockOutputPath();
@@ -499,6 +504,17 @@ class SceneModuleInterface
499504
}
500505
}
501506

507+
void updateRTCStatusForSuccess()
508+
{
509+
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
510+
if (ptr) {
511+
ptr->updateCooperateStatus(
512+
uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits<double>::lowest(),
513+
std::numeric_limits<double>::lowest(), clock_->now());
514+
}
515+
}
516+
}
517+
502518
void setObjectsOfInterestData(
503519
const geometry_msgs::msg::Pose & obj_pose,
504520
const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name)

0 commit comments

Comments
 (0)