Feature/move over law (#230)

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

# PR Details
## Description
This is for adding new code in carma messenger ambassador, sumo
ambassador and carma messenger vehicle plugin to prepare for the carma
messenger integration test


## Related GitHub Issue

<!--- This project only accepts pull requests related to open GitHub
issues or Jira Keys -->
<!--- 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 DO NOT name partially fixed issues, instead open an issue
specific to this fix -->
<!--- Please link to the issue here: -->

## Related Jira Key

<!-- e.g. CAR-123 -->

## 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)
- [x] 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! -->

- [ ] I have added any new packages to the sonar-scanner.properties file
- [ ] My change requires a change to the documentation.
- [ ] I have updated the documentation accordingly.
- [ ] I have read the
[**CONTRIBUTING**](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md)
document.
- [ ] I have added tests to cover my changes.
- [x] All new and existing tests passed.

carma_messenger_vehicle_plugin/logs/README

@@ -0,0 +1 @@
+This folder is for plugin logs

carma_messenger_vehicle_plugin/src/move_over_law.py

@@ -0,0 +1,73 @@
+ #   Copyright (C) 2022 LEIDOS.
+ #
+ #   Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ #   use this file except in compliance with the License. You may obtain a copy of
+ #   the License at
+ #
+ #   http://www.apache.org/licenses/LICENSE-2.0
+ #
+ #   Unless required by applicable law or agreed to in writing, software
+ #   distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ #   WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ #   License for the specific language governing permissions and limitations under
+ #   the License.
+
+from sumo_connector import SumoConnector
+
+import configparser
+
+class MoveOverLaw:
+
+    def __init__(self, sumo_connector, veh_id):
+
+        config = configparser.ConfigParser()
+        config.read('resources/move_over_law_cfg.ini')
+        self._closure_uptrack = config.get('Settings', 'closure_uptrack')
+        self._closure_downtrack = config.get('Settings', 'closure_downtrack')
+        self._target_veh_id = config.get('Settings', 'target_id')
+        self._veh_id = veh_id
+        self._min_gap = config.get('Settings', 'min_gap')
+        self._advisory_speed_limit = config.get('Settings', 'advisory_speed_limit')
+        self._stop_route = config.get('Settings', 'stop_route')
+        self._stop_pos = config.get('Settings', 'stop_pos')
+        self._start_lane = config.get('Settings', 'start_lane')
+        self.sumo_connector = sumo_connector
+        self.sumo_connector.create_stop_veh(self._target_veh_id, self._stop_pos, self._stop_route)
+        self.first_time_two_vehicles = True
+
+    def close_lane(self):
+        #send lane closure message
+        combined_string = self._closure_uptrack + ";" + self._closure_downtrack + ";" + self._min_gap + ";" + self._advisory_speed_limit
+        self.sumo_connector.set_parameter(self._veh_id, 'VehicleBroadcastTrafficEvent', combined_string)
+
+    def park_messenger(self):
+        target_lane = self.sumo_connector.get_veh_lane(self._target_veh_id)
+        target_lane_index = int(target_lane.split('_')[-1])
+        self.sumo_connector.move_veh_lane(self._veh_id, target_lane_index)
+        self.sumo_connector.stop_veh(self._veh_id)
+        return
+
+    def get_closer(self):
+        target_lane = self.sumo_connector.get_veh_lane(self._target_veh_id)
+        lane_index = int(target_lane.split('_')[-1])
+        target_lane_index = lane_index+1
+        self.sumo_connector.move_veh_lane(self._veh_id, target_lane_index)
+        return
+
+    def move_over(self):
+        if len(self.sumo_connector.get_veh_ids()) >=2:
+
+            if self.first_time_two_vehicles:
+                self.sumo_connector.set_veh_lane(self._veh_id, self._start_lane)
+                self.first_time_two_vehicles = False
+
+            veh_pos = self.sumo_connector.get_veh_pos(self._veh_id)
+            target_pos = self.sumo_connector.get_veh_pos(self._target_veh_id)
+            distance = self.sumo_connector.cal_distance(veh_pos, target_pos)
+            print("Distance: " + str(distance))
+
+            if distance < 30:
+                self.park_messenger()
+                self.close_lane()
+            elif distance < 250:
+                self.get_closer()

carma_messenger_vehicle_plugin/src/msger_veh_wrapper.py

@@ -16,13 +16,17 @@
 import logging
 from sumo_connector import SumoConnector
 from msger_veh_cfg import MsgerVehicleCfg
-from msger_veh_cfg import VehicleState 
+from msger_veh_cfg import VehicleState
+from move_over_law import MoveOverLaw
+from datetime import datetime
 
 def setup_logging(level):
     numeric_level = getattr(logging, level.upper(), None)
     if not isinstance(numeric_level, int):
         raise ValueError('Invalid log level: %s' % level)
-    logging.basicConfig(level=numeric_level, format='%(asctime)s - %(levelname)s - %(message)s')
+    current_time = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
+    log_filename = f"../logs/log_{current_time}.log"
+    logging.basicConfig(level=numeric_level, format='%(asctime)s - %(levelname)s - %(message)s', filename=log_filename)
 
 def run(args):
     """
@@ -35,15 +39,16 @@ def run(args):
     """
     setup_logging(args.log_level)
     try:
+        logging.info("Enter run")
         msger_veh_cfg = MsgerVehicleCfg(args.msger_veh_cfg_path, log_level=args.log_level)
         sumo_connector = SumoConnector(args.traci_ip, args.traci_port, args.traci_order_num, log_level=args.log_level)
         msg_veh_ids = msger_veh_cfg.get_veh_ids()
+        move_over_law_veh_dict = {}
         with sumo_connector.traci_handler():
             while True:
                 sumo_connector.tick()
                 sumo_veh_ids = sumo_connector.get_veh_ids()
                 for msg_veh_id in msg_veh_ids:
-
                     if msg_veh_id not in sumo_veh_ids and \
                        msger_veh_cfg.get_veh_state(msg_veh_id) == VehicleState.NOT_CREATED and \
                        msger_veh_cfg.get_veh_departure_time(msg_veh_id) <= sumo_connector.get_sim_time():
@@ -55,13 +60,20 @@ def run(args):
                         sumo_connector.set_veh_lcm(msg_veh_id, msger_veh_cfg.get_veh_lcm(msg_veh_id))
                         sumo_connector.set_veh_type(msg_veh_id, msger_veh_cfg.get_veh_cfm(msg_veh_id))
                         msger_veh_cfg.set_veh_state(msg_veh_id, VehicleState.CREATED_AND_DRIVING)
+
+                        if msger_veh_cfg.get_veh_cfm(msg_veh_id) == "MoveOverLaw":
+                            move_over_law_veh_dict[msg_veh_id] = MoveOverLaw(sumo_connector, msg_veh_id)
+                        ## TODO Init Freight ERV Vehicle
+
                     elif msg_veh_id not in sumo_veh_ids and msger_veh_cfg.get_veh_state(msg_veh_id) == VehicleState.CREATED_AND_DRIVING:
                         ### Remove
                         msger_veh_cfg.set_veh_state(msg_veh_id, VehicleState.FINISHED_AND_DESTROYED)
+                        # move_over_law_veh =
                         logging.info("Vehicle " + msg_veh_id + " finished route.")
-
-                    ## TODO
-
+
+                    if msg_veh_id in move_over_law_veh_dict:
+                        move_over_law_veh_dict[msg_veh_id].move_over()
+
     except Exception as e:
         logging.error(f"An error occurred during simulation: {e}")
 

carma_messenger_vehicle_plugin/src/resources/move_over_law_cfg.ini

@@ -0,0 +1,9 @@
+[Settings]
+closure_uptrack:50
+closure_downtrack:50
+target_id:target1
+min_gap:5
+advisory_speed_limit:10.0
+stop_route:stop1
+stop_pos:298
+start_lane:-50_4

carma_messenger_vehicle_plugin/src/resources/msger_veh_cfg.json

@@ -2,32 +2,12 @@
     "msgerVehs":
     [
         {
-            "id": "vehicle_1",
-            "route": [  "1_26704526_26704482",
-                        "32909782_26704482_26785753",
-                        "25185001_26785753_26704584",
-                        "25185007_26704584_21487146",
-                        "25185006_21487146_21487168",
-                        "4068038_21487168_251150126",
-                        "4068038_251150126_428788319"],
-            "speed": 15,
-            "departureTime": 5,
-            "lcm" : 0,
-            "cfm" : "FreightERV"
-        },
-        {
-            "id": "vehicle_2",
-            "route": [  "1_26704526_26704482",
-                        "32909782_26704482_26785753",
-                        "25185001_26785753_26704584",
-                        "25185007_26704584_21487146",
-                        "25185006_21487146_21487168",
-                        "4068038_21487168_251150126",
-                        "4068038_251150126_428788319"],
+            "id": "msger_1",
+            "route": ["-50","-43"],
             "speed": 10,
-            "departureTime": 10,
+            "departureTime": 5,
             "lcm" : 0,
-            "cfm" : "FreightERV"
+            "cfm" : "MoveOverLaw"
         }
     ]
 }

carma_messenger_vehicle_plugin/src/sumo_connector.py

@@ -15,6 +15,7 @@
 import sys
 import os
 import logging
+import math
 
 if 'SUMO_HOME' in os.environ:
     tools_path = os.path.join(os.environ['SUMO_HOME'], 'tools')
@@ -157,17 +158,17 @@ def get_sim_time(self):
         except Exception as e:
             logging.error("Failed to get simulation time: " + str(e))
             raise
-    
+
     def set_veh_type(self, veh_id, model):
         """
         Sets the vehicle type for a specified vehicle based on the given model.
         The vehicle type determines the car-following model to be used by the vehicle in the simulation.
         The vehicle type must be predefined in the SUMO .rou.xml configuration file of the CDASim scenario.
-        
+
         Parameters:
         - veh_id: The identifier for the vehicle whose type is to be set.
         - model: The vehicle type identifier, which must correspond to one of the vehicle types defined in the .rou.xml file.
-        
+
         Raises:
         - Exception: If there is an error in setting the vehicle type, such as if the vehicle ID does not exist or the model is not defined.
         """
@@ -179,11 +180,11 @@ def set_veh_type(self, veh_id, model):
 
     def set_veh_signal(self, veh_id, signal):
         """
-        Sets the vehicle signals for a specified vehicle in the SUMO simulation. 
-        This function is used to control visual indicators like emergency lights on a vehicle. 
+        Sets the vehicle signals for a specified vehicle in the SUMO simulation.
+        This function is used to control visual indicators like emergency lights on a vehicle.
         The signal parameter is an integer that encodes the state of the vehicle’s signals according to the SUMO vehicle signaling documentation.
 
-        The function is particularly useful for integrations where external systems (such as MOSAIC) monitor these signals for various purposes, 
+        The function is particularly useful for integrations where external systems (such as MOSAIC) monitor these signals for various purposes,
         including activating functionalities like the Broadcast of Basic Safety Messages (BSM) in connected vehicle environments.
 
         See the SUMO documentation for detailed signal encoding: https://sumo.dlr.de/docs/TraCI/Vehicle_Signalling.html
@@ -201,4 +202,87 @@ def set_veh_signal(self, veh_id, signal):
         except Exception as e:
             logging.error(f"Failed to set vehicle signal for vehicle ID '{veh_id}': {e}")
             raise
+
+    def cal_distance(self, pos_1, pos_2):
+        """
+        Calculates the distance between a certain location and a vehicle in SUMO
+        """
+        try:
+            distance = math.sqrt((pos_1[0] - pos_2[0])**2 + (pos_1[1] - pos_2[1])**2)
+            return distance
+        except Exception as e:
+            logging.error(f"Failed to calculate vehicle distance ")
+            raise
+
+    def stop_veh(self, veh_id):
+        """
+        stops vehicle at current place in SUMO
+        """
+        try:
+            traci.vehicle.setSpeed(veh_id, 0)
+
+        except Exception as e:
+            logging.error(f"Failed to stop vehicle for vehicle ID '{veh_id}': {e}")
+            raise
+
+    def move_veh_lane(self, veh_id, target_lane):
+
+        try:
+            traci.vehicle.setLaneChangeMode(veh_id, 681)
+            current_lane = traci.vehicle.getLaneIndex(veh_id)
+            if current_lane != target_lane:
+                traci.vehicle.changeLane(veh_id, target_lane, 10)
+
+        except Exception as e:
+            logging.error(f"Failed to change vehicle lane for vehicle ID '{veh_id}': {e}")
+            raise
+
+    def set_parameter(self, veh_id, para_name, para_value):
+
+        try:
+            traci.vehicle.setParameter(veh_id, para_name, para_value)
+
+        except Exception as e:
+            logging.error(f"Failed to set vehicle parameter for vehicle ID '{veh_id}': {e}")
+            raise
+
+    def get_veh_pos(self, veh_id):
+
+        try:
+            pos = traci.vehicle.getPosition(veh_id)
+            return pos
+        except Exception as e:
+            logging.error(f"Failed to get vehicle position for vehicle ID '{veh_id}': {e}")
+            raise
+
+    def create_stop_veh(self, veh_id, end_pos, stop_route):
+        try:
+            traci.vehicle.add(vehID = veh_id, routeID=stop_route, typeID="car", depart=0, departPos=end_pos)
+            traci.vehicle.setSpeed(veh_id, 0)
+        except Exception as e:
+            logging.error(f"Failed to create stopped vehicle for vehicle ID '{veh_id}': {e}")
+            raise
+
+    def set_veh_lane(self, veh_id, lane):
+        try:
+            traci.vehicle.moveTo(veh_id, lane, 0)
+            traci.vehicle.setLaneChangeMode(veh_id, 0)
+        except Exception as e:
+            logging.error(f"Failed to set vehicle to desired lane for vehicle ID '{veh_id}': {e}")
+            raise
+
+    def get_veh_lane(self, veh_id):
+
+        return traci.vehicle.getLaneID(veh_id)
 
+    def get_leftmost_lane(self, veh_id):
+        try:
+            current_lane_id = traci.vehicle.getLaneID(veh_id)
+            edge_id = current_lane_id.rsplit("_", 1)[0]
+            num_lanes = traci.edge.getLaneNumber(edge_id)
+            leftmost_lane_index = num_lanes -1
+
+            return leftmost_lane_index
+        except Exception as e:
+            logging.error(f"Failed to get leftmost lane of vehicle location for vehicle ID '{veh_id}': {e}")
+            raise

co-simulation/NOTICE-THIRD-PARTY.md

@@ -333,6 +333,22 @@ org.leadpony.justify (1.1.0)
  * Source: https://github.com/leadpony/justify
 
 
+Proj4J (1.3.0)
+
+ * License: Apache License, Version 2.0
+ * Maven artifact: `org.locationtech.proj4j:proj4j:1.3.0`
+ * Project: https://github.com/locationtech/proj4j
+ * Source: https://github.com/locationtech/proj4j
+
+
+Proj4J EPSG (1.3.0)
+
+ * License: Apache License, Version 2.0, EPSG database distribution license
+ * Maven artifact: `org.locationtech.proj4j:proj4j-epsg:1.3.0`
+ * Project: https://github.com/locationtech/proj4j
+ * Source: https://github.com/locationtech/proj4j
+
+
 Protocol Buffers [Core] (3.8.0)
 
  * License: 3-Clause BSD License

co-simulation/bundle/pom.xml

@@ -149,6 +149,11 @@
             <artifactId>mosaic-carma-cloud</artifactId>
             <version>${mosaic.version}</version>
         </dependency>
+        <dependency>
+            <groupId>org.eclipse.mosaic</groupId>
+            <artifactId>mosaic-carma-messenger</artifactId>
+            <version>${mosaic.version}</version>
+        </dependency>
         <!-- Tutorial Applications -->
 
         <dependency>
@@ -179,6 +184,7 @@
 
 
 
+
         <!-- Dependencies -->
         <dependency>
             <groupId>ch.qos.logback</groupId>

co-simulation/bundle/src/assembly/resources/etc/logback.xml

@@ -105,6 +105,13 @@
                     <pattern>%date %-5level %C{0}:%line - %msg%n</pattern>
                 </encoder>
             </appender>
+            <appender name="CarmaMessengerLog" class="ch.qos.logback.core.FileAppender">
+                <charset>UTF-8</charset>
+                <file>${logDirectory}/CarmaMessenger.log</file>
+                <encoder class="ch.qos.logback.classic.encoder.PatternLayoutEncoder">
+                    <pattern>%date %-5level %C{0}:%line - %msg%n</pattern>
+                </encoder>
+            </appender>
             <appender name="EnvironmentLog" class="ch.qos.logback.core.FileAppender">
                 <charset>UTF-8</charset>
                 <file>${logDirectory}/Environment.log</file>
@@ -220,6 +227,10 @@
                 <appender-ref ref="CarmaCloudLog"/>
             </logger>
 
+            <logger name="org.eclipse.mosaic.fed.carmamessenger" additivity="false" level="TRACE">
+                <appender-ref ref="CarmaMessengerLog"/>
+            </logger>
+
             <logger name="org.eclipse.mosaic.fed.output" additivity="false" level="INFO">
                 <appender-ref ref="MosaicLog"/>
             </logger>