From 0dd4329d4fe3d833af6237ccdc048a265fca07e4 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 26 Aug 2021 15:46:56 -0400 Subject: [PATCH] Fix/wz full integ (#1403) # PR Details ## Description This PR has numerous fixes for making workzone use case work. Notable changes include: - When TCM is received, the wm_broadcaster does not immediately process (get affected_llts and regulatory elements) and schedule, but it only schedules it. And only when it is activated, it processes necessary information from map, so that new map updates are not lost due to the order of the TCM messages received. - Since carma-cloud is not able to send TCM with points with direction opposite to that of the road, there is now a custom logic in detecting "Speed-TCM" and creating duplicate geofence only with opposite direction. This is needed so that the two-directional portion of the road's speed can be changed for both directions. Carma-cloud's workzone related speed TCM is now required to include "SIG_WZ" string in their label - Calculation for SPAT's min_end_time is fixed as it accounts for moy (minute of year) now. - Original assumption for traffic light's SPAT message was that it would only broadcast different states consecutively. However, it broadcasts back-to-back STOP_AND_REMAIN states that are valid (one is for current direction, the next is for letting other direction's cars to finish its movement). It accounts for that now by only "setting" the "last recorded state" when new, different state is received - There was jerk at the end of the stop-and-wait. It was due to buffer stopping points added in the end which were duplicates. Therefore, yaw calculated between them was always 0, which gave constant jerk to one side. It is fixed by copying over the last valid yaw - Minor fixes for service name, launch files NOTE: Currently unit tests for carma_wm_ctrl has a lot of failing tests due to geofenceFromMsg function is refactored out from geofenceCallback into addGeofence. (develop itself has many failing tests as well that need be fixed before or at the same time) ## Related Issue https://github.com/usdot-fhwa-stol/carma-platform/issues/1417 ## Motivation and Context ## How Has This Been Tested? ## Types of changes - [x] Defect fix (non-breaking change that fixes an issue) - [ ] New feature (non-breaking change that adds functionality) - [ ] Breaking change (fix or feature that cause existing functionality to change) ## Checklist: - [X] I have added any new packages to the sonar-scanner.properties file - [ ] My change requires a change to the documentation. - [X] I have updated the documentation accordingly. - [X] I have read the **CONTRIBUTING** document. [CARMA Contributing Guide](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md) - [X] I have added tests to cover my changes. - [ ] All new and existing tests passed. --- arbitrator/src/capabilities_interface.cpp | 11 +- carma/launch/guidance.launch | 4 + carma_wm/include/carma_wm/CARMAWorldModel.h | 2 + .../include/carma_wm/WMTestLibForGuidance.h | 6 +- carma_wm/src/CARMAWorldModel.cpp | 94 +++++++--- carma_wm/src/WMListenerWorker.cpp | 20 +- carma_wm/src/WMListenerWorker.h | 5 +- carma_wm/test/CARMAWorldModelTest.cpp | 40 +++- .../include/carma_wm_ctrl/Geofence.h | 4 + .../include/carma_wm_ctrl/WMBroadcaster.h | 17 +- .../include/carma_wm_ctrl/WMBroadcasterNode.h | 1 + carma_wm_ctrl/src/GeofenceScheduler.cpp | 3 +- carma_wm_ctrl/src/WMBroadcaster.cpp | 177 +++++++++++++----- carma_wm_ctrl/src/WMBroadcasterNode.cpp | 2 +- carma_wm_ctrl/test/WMBroadcasterTest.cpp | 36 ++-- .../intersection_transit_maneuvering_node.h | 11 +- .../include/itm_service.h | 2 +- .../intersection_transit_maneuvering.launch | 1 + .../src/intersection_transit_maneuvering.cpp | 4 +- .../src/itm_service.cpp | 2 +- route/src/route_generator_worker.cpp | 2 +- .../src/stop_and_wait_plugin.cpp | 11 +- .../test/test_stopandwait_plugin.cpp | 2 +- wz_strategic_plugin/config/parameters.yaml | 2 +- .../launch/wz_strategic_plugin.launch | 1 + wz_strategic_plugin/src/main.cpp | 2 +- .../src/wz_strategic_plugin.cpp | 50 +++-- .../test/test_strategic_plugin_helpers.cpp | 16 +- 28 files changed, 368 insertions(+), 160 deletions(-) diff --git a/arbitrator/src/capabilities_interface.cpp b/arbitrator/src/capabilities_interface.cpp index b10227a3fd..0128451c62 100644 --- a/arbitrator/src/capabilities_interface.cpp +++ b/arbitrator/src/capabilities_interface.cpp @@ -17,6 +17,7 @@ #include "capabilities_interface.hpp" #include #include +#include namespace arbitrator { @@ -32,7 +33,15 @@ namespace arbitrator if (query_string == STRATEGIC_PLAN_CAPABILITY && sc_s.call(srv)) { topics = srv.response.plan_service; - ROS_INFO_STREAM("Received Topic: " << topics.front()); + + // Log the topics + std::ostringstream stream; + stream << "Received Topics: "; + for (const auto& topic : topics) { + stream << topic << ", "; + } + stream << std::endl; + ROS_INFO(stream.str().c_str()); } return topics; diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 3b18b2dbd2..3fd3becb6c 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -114,6 +114,10 @@ + + + + diff --git a/carma_wm/include/carma_wm/CARMAWorldModel.h b/carma_wm/include/carma_wm/CARMAWorldModel.h index 21cccc63a6..d022205b60 100644 --- a/carma_wm/include/carma_wm/CARMAWorldModel.h +++ b/carma_wm/include/carma_wm/CARMAWorldModel.h @@ -28,6 +28,8 @@ #include #include "TrackPos.h" #include +#include "boost/date_time/posix_time/posix_time.hpp" + namespace carma_wm { diff --git a/carma_wm/include/carma_wm/WMTestLibForGuidance.h b/carma_wm/include/carma_wm/WMTestLibForGuidance.h index 1921888c3c..546376d7f5 100644 --- a/carma_wm/include/carma_wm/WMTestLibForGuidance.h +++ b/carma_wm/include/carma_wm/WMTestLibForGuidance.h @@ -494,10 +494,10 @@ inline void setRouteByIds (std::vector lanelet_ids, std::shared_ptr inline void addTrafficLight(std::shared_ptr cmw, lanelet::Id light_id, lanelet::Id owning_lanelet_id, std::vector controlled_lanelet_ids, std::vector> timing_plan = { - std::make_pair(ros::Time(0), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED), // Just ended green - std::make_pair(ros::Time(4.0), lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE), // 4 sec yellow + std::make_pair(ros::Time(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED), // Just ended green + std::make_pair(ros::Time(4.0), lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE), // 4 sec yellow std::make_pair(ros::Time(24.0), lanelet::CarmaTrafficLightState::STOP_AND_REMAIN), // 20 sec red - std::make_pair(ros::Time(44.0), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED) // 20 sec green + std::make_pair(ros::Time(44.0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED) // 20 sec green }) { std::vector controlled_lanelets; diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index c321551d31..f7cca053a0 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -1181,10 +1181,10 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) ", and signal_group_id: " << (int)current_movement_state.signal_group); traffic_light_states_[curr_intersection.id.id].clear(); } - // if full cycle is already set - if (traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].size() >= 2 && - traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].front().second == - traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second) + + // if full cycle is already set + // checking >4 because 3 unique states and 1 more state to complete full cycle + if (traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].size() > 4) { continue; } @@ -1197,33 +1197,78 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) continue; } + // raw min_end_time in seconds measured from the most recent full hour + ros::Time min_end_time(current_movement_state.movement_event_list[0].timing.min_end_time); + + if (curr_intersection.moy_exists) //account for minute of the year + { + auto inception_boost(boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); // inception of epoch + auto duration_since_inception(boost::posix_time::seconds(ros::Time::now().toSec())); + auto curr_time_boost = inception_boost + duration_since_inception; + ROS_DEBUG_STREAM("Calculated current time: " << boost::posix_time::to_simple_string(curr_time_boost)); + + int curr_year = curr_time_boost.date().year(); + auto curr_year_start_boost(boost::posix_time::time_from_string(std::to_string(curr_year)+ "-01-01 00:00:00.000")); + + ROS_DEBUG_STREAM("MOY extracted: " << (int)curr_intersection.moy); + auto curr_minute_stamp_boost = curr_year_start_boost + boost::posix_time::minutes((int)curr_intersection.moy); + + int hours_of_day = curr_minute_stamp_boost.time_of_day().hours(); + int curr_month = curr_minute_stamp_boost.date().month(); + int curr_day = curr_minute_stamp_boost.date().day(); + + auto curr_day_boost(boost::posix_time::time_from_string(std::to_string(curr_year) + "/" + std::to_string(curr_month) + "/" + std::to_string(curr_day) +" 00:00:00.000")); // GMT is the standard + auto curr_hour_boost = curr_day_boost + boost::posix_time::hours(hours_of_day); + + auto curr_hour_stamp = ros::Time::fromBoost(curr_hour_boost); + + min_end_time += ros::Duration(curr_hour_stamp.toSec()); + ROS_DEBUG_STREAM("New min_end_time: " << std::to_string(min_end_time.toSec())); + } + + //if same data as last time: + //where state is same and timestamp is equal or less, skip if (traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].size() > 0 && traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second == - static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state)) //if same data as last time, skip + static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) && + traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first >= min_end_time) { continue; } - ROS_DEBUG_STREAM("Setting new state: " << curr_light_id << ", with state: " << static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) << - ", time: " << ros::Time(current_movement_state.movement_event_list[0].timing.min_end_time)); - traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(ros::Time(current_movement_state.movement_event_list[0].timing.min_end_time), - static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state))); - + // if received same state as last time, but with updated time, just update the time of last state + // skip setting state until received a new state that is different from last recorded one + if (traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].size() > 0 && traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second == + static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) && + traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first < min_end_time) + { + ROS_DEBUG_STREAM("Updated time for state: " << static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) << ", with time: " + << std::to_string(min_end_time.toSec())); + traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first = min_end_time; + continue; + } + + // detected that new state received; therefore, set the last recorded state (not new one received) + ROS_DEBUG_STREAM("Received new state for light: " << curr_light_id << ", with state: " << static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) << + ", time: " << ros::Time(min_end_time)); + + if (traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].size() >= 2 && traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].front().second == traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second) - { - curr_light->setStates(traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group], curr_intersection.revision); - ROS_DEBUG_STREAM("Set new cycle of total seconds: " << curr_light->fixed_cycle_duration.toSec()); - - } + { + ROS_DEBUG_STREAM("Setting last recorded state for light: " << curr_light_id << ", with state: " << traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second << + ", time: " << traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first); + curr_light->setStates(traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group], curr_intersection.revision); + ROS_DEBUG_STREAM("Set new cycle of total seconds: " << curr_light->fixed_cycle_duration.toSec()); + } else if (curr_light->recorded_time_stamps.empty()) // if it was never initialized, do its best to plan with the current state until the future state is also received. { std::vector> default_state; // green 20sec, yellow 3sec, red 20sec, back to green 20sec etc... - default_state.push_back(std::make_pair(ros::Time(0), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED)); - default_state.push_back(std::make_pair(default_state.back().first + ros::Duration(YELLOW_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE)); + default_state.push_back(std::make_pair(ros::Time(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); + default_state.push_back(std::make_pair(default_state.back().first + ros::Duration(YELLOW_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE)); default_state.push_back(std::make_pair(default_state.back().first + ros::Duration(RED_LIGHT_DURATION), lanelet::CarmaTrafficLightState::STOP_AND_REMAIN)); - default_state.push_back(std::make_pair(default_state.back().first + ros::Duration(GREEN_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED)); + default_state.push_back(std::make_pair(default_state.back().first + ros::Duration(GREEN_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); curr_light->setStates(default_state, curr_intersection.revision); ROS_DEBUG_STREAM("Set default cycle of total seconds: " << curr_light->fixed_cycle_duration.toSec()); @@ -1236,7 +1281,8 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) std::vector> partial_states; // set the partial cycle. - + ROS_DEBUG_STREAM("Setting last recorded state for light: " << curr_light_id << ", with state: " << traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second << + ", time: " << traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first); for (size_t i = 0; i < traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].size() - 1; i ++) { auto light_state = traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].second; @@ -1251,15 +1297,19 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) yellow_light_duration = traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].first - traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i].first; } - partial_states.push_back(std::make_pair(ros::Time(0), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED)); - partial_states.push_back(std::make_pair(partial_states.back().first + yellow_light_duration, lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE)); + partial_states.push_back(std::make_pair(ros::Time(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); + partial_states.push_back(std::make_pair(partial_states.back().first + yellow_light_duration, lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE)); partial_states.push_back(std::make_pair(partial_states.back().first + red_light_duration, lanelet::CarmaTrafficLightState::STOP_AND_REMAIN)); - partial_states.push_back(std::make_pair(partial_states.back().first + green_light_duration, lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED)); + partial_states.push_back(std::make_pair(partial_states.back().first + green_light_duration, lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); curr_light->setStates(partial_states, curr_intersection.revision); ROS_DEBUG_STREAM("Set new partial cycle of total seconds: " << curr_light->fixed_cycle_duration.toSec()); } + + // record the new state received + traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time, + static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state))); } } } diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index da2b7ecb20..78e2748865 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/carma_wm/src/WMListenerWorker.cpp @@ -113,7 +113,7 @@ void WMListenerWorker::enableUpdatesWithoutRoute() void WMListenerWorker::mapUpdateCallback(const autoware_lanelet2_msgs::MapBinPtr& geofence_msg) { ROS_INFO_STREAM("Map Update Being Evaluated. SeqNum: " << geofence_msg->header.seq); - if (rerouting_flag_) + if (rerouting_flag_) // no update should be applied if rerouting { ROS_INFO_STREAM("Currently new route is being processed. Queueing this update. Received seq: " << geofence_msg->header.seq << " prev seq: " << most_recent_update_msg_seq_); map_update_queue_.push(geofence_msg); @@ -134,6 +134,7 @@ void WMListenerWorker::mapUpdateCallback(const autoware_lanelet2_msgs::MapBinPtr if(geofence_msg->invalidates_route==true && world_model_->getRoute()) { rerouting_flag_=true; + recompute_route_flag_ = true; ROS_DEBUG_STREAM("Received notice that route has been invalidated in mapUpdateCallback"); if(route_node_flag_!=true) @@ -234,8 +235,11 @@ void WMListenerWorker::mapUpdateCallback(const autoware_lanelet2_msgs::MapBinPtr } // set the Map to trigger a new route graph construction if rerouting was required by the updates. - world_model_->setMap(world_model_->getMutableMap(), current_map_version_, rerouting_flag_); - + world_model_->setMap(world_model_->getMutableMap(), current_map_version_, recompute_route_flag_); + + // no need to reroute again unless received invalidated msg again + if (recompute_route_flag_) + recompute_route_flag_ = false; ROS_INFO_STREAM("Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_); @@ -345,17 +349,17 @@ void WMListenerWorker::routeCallback(const cav_msgs::RouteConstPtr& route_msg) return; } - if(rerouting_flag_==true && route_msg->is_rerouted && !route_node_flag_) + if(rerouting_flag_==true && route_msg->is_rerouted ) { // After setting map evaluate the current update queue to apply any updates that arrived before the map bool more_updates_to_apply = true; - rerouting_flag_ = false; while(!map_update_queue_.empty() && more_updates_to_apply) { - + auto update = map_update_queue_.front(); // Get first update map_update_queue_.pop(); // Remove update from queue - update->invalidates_route = false; + rerouting_flag_ = false; // route node has finished routing, allow update + update->invalidates_route=false; // do not trigger rerouting for route node again if (update->map_version < current_map_version_) { // Drop any so far unapplied updates for the current map ROS_WARN_STREAM("Apply from reroute: There were unapplied updates in carma_wm when a new map was recieved."); @@ -372,7 +376,7 @@ void WMListenerWorker::routeCallback(const cav_msgs::RouteConstPtr& route_msg) } } - + recompute_route_flag_ = false; rerouting_flag_ = false; if (!world_model_->getMap()) { // This check is a bit redundant but still useful from a debugging perspective as the alternative is a segfault diff --git a/carma_wm/src/WMListenerWorker.h b/carma_wm/src/WMListenerWorker.h index 82a0dc0731..794eb30303 100644 --- a/carma_wm/src/WMListenerWorker.h +++ b/carma_wm/src/WMListenerWorker.h @@ -120,8 +120,9 @@ class WMListenerWorker std::queue map_update_queue_; // Update queue used to cache map updates when they cannot be immeadiatly applied due to waiting for rerouting boost::optional delayed_route_msg_; - bool rerouting_flag_=false; - bool route_node_flag_=false; + bool recompute_route_flag_=false; // indicates whether if this node should recompute its route based on invalidated msg + bool rerouting_flag_=false; //indicates whether if route node is in middle of rerouting + bool route_node_flag_=false; //indicates whether if this node is route node long most_recent_update_msg_seq_ = -1; // Tracks the current sequence number for map update messages. Dropping even a single message would invalidate the map }; } // namespace carma_wm diff --git a/carma_wm/test/CARMAWorldModelTest.cpp b/carma_wm/test/CARMAWorldModelTest.cpp index 4a517be54a..97de016fdc 100755 --- a/carma_wm/test/CARMAWorldModelTest.cpp +++ b/carma_wm/test/CARMAWorldModelTest.cpp @@ -1314,19 +1314,30 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // partial green - EXPECT_EQ(ros::Duration(44), lights1[0]->fixed_cycle_duration); + // nothing changed + EXPECT_EQ(ros::Duration(43), lights1[0]->fixed_cycle_duration); - // call the processSpatFromMsg with that msg 2 + // call the processSpatFromMsg with that msg 2a event.event_state.movement_phase_state = 3; - event.timing.min_end_time = 45; + event.timing.min_end_time = 44.5; movement.movement_event_list[0] = event; state.movement_list[0] = movement; spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // partial green - EXPECT_EQ(ros::Duration(45), lights1[0]->fixed_cycle_duration); + // partial state 7 + EXPECT_EQ(ros::Duration(44.0), lights1[0]->fixed_cycle_duration); + + // call the processSpatFromMsg with that msg 2b + event.event_state.movement_phase_state = 3; + event.timing.min_end_time = 45.0; + movement.movement_event_list[0] = event; + state.movement_list[0] = movement; + spat.intersection_state_list[0] = state; + cmw.processSpatFromMsg(spat); + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + // partial state 7 + EXPECT_EQ(ros::Duration(44.0), lights1[0]->fixed_cycle_duration); // call the processSpatFromMsg with that msg 3 event.event_state.movement_phase_state = 5; @@ -1336,8 +1347,8 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); - // partial green - EXPECT_EQ(ros::Duration(46), lights1[0]->fixed_cycle_duration); + // partial state 3 + EXPECT_EQ(ros::Duration(45), lights1[0]->fixed_cycle_duration); // call the processSpatFromMsg with that msg 4 event.event_state.movement_phase_state = 1; @@ -1350,6 +1361,19 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc EXPECT_EQ(ros::Duration(46), lights1[0]->fixed_cycle_duration); + + ROS_DEBUG_STREAM("Calling again... Should have no change"); + // call the processSpatFromMsg with that msg 5 + event.event_state.movement_phase_state = 3; + event.timing.min_end_time = 68; + movement.movement_event_list[0] = event; + state.movement_list[0] = movement; + spat.intersection_state_list[0] = state; + cmw.processSpatFromMsg(spat); + + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc + EXPECT_EQ(ros::Duration(46), lights1[0]->fixed_cycle_duration); } TEST(CARMAWorldModelTest, getLightsAlongRoute) diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h b/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h index 5e71c439c4..0c522476fb 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h @@ -28,6 +28,7 @@ #include #include #include +#include namespace carma_wm_ctrl @@ -74,6 +75,9 @@ specific type of regulatory element (such as digital speed limit, passing contro // we need mutable elements saved here as they will be added back through update function which only accepts mutable objects std::vector> prev_regems_; lanelet::ConstLaneletOrAreas affected_parts_; + + // original message for this geofence + cav_msgs::TrafficControlMessageV01 msg_; // Helper member for PassingControlLine type regulatory geofence bool pcl_affects_left_ = false; diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h index 38f281603d..90d6169763 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h @@ -170,13 +170,12 @@ class WMBroadcaster void setConfigSpeedLimit(double cL); /*! - * \brief Returns geofence object from TrafficControlMessageV01 ROS Msg + * \brief Fills geofence object from TrafficControlMessageV01 ROS Msg + * \param Geofence object to fill with information extracted from this msg and previously cached msgs that are relevant * \param geofence_msg The ROS msg that contains geofence information * \throw InvalidObjectStateError if base_map is not set or the base_map's georeference is empty - * \return list of geofences extracted from this geofence and previously cached msgs that are relevant - * NOTE: Currently only work_zone related msgs return multiple geofences upon receiving all necessary msgs in work_zone_geofence_cache_ */ - std::vector> geofenceFromMsg(const cav_msgs::TrafficControlMessageV01& geofence_msg); + void geofenceFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& geofence_msg); /*! * \brief Returns the route distance (downtrack or crosstrack in meters) to the nearest active geofence lanelet @@ -257,9 +256,9 @@ class WMBroadcaster * \param work_zone_geofence_cache Geofence map with size of 4 corresponding to CLOSED, TAPERRIGHT, OPENRIGHT, REVERSE TrafficControlMessages. Each should have gf_pts, affected_parts, schedule, and id filled. TAPERRIGHT's id and schedule is used as all should have same schedule. \throw InvalidObjectStateError if no map is available - \return vector with one geofence housing all necessary workzone elements + \return geofence housing all necessary workzone elements */ - std::vector> createWorkzoneGeofence(std::unordered_map> work_zone_geofence_cache); + std::shared_ptr createWorkzoneGeofence(std::unordered_map> work_zone_geofence_cache); /*! * \brief Preprocess for workzone area. Parallel_llts will have front_parallel and back_parallel lanelets that were created from splitting (if necessary) @@ -342,6 +341,7 @@ class WMBroadcaster uint32_t generate32BitId(const std::string& label); void setErrorDistance (double error_distance); + private: double error_distance_ = 5; //meters lanelet::ConstLanelets route_path_; @@ -355,10 +355,13 @@ class WMBroadcaster bool shouldChangeControlLine(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr gf_ptr) const; void addPassingControlLineFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const; void addScheduleFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01); + void scheduleGeofence(std::shared_ptr gf_ptr_list); + lanelet::LineString3d createLinearInterpolatingLinestring(const lanelet::Point3d& front_pt, const lanelet::Point3d& back_pt, double increment_distance = 0.25); lanelet::Lanelet createLinearInterpolatingLanelet(const lanelet::Point3d& left_front_pt, const lanelet::Point3d& right_front_pt, const lanelet::Point3d& left_back_pt, const lanelet::Point3d& right_back_pt, double increment_distance = 0.25); std::unordered_set filterSuccessorLanelets(const std::unordered_set& possible_lanelets, const std::unordered_set& root_lanelets); + lanelet::LaneletMapPtr base_map_; lanelet::LaneletMapPtr current_map_; lanelet::routing::RoutingGraphUPtr current_routing_graph_; // Current map routing graph @@ -374,6 +377,8 @@ class WMBroadcaster GeofenceScheduler scheduler_; std::string base_map_georef_; double max_lane_width_; + std::vector workzone_remaining_msgs_; + bool workzone_geometry_published_ = false; /* Version ID of the current_map_ variable. Monotonically increasing value * NOTE: This parameter needs to be incremented any time a new map is ready to be published. * It should not be incremented for updates that do not require a full map publication. diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.h b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.h index b212db4028..7069a7b004 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.h @@ -101,6 +101,7 @@ class WMBroadcasterNode ros::Subscriber curr_location_sub_; ros::Subscriber route_cache_sub_; ros::Timer timer; + ros::Timer timer1; WMBroadcaster wmb_; }; diff --git a/carma_wm_ctrl/src/GeofenceScheduler.cpp b/carma_wm_ctrl/src/GeofenceScheduler.cpp index d796d9b5ea..293ae4e89f 100644 --- a/carma_wm_ctrl/src/GeofenceScheduler.cpp +++ b/carma_wm_ctrl/src/GeofenceScheduler.cpp @@ -57,7 +57,7 @@ void GeofenceScheduler::clearTimers() } void GeofenceScheduler::addGeofence(std::shared_ptr gf_ptr) -{ +{ std::lock_guard guard(mutex_); ROS_INFO_STREAM("Attempting to add Geofence with Id: " << gf_ptr->id_); @@ -89,6 +89,7 @@ void GeofenceScheduler::addGeofence(std::shared_ptr gf_ptr) timers_[timer_id] = std::make_pair(std::move(timer), false); // Add start timer to map by Id } + } void GeofenceScheduler::startGeofenceCallback(const ros::TimerEvent& event, std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id) diff --git a/carma_wm_ctrl/src/WMBroadcaster.cpp b/carma_wm_ctrl/src/WMBroadcaster.cpp index 97c278e06b..d81c549e50 100644 --- a/carma_wm_ctrl/src/WMBroadcaster.cpp +++ b/carma_wm_ctrl/src/WMBroadcaster.cpp @@ -199,20 +199,21 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c } } -std::vector> WMBroadcaster::geofenceFromMsg(const cav_msgs::TrafficControlMessageV01& msg_v01) +void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01) { - std::vector> return_list; - auto gf_ptr = std::make_shared(); + bool detected_workzone_signal = msg_v01.package.label_exists && msg_v01.package.label.find("SIG_WZ") != std::string::npos; + cav_msgs::TrafficControlDetail msg_detail = msg_v01.params.detail; + // Get ID std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin()); - - // Get affected lanelet or areas by converting the georeference and querying the map using points in the geofence + gf_ptr->gf_pts = getPointsInLocalFrame(msg_v01); + gf_ptr->affected_parts_ = getAffectedLaneletOrAreas(gf_ptr->gf_pts); if (gf_ptr->affected_parts_.size() == 0) { ROS_WARN_STREAM("There is no applicable component in map for the new geofence message received by WMBroadcaster with id: " << gf_ptr->id_); - return {}; // Return empty geofence list + return; // Return empty geofence list } std::vector affected_llts; @@ -225,50 +226,56 @@ std::vector> WMBroadcaster::geofenceFromMsg(const cav_ if (llt_or_area.isArea()) affected_areas.push_back(current_map_->areaLayer.get(llt_or_area.area()->id())); } - // process schedule from message - addScheduleFromMsg(gf_ptr, msg_v01); - // TODO: logic to determine what type of geofence goes here // currently only converting portion of control message that is relevant to: // - digital speed limit, passing control line, digital minimum gap, region access rule, and series of workzone related messages lanelet::Velocity sL; - cav_msgs::TrafficControlDetail msg_detail = msg_v01.params.detail; + if (msg_detail.choice == cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) { //Acquire speed limit information from TafficControlDetail msg sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPH()); - + std::string reason = ""; + if (msg_v01.package.label_exists) + reason = msg_v01.package.label; + if(config_limit > 0_mph && config_limit < 80_mph && config_limit < sL)//Accounting for the configured speed limit, input zero when not in use sL = config_limit; //Ensure Geofences do not provide invalid speed limit data (exceed predetermined maximum value) // @SONAR_STOP@ if(sL > 80_mph ) { - ROS_WARN_STREAM("Digital maximum speed limit is invalid. Value capped at max speed limit."); //Output warning message - sL = 80_mph; //Cap the speed limit to the predetermined maximum value + ROS_WARN_STREAM("Digital maximum speed limit is invalid. Value capped at max speed limit."); //Output warning message + sL = 80_mph; //Cap the speed limit to the predetermined maximum value } if(sL < 0_mph) { - ROS_WARN_STREAM("Digital speed limit is invalid. Value set to 0mph."); + ROS_WARN_STREAM("Digital speed limit is invalid. Value set to 0mph."); sL = 0_mph; }// @SONAR_START@ + gf_ptr->regulatory_element_ = std::make_shared(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), - sL, affected_llts, affected_areas, participantsChecker(msg_v01) )); + sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason)); } if (msg_detail.choice == cav_msgs::TrafficControlDetail::MINSPEED_CHOICE) { //Acquire speed limit information from TafficControlDetail msg - sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPH()); - if(config_limit > 0_mph && config_limit < 80_mph)//Accounting for the configured speed limit, input zero when not in use + sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPH()); + if(config_limit > 0_mph && config_limit < 80_mph)//Accounting for the configured speed limit, input zero when not in use sL = config_limit; + + std::string reason = ""; + if (msg_v01.package.label_exists) + reason = msg_v01.package.label; + //Ensure Geofences do not provide invalid speed limit data // @SONAR_STOP@ if(sL > 80_mph ) { - ROS_WARN_STREAM("Digital speed limit is invalid. Value capped at max speed limit."); - sL = 80_mph; + ROS_WARN_STREAM("Digital speed limit is invalid. Value capped at max speed limit."); + sL = 80_mph; } if(sL < 0_mph) { @@ -276,7 +283,7 @@ std::vector> WMBroadcaster::geofenceFromMsg(const cav_ sL = 0_mph; }// @SONAR_START@ gf_ptr->regulatory_element_ = std::make_shared(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), - sL, affected_llts, affected_areas, participantsChecker(msg_v01) )); + sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason)); } if (msg_detail.choice == cav_msgs::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == cav_msgs::TrafficControlDetail::LATAFFINITY_CHOICE) { @@ -295,9 +302,7 @@ std::vector> WMBroadcaster::geofenceFromMsg(const cav_ addRegionMinimumGap(gf_ptr,msg_v01, min_gap, affected_llts, affected_areas); } - bool detected_workzone_signal = msg_v01.package.label_exists && msg_v01.package.label.find("SIG_WZ") != std::string::npos; - - if (detected_workzone_signal) // if workzone message detected, save to cache to process later + if (detected_workzone_signal && msg_detail.choice != cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later { gf_ptr->label_ = msg_v01.package.label; // to extract intersection, and signal group id if (msg_detail.choice == cav_msgs::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == cav_msgs::TrafficControlDetail::CLOSED || @@ -310,27 +315,17 @@ std::vector> WMBroadcaster::geofenceFromMsg(const cav_ { work_zone_geofence_cache_[WorkZoneSection::REVERSE] = gf_ptr; } - if (work_zone_geofence_cache_.size() < WORKZONE_TCM_REQUIRED_SIZE) - { - ROS_INFO_STREAM("Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now..."); - return {}; - } - else - { - return_list = createWorkzoneGeofence(work_zone_geofence_cache_); - return return_list; - } + return; } else if (msg_detail.choice == cav_msgs::TrafficControlDetail::CLOSED_CHOICE && msg_detail.closed==cav_msgs::TrafficControlDetail::CLOSED) // if stand-alone closed signal apart from Workzone { addRegionAccessRule(gf_ptr,msg_v01,affected_llts); } - return_list.push_back(gf_ptr); - return return_list; + return; } -std::vector> WMBroadcaster::createWorkzoneGeofence(std::unordered_map> work_zone_geofence_cache) +std::shared_ptr WMBroadcaster::createWorkzoneGeofence(std::unordered_map> work_zone_geofence_cache) { std::shared_ptr> parallel_llts = std::make_shared>(std::vector()); std::shared_ptr> middle_opposite_lanelets = std::make_shared>(std::vector()); @@ -354,7 +349,7 @@ std::vector> WMBroadcaster::createWorkzoneGeofence(std } work_zone_geofence_cache.clear(); - return {gf_ptr}; + return gf_ptr; } std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_map> work_zone_geofence_cache, lanelet::Lanelet parallel_llt_front, lanelet::Lanelet parallel_llt_back, @@ -369,7 +364,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m lanelet::Lanelet front_llt_diag = createLinearInterpolatingLanelet(parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back(), middle_opposite_lanelets->back().rightBound3d().back(), middle_opposite_lanelets->back().leftBound3d().back()); ROS_DEBUG_STREAM("Created diag front_llt_diag id:" << front_llt_diag.id()); - for (auto regem : parallel_llt_front.regulatoryElements()) //copy existing regem into the new llts + for (auto regem : middle_opposite_lanelets->back().regulatoryElements()) //copy existing regem into the new llts { front_llt_diag.addRegulatoryElement(regem); } @@ -867,6 +862,7 @@ void WMBroadcaster::addPassingControlLineFromMsg(std::shared_ptr gf_pt void WMBroadcaster::addRegionAccessRule(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const { const std::string& reason = msg_v01.package.label; + gf_ptr->label_ = msg_v01.package.label; auto regulatory_element = std::make_shared(lanelet::RegionAccessRule::buildData(lanelet::utils::getId(),affected_llts,{},invertParticipants(participantsChecker(msg_v01)), reason)); if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck )) @@ -999,21 +995,77 @@ void WMBroadcaster::geofenceCallback(const cav_msgs::TrafficControlMessage& geof } checked_geofence_ids_.insert(boost::uuids::to_string(id)); - auto gf_ptr_list = geofenceFromMsg(geofence_msg.tcmV01); - if (gf_ptr_list.empty() || - (!gf_ptr_list.empty() && gf_ptr_list.front()->affected_parts_.size() == 0)) + + auto gf_ptr = std::make_shared(); + + gf_ptr->msg_ = geofence_msg.tcmV01; + + // process schedule from message + addScheduleFromMsg(gf_ptr, geofence_msg.tcmV01); + + scheduleGeofence(gf_ptr); +}; + +void WMBroadcaster::scheduleGeofence(std::shared_ptr gf_ptr) +{ + ROS_INFO_STREAM("Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_); + + bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos; + + cav_msgs::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail; + + // create workzone specific extra speed geofence + if (detected_workzone_signal && msg_detail.choice == cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) { - ROS_WARN_STREAM("Geofence message could not be converted"); - return; + // duplicate the messages with inverted points to support new lanelets created from workzone + // as carma-cloud currently does not support geofence points with direction opposite to that of the road + + auto gf_ptr_speed = std::make_shared(); + gf_ptr_speed->schedules = gf_ptr->schedules; + + cav_msgs::TrafficControlMessageV01 duplicate_msg = gf_ptr->msg_; + + std::reverse(duplicate_msg.geometry.nodes.begin() + 1, duplicate_msg.geometry.nodes.end()); + double first_x = 0; + double first_y = 0; + + // this fancy logic is needed as each node is expressed as an offset from the last one + for (auto& pt: duplicate_msg.geometry.nodes) + { + first_x+= pt.x; + first_y+= pt.y; + pt.x = -1* pt.x; + pt.y = -1* pt.y; + } + duplicate_msg.geometry.nodes[0].x = first_x; + duplicate_msg.geometry.nodes[0].y = first_y; + + gf_ptr_speed->msg_ = duplicate_msg; + scheduler_.addGeofence(gf_ptr_speed); } - for (auto gf_ptr : gf_ptr_list) + if (detected_workzone_signal && msg_detail.choice != cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later { - tcm_marker_array_.markers.push_back(composeTCMMarkerVisualizer(gf_ptr->gf_pts)); // create visualizer in rviz - scheduler_.addGeofence(gf_ptr); // Add the geofence to the scheduler - ROS_INFO_STREAM("New geofence message received by WMBroadcaster with id: " << gf_ptr->id_ << ", as part of total processed msgs size: " << gf_ptr_list.size()); + gf_ptr->label_ = gf_ptr->msg_.package.label; // to extract intersection, and signal group id + if (msg_detail.choice == cav_msgs::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == cav_msgs::TrafficControlDetail::CLOSED || + msg_detail.closed == cav_msgs::TrafficControlDetail::TAPERRIGHT || + msg_detail.closed == cav_msgs::TrafficControlDetail::OPENRIGHT)) + { + work_zone_geofence_cache_[msg_detail.closed] = gf_ptr; + } + else if (msg_detail.choice == cav_msgs::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == cav_msgs::TrafficControlDetail::REVERSE) + { + work_zone_geofence_cache_[WorkZoneSection::REVERSE] = gf_ptr; + } + if (work_zone_geofence_cache_.size() < WORKZONE_TCM_REQUIRED_SIZE) + { + ROS_INFO_STREAM("Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now..."); + return; + } } - -}; + + scheduler_.addGeofence(gf_ptr); // Add the geofence to the scheduler + +} void WMBroadcaster::geoReferenceCallback(const std_msgs::String& geo_ref) { @@ -1387,6 +1439,25 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) std::lock_guard guard(map_mutex_); ROS_INFO_STREAM("Adding active geofence to the map with geofence id: " << gf_ptr->id_); + // if applying workzone geometry geofence, utilize workzone chache to create one + bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos; + if (detected_workzone_signal && gf_ptr->msg_.params.detail.choice != cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) + { + for (auto gf_cache_ptr : work_zone_geofence_cache_) + { + geofenceFromMsg(gf_cache_ptr.second, gf_cache_ptr.second->msg_); + } + gf_ptr = createWorkzoneGeofence(work_zone_geofence_cache_); + } + else + { + geofenceFromMsg(gf_ptr, gf_ptr->msg_); + } + + + // add marker to rviz + tcm_marker_array_.markers.push_back(composeTCMMarkerVisualizer(gf_ptr->gf_pts)); // create visualizer in rviz + // Process the geofence object to populate update remove lists addGeofenceHelper(gf_ptr); @@ -1416,7 +1487,7 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) gf_msg.map_version = current_map_version_; map_update_message_queue_.push_back(gf_msg); // Add diff to current map update queue map_update_pub_(gf_msg); -}; +} void WMBroadcaster::removeGeofence(std::shared_ptr gf_ptr) { @@ -1440,7 +1511,7 @@ void WMBroadcaster::removeGeofence(std::shared_ptr gf_ptr) map_update_pub_(gf_msg_revert); -}; +} cav_msgs::Route WMBroadcaster::getRoute() { @@ -1823,7 +1894,9 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom lanelet::DigitalSpeedLimitPtr speed = std::dynamic_pointer_cast (current_map_->regulatoryElementLayer.get(regem->id())); outgoing_geof.value = speed->speed_limit_.value(); - outgoing_geof.advisory_speed = speed->speed_limit_.value(); + outgoing_geof.advisory_speed = speed->speed_limit_.value(); + outgoing_geof.reason = speed->getReason(); + ROS_DEBUG_STREAM("Active geofence has a speed limit of " << speed->speed_limit_.value()); // Cannot overrule outgoing_geof.type if it is already set to LANE_CLOSED diff --git a/carma_wm_ctrl/src/WMBroadcasterNode.cpp b/carma_wm_ctrl/src/WMBroadcasterNode.cpp index 7af4804f77..fecf1fa950 100644 --- a/carma_wm_ctrl/src/WMBroadcasterNode.cpp +++ b/carma_wm_ctrl/src/WMBroadcasterNode.cpp @@ -92,7 +92,7 @@ int WMBroadcasterNode::run() if(wmb_.getRoute().route_path_lanelet_ids.size() > 0) wmb_.routeCallbackMessage(wmb_.getRoute()); }, false); - + // Spin cnh_.spin(); return 0; diff --git a/carma_wm_ctrl/test/WMBroadcasterTest.cpp b/carma_wm_ctrl/test/WMBroadcasterTest.cpp index be6fe8c5e2..7df723936b 100644 --- a/carma_wm_ctrl/test/WMBroadcasterTest.cpp +++ b/carma_wm_ctrl/test/WMBroadcasterTest.cpp @@ -951,7 +951,8 @@ TEST(WMBroadcaster, geofenceFromMsgTest) lanelet::Velocity limit = 80_mph; - auto gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + auto gf_ptr = std::make_shared(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); ASSERT_TRUE(gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); lanelet::DigitalSpeedLimitPtr max_speed = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); @@ -961,7 +962,7 @@ TEST(WMBroadcaster, geofenceFromMsgTest) msg_v01.params.detail.maxspeed = -4; - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); ASSERT_TRUE(gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); max_speed = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_GE(max_speed->speed_limit_, 0_mph);//Check that the maximum speed limit is not smaller than 0_mph @@ -973,7 +974,7 @@ TEST(WMBroadcaster, geofenceFromMsgTest) msg_v01.params.detail.minspeed = -4.0; - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); ASSERT_TRUE(gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); lanelet::DigitalSpeedLimitPtr min_speed = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_GE(min_speed->speed_limit_, 0_mph); @@ -983,7 +984,7 @@ TEST(WMBroadcaster, geofenceFromMsgTest) msg_v01.params.detail.minspeed = 99.0; - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); ASSERT_TRUE(gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); min_speed = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_NEAR(min_speed->speed_limit_.value(), limit.value(), 0.0001) ;//Check that the minimum speed limit is not larger than 80_mph @@ -996,7 +997,8 @@ TEST(WMBroadcaster, geofenceFromMsgTest) wmb.setConfigSpeedLimit(55.0);//Set the config speed limit msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE; msg_v01.params.detail.maxspeed = 0; - auto gf_ptr2 = wmb.geofenceFromMsg(msg_v01).front(); + auto gf_ptr2 = std::make_shared(); + wmb.geofenceFromMsg(gf_ptr2, msg_v01); ASSERT_TRUE(gf_ptr2->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); lanelet::DigitalSpeedLimitPtr max_speed_cL = std::dynamic_pointer_cast(gf_ptr2->regulatory_element_); //ASSERT_NEAR(max_speed->speed_limit_.value(), 22.352, 0.00001); @@ -1008,7 +1010,7 @@ TEST(WMBroadcaster, geofenceFromMsgTest) wmb.setConfigSpeedLimit(55.0);//Set the config speed limit msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MINSPEED_CHOICE; msg_v01.params.detail.minspeed = 0; - gf_ptr2 = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr2,msg_v01); ASSERT_TRUE(gf_ptr2->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); lanelet::DigitalSpeedLimitPtr min_speed_cL = std::dynamic_pointer_cast(gf_ptr2->regulatory_element_); // ASSERT_NEAR(min_speed->speed_limit_.value(), 22.352, 0.00001); @@ -1024,12 +1026,12 @@ TEST(WMBroadcaster, geofenceFromMsgTest) // Test lataffinity msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::LATAFFINITY_CHOICE; msg_v01.params.detail.lataffinity = cav_msgs::TrafficControlDetail::LEFT; // applies to the left boundaries of the - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); ASSERT_TRUE(gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::PassingControlLine::RuleName) == 0); ASSERT_TRUE(gf_ptr->pcl_affects_left_); msg_v01.params.detail.lataffinity = cav_msgs::TrafficControlDetail::RIGHT; // applies to the right boundaries of the - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); ASSERT_TRUE(gf_ptr->pcl_affects_right_); msg_v01.params.detail.latperm[0] = cav_msgs::TrafficControlDetail::NONE; // not accessible from left @@ -1039,63 +1041,63 @@ TEST(WMBroadcaster, geofenceFromMsgTest) j2735_msgs::TrafficControlVehClass veh_class; veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::ANY; msg_v01.params.vclasses.push_back(veh_class); - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); lanelet::PassingControlLinePtr pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_EQ(pcl->right_participants_.size(), 3); msg_v01.params.vclasses = {}; veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::PEDESTRIAN; msg_v01.params.vclasses.push_back(veh_class); - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::Pedestrian) == 0); msg_v01.params.vclasses = {}; veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::BICYCLE; msg_v01.params.vclasses.push_back(veh_class); - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::Bicycle) == 0); msg_v01.params.vclasses = {}; veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::MOTORCYCLE; msg_v01.params.vclasses.push_back(veh_class); - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleMotorcycle) == 0); msg_v01.params.vclasses = {}; veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::BUS; msg_v01.params.vclasses.push_back(veh_class); - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleBus) == 0); msg_v01.params.vclasses = {}; veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::LIGHT_TRUCK_VAN; msg_v01.params.vclasses.push_back(veh_class); - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleCar) == 0); msg_v01.params.vclasses = {}; veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::THREE_AXLE_SINGLE_UNIT_TRUCK; msg_v01.params.vclasses.push_back(veh_class); - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleTruck) == 0); ASSERT_EQ(pcl->left_participants_.size(), 0); msg_v01.params.detail.latperm[0] = cav_msgs::TrafficControlDetail::PERMITTED; // accessible from left msg_v01.params.detail.latperm[1] = cav_msgs::TrafficControlDetail::NONE; // not accessible from right - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->left_participants_.begin()->data(), lanelet::Participants::VehicleTruck) == 0); ASSERT_EQ(pcl->right_participants_.size(), 0); msg_v01.params.detail.latperm[0] = cav_msgs::TrafficControlDetail::EMERGENCYONLY; msg_v01.params.detail.latperm[1] = cav_msgs::TrafficControlDetail::EMERGENCYONLY; - gf_ptr = wmb.geofenceFromMsg(msg_v01).front(); + wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleEmergency) == 0); ASSERT_TRUE(strcmp(pcl->left_participants_.begin()->data(), lanelet::Participants::VehicleEmergency) == 0); diff --git a/intersection_transit_maneuvering/include/intersection_transit_maneuvering_node.h b/intersection_transit_maneuvering/include/intersection_transit_maneuvering_node.h index 6871cf4b2d..a271d5e9f2 100644 --- a/intersection_transit_maneuvering/include/intersection_transit_maneuvering_node.h +++ b/intersection_transit_maneuvering/include/intersection_transit_maneuvering_node.h @@ -37,7 +37,6 @@ class IntersectionTransitManeuveringNode void run() { - //CARMA ROS node handles ros::CARMANodeHandle nh_; @@ -53,18 +52,18 @@ class IntersectionTransitManeuveringNode //Plugin discovery message cav_msgs::Plugin plugin_discovery_msg_; - + plugin_discovery_pub_ = nh_.advertise("plugin_discovery",1); carma_wm::WMListener wml_; - + carma_wm::WorldModelConstPtr wm_ = wml_.getWorldModel(); - - std::shared_ptr srv; + + std::shared_ptr srv = std::make_shared(); ros::ServiceClient trajectory_client = nh_.serviceClient("plugin/InlaneCruisingPlugin/plan_trajectory"); srv->set_client(trajectory_client); IntersectionTransitManeuvering worker(wm_,[&plugin_discovery_pub_](const auto& msg) {plugin_discovery_pub_.publish(msg);}, srv); trajectory_srv_ = nh_.advertiseService("plan_trajectory",&IntersectionTransitManeuvering::plan_trajectory_cb, &worker); - + discovery_pub_timer_ = nh_.createTimer( ros::Duration(ros::Rate(10.0)), [&worker](const auto&) {worker.onSpin();}); diff --git a/intersection_transit_maneuvering/include/itm_service.h b/intersection_transit_maneuvering/include/itm_service.h index d9b5c388b7..35b509f407 100644 --- a/intersection_transit_maneuvering/include/itm_service.h +++ b/intersection_transit_maneuvering/include/itm_service.h @@ -52,7 +52,7 @@ class Servicer: public CallInterface * * \param client input trajectory service client */ - void set_client(ros::ServiceClient srv_client); + void set_client(ros::ServiceClient& srv_client); private: ros::ServiceClient client; diff --git a/intersection_transit_maneuvering/launch/intersection_transit_maneuvering.launch b/intersection_transit_maneuvering/launch/intersection_transit_maneuvering.launch index aded59b207..5b95008735 100644 --- a/intersection_transit_maneuvering/launch/intersection_transit_maneuvering.launch +++ b/intersection_transit_maneuvering/launch/intersection_transit_maneuvering.launch @@ -1,3 +1,4 @@ + + diff --git a/wz_strategic_plugin/src/main.cpp b/wz_strategic_plugin/src/main.cpp index 576dc9c34c..6eb1991a39 100644 --- a/wz_strategic_plugin/src/main.cpp +++ b/wz_strategic_plugin/src/main.cpp @@ -57,7 +57,7 @@ int main(int argc, char** argv) // Setup callback connections ros::ServiceServer plan_maneuver_srv = - nh.advertiseService("plugins/WzStrategic/plan_maneuvers", &wz_strategic_plugin::WzStrategicPlugin::planManeuverCb, &wzp); + nh.advertiseService("plugins/" + config.strategic_plugin_name + "/plan_maneuvers", &wz_strategic_plugin::WzStrategicPlugin::planManeuverCb, &wzp); ros::Timer discovery_pub_timer = nh.createTimer(ros::Duration(ros::Rate(10.0)), [&wzp, &plugin_discovery_pub](const auto&) { diff --git a/wz_strategic_plugin/src/wz_strategic_plugin.cpp b/wz_strategic_plugin/src/wz_strategic_plugin.cpp index 7ad4675bf0..e0040383b7 100644 --- a/wz_strategic_plugin/src/wz_strategic_plugin.cpp +++ b/wz_strategic_plugin/src/wz_strategic_plugin.cpp @@ -57,20 +57,21 @@ bool WzStrategicPlugin::supportedLightState(lanelet::CarmaTrafficLightState stat { // NOTE: Following cases are intentional fall through. // Supported light states - case lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED: // Solid Green there could be conflict traffic - case lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE: // Yellow Solid there is a chance of conflicting - // traffic case lanelet::CarmaTrafficLightState::STOP_AND_REMAIN: // Solid Red + case lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE: // Yellow Solid no chance of conflicting traffic + case lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED: // Solid Green no chance of conflict traffic + // (normally used with arrows) return true; // Unsupported light states + case lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED: // Solid Green there could be conflict traffic + case lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE: // Yellow Solid there is a chance of conflicting + // traffic case lanelet::CarmaTrafficLightState::UNAVAILABLE: // No data available case lanelet::CarmaTrafficLightState::DARK: // Light is non-functional case lanelet::CarmaTrafficLightState::STOP_THEN_PROCEED: // Flashing Red + case lanelet::CarmaTrafficLightState::PRE_MOVEMENT: // Yellow Red transition (Found only in the EU) - case lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED: // Solid Green no chance of conflict traffic - // (normally used with arrows) - case lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE: // Yellow Solid no chance of conflicting traffic // (normally used with arrows) case lanelet::CarmaTrafficLightState::CAUTION_CONFLICTING_TRAFFIC: // Yellow Flashing default: @@ -93,6 +94,7 @@ WzStrategicPlugin::VehicleState WzStrategicPlugin::extractInitialState(const cav VehicleState state; if (!req.prior_plan.maneuvers.empty()) { + ROS_DEBUG_STREAM("Provided with initial plan..."); state.stamp = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_time); state.downtrack = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_dist); state.speed = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_speed); @@ -100,11 +102,17 @@ WzStrategicPlugin::VehicleState WzStrategicPlugin::extractInitialState(const cav } else { + ROS_DEBUG_STREAM("No initial plan provided..."); + state.stamp = req.header.stamp; state.downtrack = req.veh_downtrack; state.speed = req.veh_logitudinal_velocity; state.lane_id = stoi(req.veh_lane_id); } + ROS_DEBUG_STREAM("extractInitialState >>>> state.stamp: " << state.stamp); + ROS_DEBUG_STREAM("extractInitialState >>>> state.downtrack : " << state.downtrack ); + ROS_DEBUG_STREAM("extractInitialState >>>> state.speed: " << state.speed); + ROS_DEBUG_STREAM("extractInitialState >>>> state.lane_id: " << state.lane_id); return state; } @@ -114,7 +122,7 @@ bool WzStrategicPlugin::validLightState(const boost::optionalpredictState(early_arrival_time_at_freeflow); @@ -259,6 +271,8 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest return; ROS_DEBUG_STREAM("early_arrival_state_at_freeflow: " << early_arrival_state_at_freeflow_optional.get()); + ROS_ERROR_STREAM("early_arrival_state_at_freeflow: " << early_arrival_state_at_freeflow_optional.get()); + auto late_arrival_state_at_freeflow_optional = nearest_traffic_light->predictState(late_arrival_time_at_freeflow); @@ -269,12 +283,12 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver std::vector crossed_lanelets = - getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, false); + getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true); // We will cross the light on the green phase even if we arrive early or late - if (early_arrival_state_at_freeflow_optional.get() == lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED && + if (early_arrival_state_at_freeflow_optional.get() == lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED && late_arrival_state_at_freeflow_optional.get() == - lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED) // Green light + lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED) // Green light { ROS_DEBUG_STREAM("Planning lane follow and intersection transit maneuvers"); @@ -327,7 +341,7 @@ void WzStrategicPlugin::planWhenWAITING(const cav_srvs::PlanManeuversRequest& re if (!validLightState(current_light_state_optional, req.header.stamp)) return; - if (current_light_state_optional.get() == lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED) + if (current_light_state_optional.get() == lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED) { transition_table_.signal(TransitEvent::RED_TO_GREEN_LIGHT); // If the light is green send the light transition // signal @@ -362,7 +376,7 @@ void WzStrategicPlugin::planWhenDEPARTING(const cav_srvs::PlanManeuversRequest& // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver std::vector crossed_lanelets = - getLaneletsBetweenWithException(current_state.downtrack, intersection_end_downtrack, true, false); + getLaneletsBetweenWithException(current_state.downtrack, intersection_end_downtrack, true, true); // Compose intersection transit maneuver resp.new_plan.maneuvers.push_back(composeIntersectionTransitMessage( @@ -382,7 +396,11 @@ bool WzStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav_ // Extract vehicle data from request VehicleState current_state = extractInitialState(req); - + if (transition_table_.getState() != TransitState::UNAVAILABLE && !req.prior_plan.maneuvers.empty()) + { + ROS_WARN_STREAM("State is NOT UNAVAILABLE AND Maneuvers in request is NOT empty"); + return true; + } // Get current traffic light information ROS_DEBUG("\n\nFinding traffic_light information"); diff --git a/wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp b/wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp index e4506f550d..1832ba2994 100644 --- a/wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp +++ b/wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp @@ -43,16 +43,16 @@ TEST_F(WorkZoneTestFixture, supportedLightState) WzStrategicPluginConfig config; WzStrategicPlugin wzp(cmw_, config); - ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED)); - ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE)); + ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); + ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE)); ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::STOP_AND_REMAIN)); ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::UNAVAILABLE)); ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::DARK)); ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::STOP_THEN_PROCEED)); ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PRE_MOVEMENT)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE)); + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED)); + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE)); ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::CAUTION_CONFLICTING_TRAFFIC)); } @@ -96,16 +96,16 @@ TEST_F(WorkZoneTestFixture, validLightState) WzStrategicPluginConfig config; WzStrategicPlugin wzp(cmw_, config); - ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED, ros::Time(1))); - ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE, ros::Time(1))); + ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED, ros::Time(1))); + ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE, ros::Time(1))); ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficLightState::STOP_AND_REMAIN, ros::Time(1))); ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::UNAVAILABLE, ros::Time(1))); ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::DARK, ros::Time(1))); ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::STOP_THEN_PROCEED, ros::Time(1))); ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::PRE_MOVEMENT, ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED, ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE, ros::Time(1))); + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED, ros::Time(1))); + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE, ros::Time(1))); ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::CAUTION_CONFLICTING_TRAFFIC, ros::Time(1))); ASSERT_FALSE(wzp.validLightState(boost::none, ros::Time(1))); }