Merge feat/test mode into main (#11)

* 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

* fix based on upgrade of ros-bridge

* rename generated files

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

.gitignore

@@ -7,6 +7,7 @@
 **/*.out
 **/*.gz
 **/*.pyc
+**/*.zip
 
 **/.ipynb_checkpoints/*
 **/build/

src/graic_msgs/msg/WaypointInfo.msg

@@ -1,2 +1,3 @@
-bool isFinal
+bool reachedFinal
+string role_name
 geometry_msgs/Vector3 location

src/perception_module/graic_raceinfo_publisher/launch/graic_raceinfo_publisher.launch

@@ -5,12 +5,12 @@
   <arg name='timeout' default='10'/>
   <arg name="role_name" default="ego_vehicle" />
 
-  <param name="/carla/host" value="$(arg host)" />
-  <param name="/carla/port" value="$(arg port)" />
   <param name="/carla/timeout" value="$(arg timeout)" />
 
   <node pkg="graic_raceinfo_publisher" type="graic_raceinfo_publisher.py" name="graic_raceinfo_publisher_$(arg role_name)" output="screen">
     <param name="role_name" value="$(arg role_name)" />
+    <param name='host' value='$(arg host)' />
+    <param name='port' value='$(arg port)' />
   </node>
 
 </launch>

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

@@ -278,11 +278,11 @@ def publisher(percep_mod, role_name, label_list):
 if __name__ == "__main__":
     # reference: https://github.com/SIlvaMFPedro/ros_bridge/blob/master/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py
     rospy.init_node('graic_raceinfo_publisher', anonymous=True)
-    # host = rospy.get_param("/carla/host", "127.0.0.1")
-    # port = rospy.get_param("/carla/host", 2000)
+    host = rospy.get_param("~host", "localhost")
+    port = rospy.get_param("~port", 2000)
     # timeout = rospy.get_param("/carla/timeout", 10)
     role_name = rospy.get_param("~role_name", "ego_vehicle")
-    client = carla.Client('localhost', 2000)
+    client = carla.Client(host, port)
     # client.set_timeout(timeout)
     world = client.get_world()
     pm = PerceptionModule(world, role_name)

src/perception_module/location_publisher/launch/location_publisher.launch

@@ -5,12 +5,12 @@
   <arg name='timeout' default='10'/>
   <arg name="role_name" default="ego_vehicle" />
 
-  <param name="/carla/host" value="$(arg host)" />
-  <param name="/carla/port" value="$(arg port)" />
   <param name="/carla/timeout" value="$(arg timeout)" />
 
   <node pkg="location_publisher" type="location_publisher.py" name="location_publisher_$(arg role_name)" output="screen">
     <param name="role_name" value="$(arg role_name)" />
+    <param name='host' value='$(arg host)' />
+    <param name='port' value='$(arg port)' />
   </node>
 
 </launch>

src/perception_module/location_publisher/src/location_publisher/location_publisher.py

@@ -63,11 +63,11 @@ def publisher(location_module, role_name):
 if __name__ == "__main__":
     # reference: https://github.com/SIlvaMFPedro/ros_bridge/blob/master/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py
     rospy.init_node('graic_raceinfo_publisher', anonymous=True)
-    # host = rospy.get_param("/carla/host", "127.0.0.1")
-    # port = rospy.get_param("/carla/host", 2000)
+    host = rospy.get_param("~host", "localhost")
+    port = rospy.get_param("~port", 2000)
     # timeout = rospy.get_param("/carla/timeout", 10)
     # role_name = rospy.get_param('role_name', 'ego_vehicle')
-    client = carla.Client('localhost', 2000)
+    client = carla.Client(host, port)
     role_name = rospy.get_param("~role_name", "ego_vehicle")
     # client.set_timeout(timeout)
     world = client.get_world()

src/race/launch/carla_single.launch

@@ -9,16 +9,18 @@
   <arg name="track" default='t1_triple'/>
 
   <!-- Synchronous mode-->
-  <arg name='synchronous_mode' default=''/>
-  <arg name='synchronous_mode_wait_for_vehicle_control_command' default=''/>
-  <arg name='fixed_delta_seconds' default=''/>
+  <arg name='synchronous_mode' default='True'/>
+  <arg name='synchronous_mode_wait_for_vehicle_control_command' default='False'/>
+  <arg name='fixed_delta_seconds' default='0.05'/>
 
   <arg name='N' default='1' />
   <arg name='log' default='/tmp/' />
 
   <arg name='model_type' default='model_free' />
   <arg name='num_wheels' default='4' />
 
+  <arg name='offscreen' default='false' />
+
   <include file="$(find carla_ros_bridge)/launch/carla_ros_bridge.launch">
     <arg name='host' value='$(arg host)'/>
     <arg name='port' value='$(arg port)'/>
@@ -30,13 +32,19 @@
   </include>
 
   <node pkg="race" type="run.py" name="race_run" output="screen">
+    <param name="host" value="$(arg host)" />
+    <param name="port" value="$(arg port)" />
     <param name="N" value="$(arg N)" />
     <param name="log" value="$(arg log)" />
     <param name="track" value="$(arg track)" />
     <param name="model_type" value="$(arg model_type)" />
     <param name="num_wheels" value="$(arg num_wheels)" />
+    <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>
 
 </launch>

src/race/launch/spawn_vehicle.launch

@@ -1,12 +1,15 @@
 <!-- -->
 <launch>
   <!-- Ego vehicle -->
+  <arg name='host' default='localhost'/>
+  <arg name='port' default='2000'/>
   <arg name='role_name' default='ego_vehicle'/>
   <arg name='config_file' default=''/>
   <arg name="vehicle_filter" default='vehicle.*'/>
   <arg name="spawn_point" default=""/><!-- use comma separated format "x,y,z,roll,pitch,yaw" -->
   <arg name="track" default="t1_triple" />
   <arg name="model_free" default="1" />
+  <arg name="offscreen" default="false" />
 
   <!-- the ego vehicle, that will be controlled by an agent (e.g. carla_ad_agent) -->
   <include file="$(find carla_spawn_objects)/launch/carla_example_ego_vehicle.launch">
@@ -18,10 +21,7 @@
 
   <!-- the control bridge -->
   <!-- carla manual control -->
-  <!-- <include file="$(find carla_manual_control)/launch/carla_manual_control.launch">
-    <arg name='role_name' value='$(arg role_name)'/>
-  </include> -->
-  <node pkg="race" type="carla_manual_control.py" name="carla_manual_control_$(arg role_name)" output="screen">
+  <node pkg="race" type="carla_manual_control.py" name="carla_manual_control_$(arg role_name)" output="screen" unless="$(arg offscreen)">
     <param name="role_name" value="$(arg role_name)"/>
   </node>
 
@@ -46,16 +46,22 @@
 
   <include file="$(find model_based_node)/launch/model_based_node.launch" unless="$(arg model_free)">
     <arg name='role_name' value='$(arg role_name)' />
+    <arg name='host' value='$(arg host)' />
+    <arg name='port' value='$(arg port)' />
   </include>
 
   <!-- the perception module -->
   <!-- graic race info -->
   <include file="$(find graic_raceinfo_publisher)/launch/graic_raceinfo_publisher.launch">
     <arg name='role_name' value='$(arg role_name)' />
+    <arg name='host' value='$(arg host)' />
+    <arg name='port' value='$(arg port)' />
   </include>
 
   <include file="$(find location_publisher)/launch/location_publisher.launch">
     <arg name='role_name' value='$(arg role_name)' />
+    <arg name='host' value='$(arg host)' />
+    <arg name='port' value='$(arg port)' />
   </include>
 
   <!-- <include file="$(find envobj_bb_publisher)/launch/envobj_bb_publisher.launch">
@@ -64,11 +70,15 @@
 
   <!-- the evaluation module -->
   <include file="$(find evaluation_node)/launch/evaluation_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>
 
   <include file="$(find waypoint_node)/launch/waypoint_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>

src/race/scripts/generate_video.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+import sys
+import copy
+import time
+import rospy
+import rospkg
+
+import cv2
+import math
+import numpy as np
+import os
+
+import ffmpeg as ff
+from std_msgs.msg import String, Float32
+from sensor_msgs.msg import Image
+from sensor_msgs.msg import NavSatFix
+from cv_bridge import CvBridge, CvBridgeError
+# reference:
+# https://gist.github.com/rethink-imcmahon/77a1a4d5506258f3dc1f
+class VideoGeneration:
+    def __init__(self, role_name, path):
+         self.bridge = CvBridge()
+         self.role_name = role_name
+         self.counter = 0
+         self.path = path
+         self.image_sub = rospy.Subscriber("/carla/hero0/video_output", Image, self.image_callback)
+    def image_callback(self, msg):
+        try:
+            img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+        except CvBridgeError as e:
+            print(e)
+        else:
+            img_name = "frame{:04d}.jpg".format(self.counter)
+            # print(img_name)
+            cv2.imwrite(os.path.join(self.path, img_name), img)
+            self.counter += 1
+    def generate_video_from_images(self):
+        while not rospy.is_shutdown():
+            pass
+        (
+            ff
+            .input(os.path.join(self.path, "*.jpg"), pattern_type='glob', framerate=20)
+            .output("output_{}_{}.mp4".format(self.role_name, time.asctime().replace(' ', '_').replace(':', '_')))
+            .run()
+        )
+
+        # os.system("./video_generation/clean.sh")
+        # p=os.path.join("/home/carla/graic-workspace/src/race/scripts/video_generation/", "output.mp4")
+        # os.system("ffmpeg -framerate 25 -i frame%04d.jpg -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p " + p)
+
+if __name__ == "__main__":
+    rospy.init_node("VideoGeneration_Node")
+    role_name = rospy.get_param("~role_name", "hero0")
+
+
+    path = os.getcwd() + '/video_generation/images/'
+    # print(path)
+    # os.chdir(os.path.dirname(__file__))
+    # cwd = os.getcwd()
+    vg = VideoGeneration(role_name, path)
+
+    try:
+        vg.generate_video_from_images()
+    except rospy.exceptions.ROSInterruptException:
+        rospy.loginfo("Shutting down video generation node")