Skip to content

Commit ee57904

Browse files
committedJun 11, 2024·
Revert "feat(rtc_interface)!: add new field to rtc cooperate status (autowarefoundation#6933)"
This reverts commit 52118ba.
1 parent 03843fd commit ee57904

File tree

14 files changed

+26
-49
lines changed

14 files changed

+26
-49
lines changed
 

‎planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp

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

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

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

+6-8
Original file line numberDiff line numberDiff line change
@@ -77,17 +77,15 @@ 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(), State::WAITING_FOR_EXECUTION,
81-
candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change,
82-
clock_->now());
80+
uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change,
81+
candidate.finish_distance_to_path_change, clock_->now());
8382
candidate_uuid_ = uuid_map_.at("left");
8483
return;
8584
}
8685
if (candidate.lateral_shift < 0.0) {
8786
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
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());
87+
uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change,
88+
candidate.finish_distance_to_path_change, clock_->now());
9189
candidate_uuid_ = uuid_map_.at("right");
9290
return;
9391
}
@@ -110,7 +108,7 @@ class AvoidanceModule : public SceneModuleInterface
110108
motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
111109
const double finish_distance = start_distance + left_shift.relative_longitudinal;
112110
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
113-
left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
111+
left_shift.uuid, true, start_distance, finish_distance, clock_->now());
114112
if (finish_distance > -1.0e-03) {
115113
steering_factor_interface_ptr_->updateSteeringFactor(
116114
{left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
@@ -123,7 +121,7 @@ class AvoidanceModule : public SceneModuleInterface
123121
motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
124122
const double finish_distance = start_distance + right_shift.relative_longitudinal;
125123
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
126-
right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
124+
right_shift.uuid, true, start_distance, finish_distance, clock_->now());
127125
if (finish_distance > -1.0e-03) {
128126
steering_factor_interface_ptr_->updateSteeringFactor(
129127
{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,7 +28,6 @@
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>
3231
#include <visualization_msgs/msg/marker_array.hpp>
3332

3433
namespace behavior_path_planner
@@ -50,7 +49,6 @@ using visualization_msgs::msg::MarkerArray;
5049
// tier4 msgs
5150
using tier4_planning_msgs::msg::AvoidanceDebugMsg;
5251
using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
53-
using tier4_rtc_msgs::msg::State;
5452

5553
// tier4 utils functions
5654
using tier4_autoware_utils::appendMarkerArray;

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

+1-4
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,6 @@
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>
4544
#include <unique_identifier_msgs/msg/uuid.hpp>
4645
#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
4746

@@ -68,7 +67,6 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
6867
using tier4_planning_msgs::msg::StopFactor;
6968
using tier4_planning_msgs::msg::StopReason;
7069
using tier4_planning_msgs::msg::StopReasonArray;
71-
using tier4_rtc_msgs::msg::State;
7270
using unique_identifier_msgs::msg::UUID;
7371
using visualization_msgs::msg::MarkerArray;
7472
using PlanResult = PathWithLaneId::SharedPtr;
@@ -505,9 +503,8 @@ class SceneModuleInterface
505503
{
506504
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
507505
if (ptr) {
508-
const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING;
509506
ptr->updateCooperateStatus(
510-
uuid_map_.at(module_name), isExecutionReady(), state, start_distance, finish_distance,
507+
uuid_map_.at(module_name), isExecutionReady(), start_distance, finish_distance,
511508
clock_->now());
512509
}
513510
}

‎planning/behavior_velocity_blind_spot_module/src/manager.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -79,8 +79,7 @@ void BlindSpotModuleManager::launchNewModules(
7979
logger_.get_child("blind_spot_module"), clock_));
8080
generateUUID(module_id);
8181
updateRTCStatus(
82-
getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits<double>::lowest(),
83-
path.header.stamp);
82+
getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
8483
}
8584
}
8685

‎planning/behavior_velocity_crosswalk_module/src/manager.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -190,8 +190,8 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
190190
clock_));
191191
generateUUID(crosswalk_lanelet_id);
192192
updateRTCStatus(
193-
getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION,
194-
std::numeric_limits<double>::lowest(), path.header.stamp);
193+
getUUID(crosswalk_lanelet_id), true, std::numeric_limits<double>::lowest(),
194+
path.header.stamp);
195195
};
196196

197197
const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(

‎planning/behavior_velocity_detection_area_module/src/manager.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -66,8 +66,7 @@ void DetectionAreaModuleManager::launchNewModules(
6666
logger_.get_child("detection_area_module"), clock_));
6767
generateUUID(module_id);
6868
updateRTCStatus(
69-
getUUID(module_id), true, State::WAITING_FOR_EXECUTION,
70-
std::numeric_limits<double>::lowest(), path.header.stamp);
69+
getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
7170
}
7271
}
7372
}

‎planning/behavior_velocity_intersection_module/src/manager.cpp

+5-6
Original file line numberDiff line numberDiff line change
@@ -345,10 +345,10 @@ void IntersectionModuleManager::launchNewModules(
345345
const UUID uuid = getUUID(new_module->getModuleId());
346346
const auto occlusion_uuid = new_module->getOcclusionUUID();
347347
rtc_interface_.updateCooperateStatus(
348-
uuid, true, State::RUNNING, std::numeric_limits<double>::lowest(),
349-
std::numeric_limits<double>::lowest(), clock_->now());
348+
uuid, true, std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(),
349+
clock_->now());
350350
occlusion_rtc_interface_.updateCooperateStatus(
351-
occlusion_uuid, true, State::RUNNING, std::numeric_limits<double>::lowest(),
351+
occlusion_uuid, true, std::numeric_limits<double>::lowest(),
352352
std::numeric_limits<double>::lowest(), clock_->now());
353353
registerModule(std::move(new_module));
354354
}
@@ -404,13 +404,12 @@ void IntersectionModuleManager::sendRTC(const Time & stamp)
404404
const UUID uuid = getUUID(scene_module->getModuleId());
405405
const bool safety =
406406
scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired());
407-
updateRTCStatus(uuid, safety, State::RUNNING, scene_module->getDistance(), stamp);
407+
updateRTCStatus(uuid, safety, scene_module->getDistance(), stamp);
408408
const auto occlusion_uuid = intersection_module->getOcclusionUUID();
409409
const auto occlusion_distance = intersection_module->getOcclusionDistance();
410410
const auto occlusion_safety = intersection_module->getOcclusionSafety();
411411
occlusion_rtc_interface_.updateCooperateStatus(
412-
occlusion_uuid, occlusion_safety, State::RUNNING, occlusion_distance, occlusion_distance,
413-
stamp);
412+
occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);
414413

415414
// ==========================================================================================
416415
// module debug data

‎planning/behavior_velocity_no_stopping_area_module/src/manager.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,7 @@ void NoStoppingAreaModuleManager::launchNewModules(
6868
clock_));
6969
generateUUID(module_id);
7070
updateRTCStatus(
71-
getUUID(module_id), true, State::WAITING_FOR_EXECUTION,
72-
std::numeric_limits<double>::lowest(), path.header.stamp);
71+
getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
7372
}
7473
}
7574
}

‎planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

+2-5
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,6 @@
3131
#include <tier4_debug_msgs/msg/float64_stamped.hpp>
3232
#include <tier4_planning_msgs/msg/stop_reason.hpp>
3333
#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
34-
#include <tier4_rtc_msgs/msg/state.hpp>
3534
#include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
3635
#include <unique_identifier_msgs/msg/uuid.hpp>
3736

@@ -59,7 +58,6 @@ using tier4_debug_msgs::msg::Float64Stamped;
5958
using tier4_planning_msgs::msg::StopFactor;
6059
using tier4_planning_msgs::msg::StopReason;
6160
using tier4_rtc_msgs::msg::Module;
62-
using tier4_rtc_msgs::msg::State;
6361
using unique_identifier_msgs::msg::UUID;
6462

6563
struct ObjectOfInterest
@@ -237,10 +235,9 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
237235
virtual void setActivation();
238236

239237
void updateRTCStatus(
240-
const UUID & uuid, const bool safe, const uint8_t state, const double distance,
241-
const Time & stamp)
238+
const UUID & uuid, const bool safe, const double distance, const Time & stamp)
242239
{
243-
rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp);
240+
rtc_interface_.updateCooperateStatus(uuid, safe, distance, distance, stamp);
244241
}
245242

246243
void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); }

‎planning/behavior_velocity_planner_common/src/scene_module_interface.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -211,8 +211,7 @@ void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp)
211211
{
212212
for (const auto & scene_module : scene_modules_) {
213213
const UUID uuid = getUUID(scene_module->getModuleId());
214-
const auto state = scene_module->isActivated() ? State::RUNNING : State::WAITING_FOR_EXECUTION;
215-
updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp);
214+
updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
216215
}
217216
publishRTCStatus(stamp);
218217
}

‎planning/behavior_velocity_traffic_light_module/src/manager.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -132,8 +132,7 @@ void TrafficLightModuleManager::launchNewModules(
132132
logger_.get_child("traffic_light_module"), clock_));
133133
generateUUID(lane_id);
134134
updateRTCStatus(
135-
getUUID(lane_id), true, State::WAITING_FOR_EXECUTION,
136-
std::numeric_limits<double>::lowest(), path.header.stamp);
135+
getUUID(lane_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
137136
}
138137
}
139138
}

‎planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,6 @@
2424
#include "tier4_rtc_msgs/msg/cooperate_status.hpp"
2525
#include "tier4_rtc_msgs/msg/cooperate_status_array.hpp"
2626
#include "tier4_rtc_msgs/msg/module.hpp"
27-
#include "tier4_rtc_msgs/msg/state.hpp"
2827
#include "tier4_rtc_msgs/srv/auto_mode.hpp"
2928
#include "tier4_rtc_msgs/srv/cooperate_commands.hpp"
3029
#include <unique_identifier_msgs/msg/uuid.hpp>
@@ -42,7 +41,6 @@ using tier4_rtc_msgs::msg::CooperateResponse;
4241
using tier4_rtc_msgs::msg::CooperateStatus;
4342
using tier4_rtc_msgs::msg::CooperateStatusArray;
4443
using tier4_rtc_msgs::msg::Module;
45-
using tier4_rtc_msgs::msg::State;
4644
using tier4_rtc_msgs::srv::AutoMode;
4745
using tier4_rtc_msgs::srv::CooperateCommands;
4846
using unique_identifier_msgs::msg::UUID;
@@ -53,8 +51,8 @@ class RTCInterface
5351
RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc = true);
5452
void publishCooperateStatus(const rclcpp::Time & stamp);
5553
void updateCooperateStatus(
56-
const UUID & uuid, const bool safe, const uint8_t state, const double start_distance,
57-
const double finish_distance, const rclcpp::Time & stamp);
54+
const UUID & uuid, const bool safe, const double start_distance, const double finish_distance,
55+
const rclcpp::Time & stamp);
5856
void removeCooperateStatus(const UUID & uuid);
5957
void clearCooperateStatus();
6058
bool isActivated(const UUID & uuid) const;

‎planning/rtc_interface/src/rtc_interface.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -201,8 +201,8 @@ void RTCInterface::onTimer()
201201
}
202202

203203
void RTCInterface::updateCooperateStatus(
204-
const UUID & uuid, const bool safe, const uint8_t state, const double start_distance,
205-
const double finish_distance, const rclcpp::Time & stamp)
204+
const UUID & uuid, const bool safe, const double start_distance, const double finish_distance,
205+
const rclcpp::Time & stamp)
206206
{
207207
std::lock_guard<std::mutex> lock(mutex_);
208208
// Find registered status which has same uuid
@@ -218,7 +218,6 @@ void RTCInterface::updateCooperateStatus(
218218
status.module = module_;
219219
status.safe = safe;
220220
status.command_status.type = Command::DEACTIVATE;
221-
status.state.type = State::WAITING_FOR_EXECUTION;
222221
status.start_distance = start_distance;
223222
status.finish_distance = finish_distance;
224223
status.auto_mode = is_auto_mode_enabled_;
@@ -229,7 +228,6 @@ void RTCInterface::updateCooperateStatus(
229228
// If the registered status is found, update status
230229
itr->stamp = stamp;
231230
itr->safe = safe;
232-
itr->state.type = state;
233231
itr->start_distance = start_distance;
234232
itr->finish_distance = finish_distance;
235233
}

0 commit comments

Comments
 (0)
Please sign in to comment.