Add multiple vehicles function and decouple track feature

CMakeLists.txt

@@ -17,6 +17,14 @@ find_package(catkin REQUIRED COMPONENTS
    VehicleStateCurv.msg
    VehicleStateGlob.msg
    VehicleState.msg
+   TrackInfo.msg
+   NumVehicle.msg
+   VehicleList.msg
+ )
+
+ add_service_files(
+   FILES
+   AddNewVehicle.srv
  )
 
  generate_messages(

data/track_layout/ellipse.csv

@@ -0,0 +1,8 @@
+3.000000000000000000e+00,0.000000000000000000e+00
+2.356194490192344837e+00,-1.500000000000000000e+00
+2.000000000000000000e+00,0.000000000000000000e+00
+2.356194490192344837e+00,-1.500000000000000000e+00
+6.000000000000000000e+00,0.000000000000000000e+00
+2.356194490192344837e+00,-1.500000000000000000e+00
+2.000000000000000000e+00,0.000000000000000000e+00
+2.356194490192344837e+00,-1.500000000000000000e+00

data/track_layout/goggle_shaped.csv

@@ -0,0 +1,10 @@
+1.8,0.000000000000000000e+00
+2.4,-1.527887453682195e+00
+0.6,0.000000000000000000e+00
+2.4,-1.527887453682195e+00
+1.2,3.819718634205488e+00
+1.8,-2.864788975654116e+00
+1.2,3.819718634205488e+00
+2.4,-1.527887453682195e+00
+0.6,0.00000000000000e+00
+2.4,-1.527887453682195e+00

data/track_layout/l_shaped.csv

@@ -0,0 +1,6 @@
+1.000000000000000000e+00, 0.000000000000000000e+00
+4.500000000000000000e+00, 1.432394488000000000e+00
+2.250000000000000000e+00,-1.432394488000000000e+00
+4.500000000000000000e+00, 1.432394488000000000e+00
+2.864788976000000000e+00, 0.000000000000000000e+00
+2.250000000000000000e+00, 1.432394488000000000e+00

docs/realtime.md

@@ -10,6 +10,18 @@ Currently, the repository is tested with ROS Melodic on a Ubuntu 18.04 laptop. T
 ##### Examples
 ###### Start Simulation
 ```
-roslaunch car_racing car_racing_sim.launch ctrl_policy:=pid
+roslaunch car_racing car_racing_sim.launch track_layout:=goggle
 ```
-This allows you to run the simulator. The choices of ` ctrl_policy` could be `pid` and `mpc-lti`. Currently, only tracking function is defined in this simulator.
+This allows you to run the simulator and visualization node. Change the `track_layout`, you can get differnt tracks. Currently, following optitions are available: `ellipse`, `goggle`, `l_shape` .
+
+To add new vehicle with controller in the simulator, run the following commands in new terminals:
+
+```
+rosrun car_racing vehicle.py --veh-name vehicle1 --color blue --vx 0 --vy 0 --wz 0 --epsi 0 --s 0 --ey 0
+
+rosrun car_racing controller.py --ctrl-policy mpc-cbf --veh-name vehicle1
+
+
+```
+
+Where `--veh-name` is a self-defined vehicle's name, `--color` is the color of the vehicle in the animation. `--vs, --vy, --wz, --epsi, --s, --ey` is the initial state of the vehicle in Frenet coordinate. The choices of ` --ctrl-policy` could be `pid`, `mpc-lti` and `mpc-cbf`. 

launch/car_racing_sim.launch

@@ -1,15 +1,9 @@
 <launch>
-	<!-- Vehicle -->
-    <node pkg="car_racing" type="vehicle1.py" name="vehicle1" /> 
-
     <!-- Simulator -->
+    <arg name="track_layout" default = "ellipse"/>
+    <param name="track_layout" value="$(arg track_layout)"/>
     <node pkg="car_racing" type="simulator.py" name="simulator" />
 
-    <!-- Controller -->
-    <arg name="ctrl_policy" default = "pid"/>
-    <param name="ctrl_policy" value="$(arg ctrl_policy)"/>
-    <node pkg="car_racing" type="controller1.py" name="controller1"/>
-
     <!-- Visulization -->
     <node pkg="car_racing" type="visualization.py" name="visulization"/>
 

msg/NumVehicle.msg

@@ -0,0 +1 @@
+int32 num

msg/TrackInfo.msg

@@ -0,0 +1,4 @@
+float32 length
+float32 width
+int32 size
+float32[] point_and_tangent

msg/VehicleList.msg

@@ -0,0 +1 @@
+string[] vehicle_list

scripts/realtime/controller.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python
+import rospy
+import numpy as np
+import sys
+import time
+scripts_realtime_path = sys.path[0]
+src_utils_path = scripts_realtime_path+'/../../src/utils'
+src_path = scripts_realtime_path+'/../../src'
+matrix_A_path = scripts_realtime_path + '/../../data/sys/LTI/matrix_A.csv'
+matrix_B_path = scripts_realtime_path + '/../../data/sys/LTI/matrix_B.csv'
+sys.path.append(src_utils_path)
+sys.path.append(src_path)
+from car_racing.msg import VehicleControl, VehicleState
+import racing_env, realtime
+
+
+def set_controller(args):
+
+    veh_name = args["veh_name"]
+    ctrl_policy = args["ctrl_policy"]
+
+    node_name = 'controller_' + veh_name
+    rospy.init_node(node_name, anonymous=True)
+
+    loop_rate = 20
+    timestep = 1 / loop_rate
+
+    matrix_A = np.genfromtxt(matrix_A_path, delimiter=",")
+    matrix_B = np.genfromtxt(matrix_B_path, delimiter=",")
+    matrix_Q = np.diag([10.0, 0.0, 0.0, 0.0, 0.0, 10.0])
+    matrix_R = np.diag([0.1, 0.1])
+    if ctrl_policy == "mpc-lti":
+        ctrl = realtime.MPCTrackingRealtime(
+            matrix_A, matrix_B, matrix_Q, matrix_R, vt=0.8)
+    elif ctrl_policy == "pid":
+        ctrl = realtime.PIDTrackingRealtime(vt=0.8)
+    elif ctrl_policy == "mpc-cbf":
+        ctrl = realtime.MPCCBFRacingRealtime(
+            matrix_A, matrix_B, matrix_Q, matrix_R, vt=0.8)
+        ctrl.agent_name = veh_name
+        ctrl.set_subscriber_track()
+        ctrl.set_subscriber_veh()
+    else:
+        pass
+    ctrl.set_timestep(timestep)
+    ctrl.set_state(np.zeros(6,))
+    ctrl.u = np.zeros(2)
+    ctrl.set_subscriber_state(veh_name)
+    veh_input_topic = veh_name + '/input'
+    ctrl.__pub_input = rospy.Publisher(
+        veh_input_topic, VehicleControl, queue_size=10)
+
+    while not rospy.is_shutdown():
+        ctrl.calc_input()
+        u = ctrl.get_input()
+        ctrl.__pub_input.publish(VehicleControl(u[1], u[0]))
+
+
+if __name__ == '__main__':
+    try:
+        import argparse
+        parser = argparse.ArgumentParser()
+        parser.add_argument("--veh-name", type=str)
+        parser.add_argument("--ctrl-policy", type=str)
+        args = vars(parser.parse_args())
+        set_controller(args)
+
+    except rospy.ROSInterruptException:
+        pass

scripts/realtime/simulator.py

@@ -4,15 +4,18 @@
 import pylab
 import matplotlib.pyplot as plt
 import sys
-tests_path = sys.path[0]
-src_utils_path = tests_path+'/../../src/utils'
-src_path = tests_path+'/../../src'
+scripts_realtime_path = sys.path[0]
+src_utils_path = scripts_realtime_path+'/../../src/utils'
+src_path = scripts_realtime_path+'/../../src'
 sys.path.append(src_utils_path)
 sys.path.append(src_path)
-track_spec_path = tests_path + '/../../data/track_spec/default.csv'
-import realtime, vehicle_dynamics, racing_env
-from car_racing_sim.msg import VehicleControl, VehicleState, VehicleStateGlob, VehicleStateCurv
+ellipse_track_spec_path = scripts_realtime_path + '/../../data/track_layout/ellipse.csv'
+goggle_track_spec_path = scripts_realtime_path + '/../../data/track_layout/goggle_shaped.csv'
+l_track_spec_path = scripts_realtime_path + '/../../data/track_layout/l_shaped.csv'
 
+import realtime, vehicle_dynamics, racing_env
+from car_racing.msg import VehicleControl, VehicleState, VehicleStateGlob, VehicleStateCurv, NumVehicle, TrackInfo, VehicleList
+from car_racing.srv import AddNewVehicle
 
 
 def get_msg_xglob(state):
@@ -21,34 +24,62 @@ def get_msg_xglob(state):
 def get_msg_xcurv(state):
     return VehicleStateCurv(state[0], state[1], state[2], state[3], state[4], state[5])
 
-def start_simulator():
+def get_msg_track_info(track):
+    size0,size1 = np.shape(track.point_and_tangent)
+    point_and_tangent = np.zeros(size0*size1)
+    tmp = 0
+    for index_1 in range(size1):
+        for index_0 in range(size0):
+            point_and_tangent[tmp] = track.point_and_tangent[index_0,index_1]
+            tmp = tmp +1
+    return TrackInfo(track.lap_length, track.width, size1, point_and_tangent)
+
+def start_simulator(track_layout):
     rospy.init_node("simulator", anonymous = True)
-    track_spec = np.genfromtxt(track_spec_path, delimiter=",")
+    # get race track
+    if track_layout == "ellipse":
+        track_spec = np.genfromtxt(ellipse_track_spec_path, delimiter=",")
+    elif track_layout == "goggle":
+        track_spec = np.genfromtxt(goggle_track_spec_path, delimiter=",")
+    elif track_layout =="l_shape":
+        track_spec = np.genfromtxt(l_track_spec_path, delimiter=",")
+    else:
+        pass
     track_width = 1.0
     track = racing_env.ClosedTrack(track_spec, track_width)
-    sim1 = realtime.CarRacingSimRealtime()
-    sim1.set_track(track)
-    sim1.set_state_glob(np.zeros(6,))
-    sim1.set_state_curv(np.zeros(6,))
-    sim1.set_subscriber()
-    sim1.__pub_state = rospy.Publisher('simulator/vehicle1/state', VehicleState, queue_size = 10)
-
-    msg_state = VehicleState()
-    msg_state.name = "Vehicle1"
+
+    sim = realtime.CarRacingSimRealtime()
+    sim.set_track(track)    
+    # determin if new vehicle is added
+    s = rospy.Service('add_vehicle_simulator',AddNewVehicle,sim.add_vehicle)
 
     loop_rate = 100
-    r = rospy.Rate(loop_rate)
+    r = rospy.Rate(loop_rate)  
+
+    sim.__pub_veh_list = rospy.Publisher('vehicle_list', VehicleList, queue_size = 10)
+    sim.__pub_veh_num = rospy.Publisher('vehicle_num', NumVehicle, queue_size = 10)    
+    sim.__pub_track = rospy.Publisher('track_info', TrackInfo, queue_size = 10)
 
-
-    while not rospy.is_shutdown():   
-        msg_state.state_curv = get_msg_xcurv(sim1.vehicle_state_curv)
-        msg_state.state_glob = get_msg_xglob(sim1.vehicle_state_glob)
-        sim1.__pub_state.publish(msg_state)
+    while not rospy.is_shutdown():
+        sim.num_vehicle = 0
+        vehicle_list = []
+        for name in sim.vehicles:
+            sim.vehicles[name].msg_state.state_curv = get_msg_xcurv(sim.vehicles[name].xcurv)
+            sim.vehicles[name].msg_state.state_glob = get_msg_xglob(sim.vehicles[name].xglob)
+            tmp = 'simulator/' + name +'/state'
+            sim.vehicles[name].__pub_state = rospy.Publisher(tmp, VehicleState, queue_size=10)
+            sim.vehicles[name].__pub_state.publish(sim.vehicles[name].msg_state)
+            vehicle_list.append(name)
+            sim.num_vehicle = sim.num_vehicle + 1
+        sim.__pub_veh_list.publish(VehicleList(vehicle_list))
+        sim.__pub_veh_num.publish(NumVehicle(sim.num_vehicle))
+        sim.__pub_track.publish(get_msg_track_info(track))
         r.sleep()
 
 
 if __name__ == '__main__':
     try:
-        start_simulator()
+        track_layout = rospy.get_param("track_layout")
+        start_simulator(track_layout)
     except rospy.ROSInterruptException:
         pass