Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(behavior_velocity_planner_common): sync activation when safety status is set #5697

Merged
merged 3 commits into from
Nov 29, 2023
Merged

fix(behavior_velocity_planner_common): sync activation when safety status is set #5697

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4973293
fix(behavior_velocity_planner_common): sync activation when safety st…
rej55 Nov 28, 2023
c5d9afa
Update planning/behavior_velocity_planner_common/include/behavior_vel…
rej55 Nov 28, 2023
f75b721
Update planning/behavior_velocity_planner_common/include/behavior_vel…
rej55 Nov 28, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 10 additions & 1 deletion ...locity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,7 @@ class SceneModuleInterface
boost::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }

void setActivation(const bool activated) { activated_ = activated; }
void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; }
bool isActivated() const { return activated_; }
bool isSafe() const { return safe_; }
double getDistance() const { return distance_; }
Expand All @@ -98,6 +99,7 @@ class SceneModuleInterface
const int64_t module_id_;
bool activated_;
bool safe_;
bool rtc_enabled_;
double distance_;
rclcpp::Logger logger_;
rclcpp::Clock::SharedPtr clock_;
Expand All @@ -106,8 +108,15 @@ class SceneModuleInterface
boost::optional<int> first_stop_path_point_index_;
VelocityFactorInterface velocity_factor_;

void setSafe(const bool safe) { safe_ = safe; }
void setSafe(const bool safe)
{
safe_ = safe;
if (!rtc_enabled_) {
syncActivation();
}
}
void setDistance(const double distance) { distance_ = distance; }
void syncActivation() { setActivation(isSafe()); }

size_t findEgoSegmentIndex(
const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points) const;
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_planner_common/src/scene_module_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -218,6 +218,7 @@ void SceneModuleManagerInterfaceWithRTC::setActivation()
for (const auto & scene_module : scene_modules_) {
const UUID uuid = getUUID(scene_module->getModuleId());
scene_module->setActivation(rtc_interface_.isActivated(uuid));
scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid));
}
}

Expand Down
1 change: 1 addition & 0 deletions planning/rtc_interface/include/rtc_interface/rtc_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ class RTCInterface
void clearCooperateStatus();
bool isActivated(const UUID & uuid) const;
bool isRegistered(const UUID & uuid) const;
bool isRTCEnabled(const UUID & uuid) const;
void lockCommandUpdate();
void unlockCommandUpdate();

Expand Down
16 changes: 16 additions & 0 deletions planning/rtc_interface/src/rtc_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -280,6 +280,22 @@
return itr != registered_status_.statuses.end();
}

bool RTCInterface::isRTCEnabled(const UUID & uuid) const
{
std::lock_guard<std::mutex> lock(mutex_);
const auto itr = std::find_if(
registered_status_.statuses.begin(), registered_status_.statuses.end(),
[uuid](auto & s) { return s.uuid == uuid; });

if (itr != registered_status_.statuses.end()) {
return !itr->auto_mode;
}

RCLCPP_WARN_STREAM(
getLogger(), "[isRTCEnabled] uuid : " << to_string(uuid) << " is not found." << std::endl);
return is_auto_mode_init_;

Check warning on line 296 in planning/rtc_interface/src/rtc_interface.cpp

View check run for this annotation

Codecov / codecov/patch

planning/rtc_interface/src/rtc_interface.cpp#L296 

Added line #L296 was not covered by tests
}

Check warning on line 297 in planning/rtc_interface/src/rtc_interface.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Code Duplication 

The module contains 2 functions with similar structure: RTCInterface::isActivated,RTCInterface::isRTCEnabled. Avoid duplicated, aka copy-pasted, code inside the module. More duplication lowers the code health.

void RTCInterface::lockCommandUpdate()
{
is_locked_ = true;
Expand Down
Loading