Fix/wz full integ (#1403)

<!-- Thanks for the contribution, this is awesome. -->

# PR Details
## Description

<!--- Describe your changes in detail -->
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
#1417
<!--- This project only accepts pull requests related to open issues -->
<!--- If suggesting a new feature or change, please discuss it in an issue first -->
<!--- If fixing a bug, there should be an issue describing it with steps to reproduce -->
<!--- Please link to the issue here: -->

## Motivation and Context

<!--- Why is this change required? What problem does it solve? -->

## How Has This Been Tested?

<!--- Please describe in detail how you tested your changes. -->
<!--- Include details of your testing environment, and the tests you ran to -->
<!--- see how your change affects other areas of the code, etc. -->

## Types of changes

<!--- What types of changes does your code introduce? Put an `x` in all the boxes that apply: -->

- [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:

<!--- Go over all the following points, and put an `x` in all the boxes that apply. -->
<!--- If you're unsure about any of these, don't hesitate to ask. We're here to help! -->

- [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

@@ -17,6 +17,7 @@
 #include "capabilities_interface.hpp"
 #include <cav_srvs/PlanManeuvers.h>
 #include <exception>
+#include <sstream>
 
 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;

carma/launch/guidance.launch

@@ -114,6 +114,10 @@
     <remap from="upcoming_lane_change_status" to="$(optenv CARMA_GUIDE_NS)/upcoming_lane_change_status"/>
     <include file="$(find route_following_plugin)/launch/route_following_plugin.launch" />
   </group>
+
+   <!-- Workzone Strategic Plugin -->
+  <include file="$(find wz_strategic_plugin)/launch/wz_strategic_plugin.launch" />
+
   <!-- Platooning Control Plugin -->
   <include file="$(find platoon_control)/launch/platoon_control.launch" />
 

carma_wm/include/carma_wm/CARMAWorldModel.h

@@ -28,6 +28,8 @@
 #include <cav_msgs/SPAT.h>
 #include "TrackPos.h"
 #include <carma_wm/WorldModelUtils.h>
+#include "boost/date_time/posix_time/posix_time.hpp"
+
 
 namespace carma_wm
 {

carma_wm/include/carma_wm/WMTestLibForGuidance.h

@@ -494,10 +494,10 @@ inline void setRouteByIds (std::vector<lanelet::Id> lanelet_ids, std::shared_ptr
 inline void addTrafficLight(std::shared_ptr<carma_wm::CARMAWorldModel> cmw, lanelet::Id light_id, lanelet::Id owning_lanelet_id, std::vector<lanelet::Id> controlled_lanelet_ids, 
 std::vector<std::pair<ros::Time, lanelet::CarmaTrafficLightState>> timing_plan =
 {
-  std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(0), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED), // Just ended green
-  std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(4.0), lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE), // 4 sec yellow
+  std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED), // Just ended green
+  std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(4.0), lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE), // 4 sec yellow
   std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(24.0), lanelet::CarmaTrafficLightState::STOP_AND_REMAIN), // 20 sec red
-  std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(44.0), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED) // 20 sec green
+  std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(44.0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED) // 20 sec green
 }) {
 
   std::vector<lanelet::Lanelet> controlled_lanelets;

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<lanelet::CarmaTrafficLightState>(current_movement_state.movement_event_list[0].event_state.movement_phase_state)) //if same data as last time, skip
+          static_cast<lanelet::CarmaTrafficLightState>(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<lanelet::CarmaTrafficLightState>(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<lanelet::CarmaTrafficLightState>(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<lanelet::CarmaTrafficLightState>(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<lanelet::CarmaTrafficLightState>(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<lanelet::CarmaTrafficLightState>(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<std::pair<ros::Time, lanelet::CarmaTrafficLightState>> default_state;
         // green 20sec, yellow 3sec, red 20sec, back to green 20sec etc...
-        default_state.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(0), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED));
-        default_state.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(default_state.back().first + ros::Duration(YELLOW_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE));
+        default_state.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED));
+        default_state.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(default_state.back().first + ros::Duration(YELLOW_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE));
         default_state.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(default_state.back().first + ros::Duration(RED_LIGHT_DURATION), lanelet::CarmaTrafficLightState::STOP_AND_REMAIN));
-        default_state.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(default_state.back().first + ros::Duration(GREEN_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED));
+        default_state.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(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<std::pair<ros::Time, lanelet::CarmaTrafficLightState>> 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, lanelet::CarmaTrafficLightState>(ros::Time(0), lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED));
-        partial_states.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(partial_states.back().first + yellow_light_duration, lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE));
+        partial_states.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(ros::Time(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED));
+        partial_states.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(partial_states.back().first + yellow_light_duration, lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE));
         partial_states.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(partial_states.back().first + red_light_duration, lanelet::CarmaTrafficLightState::STOP_AND_REMAIN));
-        partial_states.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(partial_states.back().first + green_light_duration, lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED));
+        partial_states.push_back(std::make_pair<ros::Time, lanelet::CarmaTrafficLightState>(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<lanelet::CarmaTrafficLightState>(current_movement_state.movement_event_list[0].event_state.movement_phase_state)));
     }
   }
 }

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

carma_wm/src/WMListenerWorker.h

@@ -120,8 +120,9 @@ class WMListenerWorker
   std::queue<autoware_lanelet2_msgs::MapBinPtr> map_update_queue_; // Update queue used to cache map updates when they cannot be immeadiatly applied due to waiting for rerouting
   boost::optional<cav_msgs::RouteConstPtr> 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