Feat/scenario (#13)

* add setting for offscreen test mode

* add auto shutdown; add support for multiple port

* add scripts to generate videos

* fix path error

* add setting for offscreen test mode

* add auto shutdown; add support for multiple port

* add video generation code

* change frequency and publisher for synchronization

* add function to dump trajectories, add function to plot trajectories, auto start scenario and controller

* modify scenario runner path

* fix bugs for model based vehicle

* modify lane deviation rule

* modified model based node

* add tentative structure of scenario node

* integrate multiple scenario runner instance with graic

* map the waypoints onto scenarios

* first version of dynamic scenario generation

* make deterministic scenario configuration

* add reset after collision

* allow model base vehicle to run on tracks with elevation

* change evaluation function to add up time passing each waypoint to calculate score

* parameterize the port for scenario arguments

* update baseline controller; add track5 information

* add configuration node for track config

* add config for user to know vehicle typeid and sensing radius

* change publisher queue size to 1

Co-authored-by: Lucas <yixuanj3@illinois.edu>
Co-authored-by: Yangge Li <li213@illinois.edu>

.gitignore

@@ -14,4 +14,5 @@
 **/devel/
 **/.vscode/*
 **/score_*
+**/trajectory_*
 .catkin_workspace

src/perception_module/graic_raceinfo_publisher/src/graic_raceinfo_publisher/graic_raceinfo_publisher.py

@@ -10,7 +10,7 @@
 from graic_msgs.msg import BBSingleInfo
 from geometry_msgs.msg import Vector3
 class PerceptionModule():
-    def __init__(self, carla_world, role_name, radius=15):
+    def __init__(self, carla_world, role_name, radius=60):
         self.sensing_radius = radius # default ?????
         self.world = carla_world
         self.vehicle = None

src/race/CMakeLists.txt

@@ -163,6 +163,7 @@ catkin_install_python(PROGRAMS
   scripts/spawn_npc.py
   scripts/carla_manual_control.py
   scripts/video_output.py
+  src/baseline.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

src/race/launch/carla_single.launch

@@ -26,9 +26,10 @@
     <arg name='port' value='$(arg port)'/>
     <arg name='town' value='$(arg track)'/>
     <arg name='timeout' value='$(arg timeout)'/>
-    <arg name='synchronous_mode' value='$(arg synchronous_mode)'/>
-    <arg name='synchronous_mode_wait_for_vehicle_control_command' value='$(arg synchronous_mode_wait_for_vehicle_control_command)'/>
-    <arg name='fixed_delta_seconds' value='$(arg fixed_delta_seconds)'/>
+    <!-- <arg name='passive' value='True'/> -->
+    <!-- <arg name='synchronous_mode' value='$(arg synchronous_mode)'/> -->
+    <!-- <arg name='synchronous_mode_wait_for_vehicle_control_command' value='$(arg synchronous_mode_wait_for_vehicle_control_command)'/> -->
+    <!-- <arg name='fixed_delta_seconds' value='$(arg fixed_delta_seconds)'/> -->
   </include>
 
   <node pkg="race" type="run.py" name="race_run" output="screen">
@@ -42,9 +43,9 @@
     <param name="offscreen" value="$(arg offscreen)" />
   </node>
 
-  <node pkg="race" type="spawn_npc.py" name="spawn_npc" output="screen">
+  <!-- <node pkg="race" type="spawn_npc.py" name="spawn_npc" output="screen">
     <param name="host" value="$(arg host)" />
     <param name="port" value="$(arg port)" />
-  </node>
+  </node> -->
 
 </launch>

src/race/launch/spawn_vehicle.launch

@@ -29,11 +29,23 @@
     <param name="role_name" value="$(arg role_name)"/>
   </node>
 
+  <!--NOTE Uncomment the next three lines to automatically starting baseline controller-->
+  <!-- <node pkg="race" type="baseline.py" name="baseline_controller_$(arg role_name)" output="screen">
+    <param name="role_name" value="$(arg role_name)"/>
+  </node> -->
+
   <!-- control nodes -->
   <include file="$(find carla_ackermann_control)/launch/carla_ackermann_control.launch">
     <arg name='role_name' value='$(arg role_name)' />
   </include>
 
+  <include file="$(find scenario_node)/launch/scenario_node.launch">
+    <arg name='host' value='$(arg host)'/>
+    <arg name='port' value='$(arg port)'/>
+    <arg name='role_name' value='$(arg role_name)' />
+    <arg name='track' value='$(arg track)' />
+  </include>
+
   <group if="$(arg model_free)">
     <include file="$(find ackermann_node)/launch/ackermann_node.launch">
       <arg name='role_name' value='$(arg role_name)' />
@@ -74,6 +86,7 @@
     <arg name='port' value='$(arg port)' />
     <arg name='role_name' value='$(arg role_name)' />
     <arg name='track' value='$(arg track)' />
+    <arg name='model_free' value='$(arg model_free)' />
   </include>
 
   <include file="$(find waypoint_node)/launch/waypoint_node.launch">

src/race/scripts/run.py

@@ -1,7 +1,7 @@
 import os
 import sys
 # import argparse
-
+import rospkg
 import subprocess
 # from subprocess import DEVNULL, STDOUT, check_call
 import os, signal
@@ -12,13 +12,13 @@
 import rospy
 from graic_msgs.msg import WaypointInfo
 
-four_wheel_vehicle = ['vehicle.audi.a2','vehicle.tesla.model3','vehicle.bmw.grandtourer','vehicle.audi.etron','vehicle.seat.leon','vehicle.mustang.mustang','vehicle.tesla.cybertruck','vehicle.lincoln.mkz2017','vehicle.lincoln2020.mkz2020','vehicle.dodge_charger.police','vehicle.audi.tt','vehicle.jeep.wrangler_rubicon','vehicle.chevrolet.impala','vehicle.nissan.patrol','vehicle.nissan.micra','vehicle.mercedesccc.mercedesccc','vehicle.mini.cooperst','vehicle.chargercop2020.chargercop2020','vehicle.toyota.prius','vehicle.mercedes-benz.coupe','vehicle.citroen.c3','vehicle.charger2020.charger2020']
+four_wheel_vehicle = ['vehicle.audi.a2','vehicle.tesla.model3','vehicle.bmw.grandtourer','vehicle.audi.etron','vehicle.seat.leon','vehicle.mustang.mustang','vehicle.tesla.cybertruck','vehicle.lincoln.mkz2017','vehicle.lincoln2020.mkz2020','vehicle.dodge_charger.police','vehicle.audi.tt','vehicle.jeep.wrangler_rubicon','vehicle.chevrolet.impala','vehicle.nissan.patrol','vehicle.nissan.micra','vehicle.mercedesccc.mercedesccc','vehicle.mini.cooperst','vehicle.chargercop2020.chargercop2020','vehicle.toyota.prius','vehicle.mercedes-benz.coupe','vehicle.citroen.c3','vehicle.charger2020.charger2020', 'vehicle.bmw.isetta']
 two_wheel_vehicle = ['vehicle.bh.crossbike','vehicle.kawasaki.ninja','vehicle.gazelle.omafiets','vehicle.yamaha.yzf','vehicle.harley-davidson.low_rider','vehicle.diamondback.century']
 
 car_name_prefix = 'hero' # NOTE 
 
 class CommandNode:
-    def __init__(self, host, port, N, log, track, model_type, num_wheels, offscreen, set_spectator=False):
+    def __init__(self, host, port, N, log, track, model_type, num_wheels, offscreen, set_spectator=True):
         self.host = host 
         self.port = port
         self.N = N 
@@ -44,7 +44,9 @@ def setSpectator(self):
         client = carla.Client(self.host, self.port)
         world = client.get_world()
         spectator = world.get_spectator()
-        center = np.mean([v['init_pose'][:3] for v in vehicles], axis=0)
+        # center = np.mean([self.vehicles[role_name]['init_pose'][:3] for role_name in self.vehicles], axis=0)
+        # print(center)
+        center = [164,11,4]
         transform = carla.Transform(carla.Location(x=center[0], y=-center[1], z=center[2] + 40),
                                     carla.Rotation(pitch=-89, yaw=-62.5))
         spectator.set_transform(transform)
@@ -69,11 +71,11 @@ def spawnCars(self):
 
             if self.model_type != "model_free" and self.model_type != "model_based":
                 rospy.logwarn("Wrong choice of model type %s; use model_free as default"%self.model_type)
-                model_type = "model_free"
+                self.model_type = "model_free"
 
-            if self.model_type == "model_based" and self.track == "t2_triple":
-                rospy.logerr("Please don't chooce model_based vehicle when running track2")
-                raise rospy.exceptions.ROSInterruptException
+            # if self.model_type == "model_based" and self.track == "t2_triple":
+            #     rospy.logerr("Please don't chooce model_based vehicle when running track2")
+            #     raise rospy.exceptions.ROSInterruptException
 
             if self.model_type == "model_free":
                 v['model_free'] = 1
@@ -84,13 +86,17 @@ def spawnCars(self):
                 init_pose = [164,11+i*10,4,0,0,-180]
             elif self.track == "t2_triple":
                 init_pose = [95.5,107+i*10,4,0,0,-136]
+            elif self.track == "track5":
+                init_pose = [215.90, 202.04+i*10,4,0,0,82]
+            else:
+                init_pose = [0,0,4,0,0,0]
             v['init_pose'] = init_pose
             obj = obj.replace('[[spawn_point]]', '"x": %f, "y": %f, "z": %f, "roll": %f, "pitch": %f, "yaw": %f'%tuple(init_pose))
 
             v['track'] = self.track
 
             if self.num_wheels == 4 or self.model_type=="model_based":
-                vehicle = random.choice(four_wheel_vehicle)
+                vehicle = four_wheel_vehicle[1] # random.choice(four_wheel_vehicle)
             elif self.num_wheels == 2:
                 vehicle = random.choice(two_wheel_vehicle)
             else:
@@ -105,6 +111,14 @@ def spawnCars(self):
 
             v['finished'] = False
 
+            rospack = rospkg.RosPack()
+            config_path = rospack.get_path('config_node')
+            fname = config_path + '/' + 'race_config'
+            f = open(fname, 'w')
+            f.write((vehicle+'\n'))
+            f.write(str(60))
+            f.close()
+
             cmd = ('roslaunch race spawn_vehicle.launch host:=%s port:=%s config_file:=%s role_name:=%s track:=%s offscreen:=%s model_free:=%s &> %s')%tuple([self.host, self.port, v['json_file'], v['role_name'], v['track'], self.offscreen, v['model_free'], v['launch_log']])
             # The os.setsid() is passed in the argument preexec_fn so
             # it's run after the fork() and before  exec() to run the shell.
@@ -129,7 +143,7 @@ def checkFinish(self):
         if count == self.N:
             self.shut_down()
 def run(cn):
-    rate = rospy.Rate(10)
+    rate = rospy.Rate(20)
     rospy.on_shutdown(cn.shut_down)
 
     while not rospy.is_shutdown():
@@ -154,6 +168,16 @@ def run(cn):
     time.sleep(10)
     try: 
         cn = CommandNode(host, port, N, log, track, model_type, num_wheels, offscreen)
+
+        # res = subprocess.run(["python3",
+        #     "/home/carla/scenario_runner/scenario_runner.py",
+        #     "--route",
+        #     "/home/carla/scenario_runner/srunner/data/routes_race.xml", 
+        #     "/home/carla/scenario_runner/srunner/data/scenario_race.json",
+        #     "0", 
+        #     "--output"
+        # ], capture_output=True)
+
         run(cn)
     except rospy.exceptions.ROSInterruptException:
-        rospy.loginfo("CommandNode shut down")
+        rospy.loginfo("CommandNode shut down")

src/race/scripts/video_output.py

@@ -18,7 +18,7 @@
 class VideoOutput:
     def __init__(self, role_name='ego_vehicle'):
         self.bridge = CvBridge()
-        self.videoPub = rospy.Publisher("/carla/%s/video_output"%role_name, Image, queue_size=10)
+        self.videoPub = rospy.Publisher("/carla/%s/video_output"%role_name, Image, queue_size=1)
         self.videoSub = rospy.Subscriber("/carla/%s/rgb_view/image"%role_name, Image, self.imageCallback)
         self.scoreSub = rospy.Subscriber("/carla/%s/score"%role_name, Float32, self.scoreCallback)
         self.reachedSub = rospy.Subscriber("/carla/%s/reached"%role_name, String, self.reachedCallback)

src/race/src/baseline.py

@@ -1,4 +1,5 @@
 import rospy
+import rospkg
 import numpy as np
 import argparse
 import time
@@ -23,7 +24,7 @@ def __init__(self, role_name):
         self.target_y = None
         self.change_lane = False
         self.change_lane_wp_idx = 0
-        self.detect_dist = 15
+        self.detect_dist = 30
         self.speed = 20
 
         self.reachEnd = False
@@ -114,11 +115,11 @@ def get_ref_state(self, currState, obstacleList):
                     self.vehicle_state = "stop"
 
         if self.vehicle_state == "stop":
-            self.speed = 0
+            self.speed = 5
         else:
             self.speed = 20
 
-        print(front_dist, self.lane_state, self.vehicle_state, obs_front, obs_left, obs_right)
+        # print(front_dist, self.lane_state, self.vehicle_state, obs_front, obs_left, obs_right)
 
         while not self.target_x or not self.target_y:
             continue
@@ -139,15 +140,15 @@ def get_ref_state(self, currState, obstacleList):
 
             if self.vehicle_state == "turn-right":
                 # self.change_lane = False
-                tmp_x = 4
+                tmp_x = 6
                 tmp_y = 0
                 x_offset = np.cos(target_orientation+np.pi/2)*tmp_x - np.sin(target_orientation+np.pi/2)*tmp_y
                 y_offset = np.sin(target_orientation+np.pi/2)*tmp_x + np.cos(target_orientation+np.pi/2)*tmp_y
                 self.target_x = self.target_x + x_offset
                 self.target_y = self.target_y + y_offset
             elif self.vehicle_state == "turn-left":
                 # self.change_lane = False
-                tmp_x = 4
+                tmp_x = 6
                 tmp_y = 0
                 x_offset = np.cos(target_orientation-np.pi/2)*tmp_x - np.sin(target_orientation-np.pi/2)*tmp_y
                 y_offset = np.sin(target_orientation-np.pi/2)*tmp_x + np.cos(target_orientation-np.pi/2)*tmp_y
@@ -239,7 +240,7 @@ def obstacleCallback(self, data):
 
 def run_model(role_name):
 
-    rate = rospy.Rate(100)  # 100 Hz    
+    rate = rospy.Rate(20)  # 100 Hz    
 
     perceptionModule = VehiclePerception(role_name=role_name)
     decisionModule = VehicleDecision(role_name)
@@ -268,15 +269,13 @@ def shut_down():
         controlModule.execute(currState, refState)
 
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser(description="Running vechile")
-
-    role_name_default = 'ego_vehicle'
-
-    parser.add_argument('--name', type=str, help='Rolename of the vehicle', default=role_name_default)
-    argv = parser.parse_args()
-    role_name = argv.name
-    rospy.init_node("baseline")
+    roskpack = rospkg.RosPack() 
+    config_path = roskpack.get_path('config_node')
+    race_config = open(config_path+'/'+'race_config', 'rb')
+    vehicle_typeid = race_config.readline().decode('ascii').strip()
+    sensing_radius = race_config.readline().decode('ascii').strip()
     role_name = 'ego_vehicle'
+    rospy.init_node("baseline")
     try:
         run_model(role_name)
     except rospy.exceptions.ROSInterruptException:

src/race/src/starter.py

@@ -1,4 +1,5 @@
 import rospy
+import rospkg
 import numpy as np
 import argparse
 import time
@@ -76,15 +77,14 @@ def shut_down():
         controlModule.execute()
 
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser(description="Running vechile")
+    roskpack = rospkg.RosPack() 
+    config_path = roskpack.get_path('config_node')
+    race_config = open(config_path+'/'+'race_config', 'rb')
+    vehicle_typeid = race_config.readline().decode('ascii').strip()
+    sensing_radius = race_config.readline().decode('ascii').strip()
+    role_name = 'ego_vehicle'
 
-    role_name_default = 'ego_vehicle'
-
-    parser.add_argument('--name', type=str, help='Rolename of the vehicle', default=role_name_default)
-    argv = parser.parse_args()
-    role_name = argv.name
     rospy.init_node("baseline")
-    role_name = 'hero0'
     try:
         run_model(role_name)
     except rospy.exceptions.ROSInterruptException: