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))); }