forked from JenJenChung/pioneer_gazebo_ros
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrun_utm_demo_auro_no_agents
executable file
·160 lines (141 loc) · 4.25 KB
/
run_utm_demo_auro_no_agents
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
#!/bin/bash
my_pid=$$
echo "My process ID is $my_pid"
echo "Launching roscore..."
roscore &
pid=$!
echo "Launching Gazebo..."
sleep 5s
roslaunch pioneer_gazebo pioneer_traffic_utm_auro_world.launch gui:=false &
pid="$pid $!"
echo "Loading initialisation parameters..."
sleep 5s
roslaunch pioneer_description utm_auro_traffic_description.launch
echo "Launching Pioneers in Gazebo stack..."
n_robots=8
sleep 5s
for i in `seq 1 $n_robots`;
do
roslaunch pioneer_description generic_pioneer.launch name:=pioneer$i pose:="-x $(rosparam get /pioneer$i/x) -y $(rosparam get /pioneer$i/y) -Y $(rosparam get /pioneer$i/a)" &
pid="$pid $!"
sleep 2s
done
echo "Launching map server..."
sleep 5s
roslaunch nav_bundle map_server.launch map_name:=utm_auro_circ &
pid="$pid $!"
echo "Launching navigation stack..."
rosparam load $(rospack find utm_traffic)/launch/utm_agent_auro_params.yaml &
sleep 5s
for i in `seq 1 $n_robots`;
do
roslaunch nav_bundle single_navigation_utm_auro_traffic.launch robot_name:=pioneer$i x:="$(rosparam get /pioneer$i/x)" y:="$(rosparam get /pioneer$i/y)" yaw:="$(rosparam get /pioneer$i/a)" pioneer_traffic:="pioneer_traffic_sim_auro" waypoint_file:="waypoints_auro"&
pid="$pid $!"
sleep 2s
done
#echo "Initializing UTM graph manager..."
#sleep 5s
#rosrun utm_agents utm_manager &
#pid="$pid $!"
#echo "Launching agents..."
#n_agents=37
#rosparam load $(rospack find utm_agents)/launch/utm_auro_agents_params.yaml &
#sleep 5s
#for i in `seq 0 $n_agents`;
#do
# roslaunch utm_agents utm_agent.launch agentID:=$i num_traffic:=$n_robots&
# pid="$pid $!"
# sleep 2s
#done
echo "Initializing graph..."
sleep 5s
rosrun utm_traffic utm_graph_publisher &
echo "Launching rviz..."
sleep 5s
roslaunch pioneer_description multi_pioneer_rviz.launch rviz_config:=utm_auro_multi_pioneer&
pid="$pid $!"
if [ $n_robots -gt 0 ]
then
echo "Sending initial waypoints..."
sleep 10s
rostopic pub -1 /pioneer1/cmd_map_goal geometry_msgs/Twist '[17.0, -17.0, 0.0]' '[0.0, 0.0, 0.0]' &
fi
if [ $n_robots -gt 1 ]
then
sleep 2s
rostopic pub -1 /pioneer2/cmd_map_goal geometry_msgs/Twist '[17.0, -20.0, 0.0]' '[0.0, 0.0, 0.0]' &
fi
if [ $n_robots -gt 2 ]
then
sleep 2s
rostopic pub -1 /pioneer3/cmd_map_goal geometry_msgs/Twist '[20.0, -17.0, 0.0]' '[0.0, 0.0, 0.0]' &
fi
if [ $n_robots -gt 3 ]
then
sleep 2s
rostopic pub -1 /pioneer4/cmd_map_goal geometry_msgs/Twist '[20.0, -20.0, 0.0]' '[0.0, 0.0, 0.0]' &
fi
sleep 10s
if [ $n_robots -gt 4 ]
then
rostopic pub -1 /pioneer5/cmd_map_goal geometry_msgs/Twist '[4.0, -4.0, 0.0]' '[0.0, 0.0, 0.0]' &
fi
if [ $n_robots -gt 5 ]
then
sleep 2s
rostopic pub -1 /pioneer6/cmd_map_goal geometry_msgs/Twist '[4.0, -1.0, 0.0]' '[0.0, 0.0, 0.0]' &
fi
if [ $n_robots -gt 6 ]
then
sleep 2s
rostopic pub -1 /pioneer7/cmd_map_goal geometry_msgs/Twist '[1.0, -4.0, 0.0]' '[0.0, 0.0, 0.0]' &
fi
if [ $n_robots -gt 7 ]
then
sleep 2s
rostopic pub -1 /pioneer8/cmd_map_goal geometry_msgs/Twist '[1.0, -1.0, 0.0]' '[0.0, 0.0, 0.0]' &
fi
#if [ $n_robots -gt 0 ]
#then
# echo "Sending initial waypoints..."
# sleep 15s
# rostopic pub -1 /pioneer1/cmd_map_goal geometry_msgs/Twist '[17.0, -17.0, 0.0]' '[0.0, 0.0, 0.0]' &
#fi
#if [ $n_robots -gt 2 ]
#then
# sleep 2s
# rostopic pub -1 /pioneer3/cmd_map_goal geometry_msgs/Twist '[17.0, -20.0, 0.0]' '[0.0, 0.0, 0.0]' &
#fi
#if [ $n_robots -gt 4 ]
#then
# sleep 2s
# rostopic pub -1 /pioneer5/cmd_map_goal geometry_msgs/Twist '[20.0, -17.0, 0.0]' '[0.0, 0.0, 0.0]' &
#fi
#if [ $n_robots -gt 6 ]
#then
# sleep 2s
# rostopic pub -1 /pioneer7/cmd_map_goal geometry_msgs/Twist '[20.0, -20.0, 0.0]' '[0.0, 0.0, 0.0]' &
#fi
#sleep 15s
#if [ $n_robots -gt 1 ]
#then
# rostopic pub -1 /pioneer2/cmd_map_goal geometry_msgs/Twist '[4.0, -4.0, 0.0]' '[0.0, 0.0, 0.0]' &
#fi
#if [ $n_robots -gt 3 ]
#then
# sleep 2s
# rostopic pub -1 /pioneer4/cmd_map_goal geometry_msgs/Twist '[4.0, -1.0, 0.0]' '[0.0, 0.0, 0.0]' &
#fi
#if [ $n_robots -gt 5 ]
#then
# sleep 2s
# rostopic pub -1 /pioneer6/cmd_map_goal geometry_msgs/Twist '[1.0, -4.0, 0.0]' '[0.0, 0.0, 0.0]' &
#fi
#if [ $n_robots -gt 7 ]
#then
# sleep 2s
# rostopic pub -1 /pioneer8/cmd_map_goal geometry_msgs/Twist '[1.0, -1.0, 0.0]' '[0.0, 0.0, 0.0]' &
#fi
trap "echo Killing all processes.; kill -2 TERM $pid; exit" SIGINT SIGTERM
echo "Simulation active"
sleep 24h