-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathmarble4
More file actions
executable file
·182 lines (166 loc) · 8.08 KB
/
Copy pathmarble4
File metadata and controls
executable file
·182 lines (166 loc) · 8.08 KB
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
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
#!/bin/bash
if [ "$1" == "start" ]; then
echo "Starting MARBLE services..."
export X1_SENSOR_CONFIG_4=1
# Run our launch file
if [ "$2" == "urban" ]; then
roslaunch ~/marble/scripts/subt-4.launch scenario:=urban_underground &
else
roslaunch ~/marble/scripts/subt-4.launch scenario:=tunnel_practice_2 &
fi
sleep 10
# Run the python scripts
# TF Transforms
python2 ~/marble/scripts/world_to_bot_tf.py &
# Odometry correction
python2 ~/catkin/src/marble-frontier/FastMarching3D_ROS/src/subt_true_odom.py X1 &
python2 ~/catkin/src/marble-frontier/FastMarching3D_ROS/src/subt_true_odom.py X2 &
python2 ~/catkin/src/marble-frontier/FastMarching3D_ROS/src/subt_true_odom.py X3 &
python2 ~/catkin/src/marble-frontier/FastMarching3D_ROS/src/subt_true_odom.py X4 &
# Start comms
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_handler.py Base B01 B02 B03 B04 B05 B06 B07 B08 X1 X2 X3 X4 &
sleep 5
# Turn on headlights. See if we can move to launch file
rosservice call /X1/left_headlight/enable "data: true"
rosservice call /X1/right_headlight/enable "data: true"
rosservice call /X2/left_headlight/enable "data: true"
rosservice call /X2/right_headlight/enable "data: true"
# Start the comms checker
~/marble/scripts/marble commscheck
# Start Multi-Agent nodes
# ~/marble/scripts/marble multiagent
sleep 10
roslaunch ~/marble/scripts/rviz-4.launch &
elif [ "$1" == "stop" ]; then
if [ "$2" == "explore_frontier" ]; then
echo "Stopping MARBLE frontier exploration..."
kill -9 $(pgrep -f 'python2.*subt_guidance_node.py')
rosnode kill X1/global_planning
rosnode kill X2/global_planning
rosnode kill X3/global_planning
rosnode kill X4/global_planning
elif [ "$2" == "explore_graph" ]; then
echo "Stopping MARBLE graph exploration..."
rosnode kill X1/centering
rosnode kill X1/cmd_vel_switch
rosnode kill X1/gbm_node
rosnode kill X1/graph2path
rosnode kill X1/graphmsg2rviz
rosnode kill X1/map2graph
rosnode kill X2/centering
rosnode kill X2/cmd_vel_switch
rosnode kill X2/gbm_node
rosnode kill X2/graph2path
rosnode kill X2/graphmsg2rviz
rosnode kill X2/map2graph
kill -9 $(pgrep -f 'python.*graph.*.py')
killall -HUP gbm_node
elif [ "$2" == "multiagent" ]; then
echo "Stopping MARBLE Multi-Agent nodes..."
# kill -9 $(pgrep -f 'python2.*multi_agent.py')
rosnode kill Base/multi_agent
rosnode kill X1/multi_agent
rosnode kill X2/multi_agent
rosnode kill X3/multi_agent
rosnode kill X4/multi_agent
rosnode kill B01/multi_agent
rosnode kill B02/multi_agent
rosnode kill B03/multi_agent
rosnode kill B04/multi_agent
rosnode kill B05/multi_agent
rosnode kill B06/multi_agent
rosnode kill B07/multi_agent
rosnode kill B08/multi_agent
elif [ "$2" == "commscheck" ]; then
echo "Stopping MARBLE Comms Check nodes..."
kill -9 $(pgrep -f 'python2.*comms_sim_checker.py')
else
echo "Stopping MARBLE services..."
rosnode kill -a
killall -9 gzserver
killall -9 rosmaster
kill -9 $(pgrep -f 'python.*graph.*.py')
# kill -9 $(pgrep -f 'python2.*multi_agent.py')
kill -9 $(pgrep -f 'python2.*comms_sim_checker.py')
kill -9 $(pgrep -f 'python2.*comms_sim_handler.py')
kill -9 $(pgrep -f 'python2.*world_to_bot_tf.py')
kill -9 $(pgrep -f 'python2.*subt_true_odom.py')
kill -9 $(pgrep -f 'python2.*subt_guidance_node.py')
killall -9 relay
killall -HUP gbm_node
fi
elif [ "$1" == "explore_frontier" ]; then
echo "Starting MARBLE Frontier exploration..."
rostopic pub /X1/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
rostopic pub /X1/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
rostopic pub /X1/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
roslaunch msfm3d msfm3d.launch vehicle:=X1 &
python2 ~/catkin/src/marble-frontier/FastMarching3D_ROS/src/subt_guidance_node.py "X1" "ground" "L2" 1.0 &
sleep 30
rostopic pub /X2/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
rostopic pub /X2/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
rostopic pub /X2/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
roslaunch msfm3d msfm3d.launch vehicle:=X2 &
python2 ~/catkin/src/marble-frontier/FastMarching3D_ROS/src/subt_guidance_node.py "X2" "ground" "L2" 1.0 &
sleep 30
rostopic pub /X3/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
rostopic pub /X3/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
rostopic pub /X3/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
roslaunch msfm3d msfm3d.launch vehicle:=X3 &
python2 ~/catkin/src/marble-frontier/FastMarching3D_ROS/src/subt_guidance_node.py "X3" "ground" "L2" 1.0 &
sleep 30
rostopic pub /X4/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
rostopic pub /X4/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
rostopic pub /X4/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
roslaunch msfm3d msfm3d.launch vehicle:=X4 &
python2 ~/catkin/src/marble-frontier/FastMarching3D_ROS/src/subt_guidance_node.py "X4" "ground" "L2" 1.0 &
elif [ "$1" == "explore_graph" ]; then
echo "Starting MARBLE Graph exploration..."
roslaunch graph-planner centering.launch vehicle:=X1 &
roslaunch graph-planner junction_switch.launch vehicle:=X1 &
roslaunch graph-planner junction_planner.launch vehicle:=X1 &
roslaunch graph-planner graph.launch vehicle:=X1 &
sleep 60
roslaunch graph-planner centering.launch vehicle:=X2 &
roslaunch graph-planner junction_switch.launch vehicle:=X2 &
roslaunch graph-planner junction_planner.launch vehicle:=X2 &
roslaunch graph-planner graph.launch vehicle:=X2 &
elif [ "$1" == "multiagent" ]; then
echo "Starting MARBLE Multi-Agent nodes..."
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=Base type:=base &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=X1 delayDrop:=False &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=X2 &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=X3 &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=X4 &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B01 type:=beacon &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B02 type:=beacon &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B03 type:=beacon &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B04 type:=beacon &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B05 type:=beacon &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B06 type:=beacon &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B07 type:=beacon &
roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B08 type:=beacon &
elif [ "$1" == "commscheck" ]; then
echo "Starting MARBLE Comms Check nodes..."
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py Base &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B01 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B02 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B03 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B04 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B05 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B06 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B07 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B08 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py X1 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py X2 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py X3 &
python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py X4 &
elif [ "$1" == "lights" ]; then
echo "Turning on lights..."
rosservice call /X1/left_headlight/enable "data: true"
rosservice call /X1/right_headlight/enable "data: true"
rosservice call /X2/left_headlight/enable "data: true"
rosservice call /X2/right_headlight/enable "data: true"
else
echo "Please tell me if you want to start, stop or explore!"
fi