-
Notifications
You must be signed in to change notification settings - Fork 23
/
sim_for_rtreach_multi_agent.launch
76 lines (51 loc) · 3.55 KB
/
sim_for_rtreach_multi_agent.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
<?xml version="1.0"?>
<launch>
<!--This prevents the gazebo from displaying in a window
The idea is this will help with performance. We will
have to see-->
<arg name="gui" default="true" />
<arg name="docker" default="false"/>
<!--How many boxes to let rviz visualize-->
<arg name="box_display_limit" default="100"/>
<!-- reach-time is the time we are computing reachability for-->
<arg name="reach_time" default="1.0"/>
<!--wall-time is the time we allow the reachability algorithm to run for-->
<arg name="wall_time" default="1.0"/>
<arg name="debug" default=""/>
<!--number of vehicles to launch in the simulation environment-->
<arg name='number_of_cars' default='2'/>
<!--Select the car we are computing reachability for-->
<arg name='car_name' default='racecar2'/>
<arg name="rviz_disp" default="multiagent_reach.rviz"/>
<!--network model utilized for end-to-end driving-->
<arg name="model_name" default="minivgg_center_data.hdf5"/>
<!--Launch The Model-->
<node pkg="computer_vision" name="lec_model" type="ros_classifier.py" args="$(arg car_name) $(find computer_vision)models/$(arg model_name) 1" required="true" output="log"/>
<!-- This is the main launch file that launches the world, plugins, controllers, vehicles -->
<include file="$(find race)/launch/multi_parametrizeable.launch">
<arg name="number_of_cars" value="$(arg number_of_cars)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="rviz_disp" value="$(arg rviz_disp)"/>
</include>
<!--launch the speed node it will be constant in this case-->
<node name="speed_node_$(arg car_name)" pkg="race" type="speed_node.py" args='$(arg car_name)'/>
<!-- generation of safety controller-->
<node pkg="race" type="safety_controller.py" name="safety_node" args="$(arg car_name)/scan $(arg car_name)/safety" output="screen"/>
<!-- decision manager-->
<node pkg="race" type="rtreach_decision_manager.py" name="decision_manager" args="$(arg car_name) vesc2" output="screen"/>
<!-- These are the nodes for publishing the reachability information for each of the vehicles-->
<node name="vehicle_1" pkg="rtreach" type="visualize_obs" unless="$(arg docker)" args="racecar $(arg wall_time) $(arg reach_time) $(arg box_display_limit)" output="log"/>
<node name="vehicle_2" pkg="rtreach" type="visualize_obs" unless="$(arg docker)" args="racecar3 $(arg wall_time) $(arg reach_time) $(arg box_display_limit)" output="log"/>
<!--Node for Visualizing reachsets that allows customization through parameters-->
<node name="vis_agent_param" pkg="rtreach" unless="$(arg docker)" type="vis_node_param" output="screen" args=" 10 $(arg reach_time) $(arg box_display_limit) $(arg debug)"/>
<node pkg="race" name="disparity_extender_racear" type="disparity_extender_vanderbilt_gen.py" args="racecar"/>
<node pkg="race" name="disparity_extender_racear3" if="$(eval arg('number_of_cars')==3)" type="disparity_extender_vanderbilt_gen.py" args="racecar3"/>
<!--Node for wall points-->
<node name="wall_points" pkg="rtreach" type="publish_wall_points.py" unless="$(arg docker)" output="screen" args="porto"/>
<!--Reachability safety node that incorporates dynamic obstacles-->
<node name="reach_agent_param" pkg="rtreach" type="reach_node_dyn" output="log" unless="$(arg docker)" args=" $(arg wall_time) $(arg reach_time) $(arg box_display_limit) $(arg debug)"/>
<!--ttc node-->
<node pkg="race" type="ttc.py" name="ttc" required="true" args = "$(arg car_name)" output="screen"/>
<!--plot reachability results-->
<node pkg="race" type="plot_reachability_results.py" name="res_plot" required="false" output="screen"/>
</launch>