├── .gitignore
├── README.md
├── __init__.py
├── circle_test.py
├── circle_world.py
├── doc
├── circle_test.gif
└── stage2.gif
├── model
├── __init__.py
├── net.py
├── ppo.py
└── utils.py
├── policy
├── stage1_1.pth
├── stage1_2.pth
└── stage2.pth
├── ppo_stage1.py
├── ppo_stage2.py
├── stage_ros-add_pose_and_crash
├── .gitignore
├── .travis.yml
├── CHANGELOG.rst
├── CMakeLists.txt
├── launch
│ ├── maptest.launch
│ └── maptest2.launch
├── package.xml
├── rviz
│ └── stage.rviz
├── scripts
│ └── upgrade-world.sh
├── src
│ └── stageros.cpp
├── test
│ ├── cmdpose_tests.py
│ ├── cmdpose_tests.xml
│ └── hztest.xml
└── world
│ ├── Obstacles.jpg
│ ├── Obstacles.world
│ ├── rink.png
│ ├── simple.world
│ ├── willow-erratic.world
│ ├── willow-four-erratics-multisensor.world
│ ├── willow-four-erratics.world
│ └── willow-full.pgm
├── stage_world1.py
├── stage_world2.py
└── worlds
├── circle.world
├── rink.png
├── stage1.world
├── stage2.world
└── testenv.png
/.gitignore:
--------------------------------------------------------------------------------
1 | *.pyc
2 | .idea
3 | model/__pycache__/*
4 | model/*.pyc
5 |
6 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # rl-collision-avoidance
2 |
3 | This is a Pytorch implementation of the paper [Towards Optimally Decentralized Multi-Robot Collision Avoidance via Deep Reinforcement Learning](https://arxiv.org/abs/1709.10082)
4 |
5 |  | 
6 | :-------------------------:|:-------------------------:
7 |
8 | ## Requirement
9 |
10 | - python2.7
11 | - [ROS Kinetic](http://wiki.ros.org/kinetic)
12 | - [mpi4py](https://mpi4py.readthedocs.io/en/stable/)
13 | - [Stage](http://rtv.github.io/Stage/)
14 | - [PyTorch](http://pytorch.org/)
15 |
16 |
17 | ## How to train
18 | You may start with training in Stage1 and when it is well-trained you can transfer to Stage2 base on the policy model of Stage1, this is exactly what Curriculum Learning means. Training Stage2 from scratch may converge at a lower performance or not even converge.
19 | Please note that the motivation of training in Stage2 is to generalize the model, which hopefully can work well in real environment.
20 |
21 | Please use the `stage_ros-add_pose_and_crash` package instead of the default package provided by ROS.
22 | ```
23 | mkdir -p catkin_ws/src
24 | cp stage_ros-add_pose_and_crash catkin_ws/src
25 | cd catkin_ws
26 | catkin_make
27 | source devel/setup.bash
28 | ```
29 |
30 | To train Stage1, modify the hyper-parameters in `ppo_stage1.py` as you like, and running the following command:
31 | ```
32 | (leave out the -g if you want to see the GUI while training)
33 | rosrun stage_ros_add_pose_and_crash stageros -g worlds/stage1.world
34 | mpiexec -np 24 python ppo_stage1.py
35 | ```
36 | To train Stage2, modify the hyper-parameters in `ppo_stage2.py` as you like, and running the following command:
37 | ```
38 | rosrun stage_ros_add_pose_and_crash stageros -g worlds/stage2.world
39 | mpiexec -np 44 python ppo_stage2.py
40 | ```
41 | ## How to test
42 |
43 | ```
44 | rosrun stage_ros_add_pose_and_crash stageros worlds/circle.world
45 | mpiexec -np 50 python circle_test.py
46 | ```
47 |
48 | ## Notice
49 | I am not the author of the paper and not in their group either. You may contact [Jia Pan](https://sites.google.com/site/panjia/) (jpan@cs.hku.hk) for the paper related issues.
50 | If you find it useful and use it in your project, please consider citing:
51 | ```
52 | @misc{Tianyu2018,
53 | author = {Tianyu Liu},
54 | title = {Robot Collision Avoidance via Deep Reinforcement Learning},
55 | year = {2018},
56 | publisher = {GitHub},
57 | journal = {GitHub repository},
58 | howpublished = {\url{https://github.com/Acmece/rl-collision-avoidance.git}},
59 | commit = {7bc682403cb9a327377481be1f110debc16babbd}
60 | }
61 | ```
62 |
--------------------------------------------------------------------------------
/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/__init__.py
--------------------------------------------------------------------------------
/circle_test.py:
--------------------------------------------------------------------------------
1 | import os
2 | import numpy as np
3 | import rospy
4 | import torch
5 | import torch.nn as nn
6 | from mpi4py import MPI
7 |
8 | from torch.optim import Adam
9 | from collections import deque
10 |
11 | from model.net import MLPPolicy, CNNPolicy
12 | from circle_world import StageWorld
13 | from model.ppo import generate_action_no_sampling, transform_buffer
14 |
15 |
16 |
17 |
18 |
19 | MAX_EPISODES = 5000
20 | LASER_BEAM = 512
21 | LASER_HIST = 3
22 | HORIZON = 200
23 | GAMMA = 0.99
24 | LAMDA = 0.95
25 | BATCH_SIZE = 512
26 | EPOCH = 3
27 | COEFF_ENTROPY = 5e-4
28 | CLIP_VALUE = 0.1
29 | NUM_ENV = 50
30 | OBS_SIZE = 512
31 | ACT_SIZE = 2
32 | LEARNING_RATE = 5e-5
33 |
34 |
35 |
36 | def enjoy(comm, env, policy, action_bound):
37 |
38 |
39 | if env.index == 0:
40 | env.reset_world()
41 |
42 | env.reset_pose()
43 |
44 | env.generate_goal_point()
45 | step = 1
46 | terminal = False
47 |
48 | obs = env.get_laser_observation()
49 | obs_stack = deque([obs, obs, obs])
50 | goal = np.asarray(env.get_local_goal())
51 | speed = np.asarray(env.get_self_speed())
52 | state = [obs_stack, goal, speed]
53 |
54 | while not rospy.is_shutdown():
55 | state_list = comm.gather(state, root=0)
56 |
57 | # generate actions at rank==0
58 | mean, scaled_action =generate_action_no_sampling(env=env, state_list=state_list,
59 | policy=policy, action_bound=action_bound)
60 |
61 |
62 | # execute actions
63 | real_action = comm.scatter(scaled_action, root=0)
64 | if terminal == True:
65 | real_action[0] = 0
66 | env.control_vel(real_action)
67 | # rate.sleep()
68 | rospy.sleep(0.001)
69 | # get informtion
70 | r, terminal, result = env.get_reward_and_terminate(step)
71 | step += 1
72 |
73 |
74 | # get next state
75 | s_next = env.get_laser_observation()
76 | left = obs_stack.popleft()
77 | obs_stack.append(s_next)
78 | goal_next = np.asarray(env.get_local_goal())
79 | speed_next = np.asarray(env.get_self_speed())
80 | state_next = [obs_stack, goal_next, speed_next]
81 |
82 |
83 | state = state_next
84 |
85 |
86 |
87 |
88 | if __name__ == '__main__':
89 |
90 | comm = MPI.COMM_WORLD
91 | rank = comm.Get_rank()
92 | size = comm.Get_size()
93 |
94 | env = StageWorld(OBS_SIZE, index=rank, num_env=NUM_ENV)
95 | reward = None
96 | action_bound = [[0, -1], [1, 1]]
97 |
98 | if rank == 0:
99 | policy_path = 'policy'
100 | # policy = MLPPolicy(obs_size, act_size)
101 | policy = CNNPolicy(frames=LASER_HIST, action_space=2)
102 | policy.cuda()
103 | opt = Adam(policy.parameters(), lr=LEARNING_RATE)
104 | mse = nn.MSELoss()
105 |
106 | if not os.path.exists(policy_path):
107 | os.makedirs(policy_path)
108 |
109 | file = policy_path + '/stage2.pth'
110 | if os.path.exists(file):
111 | print ('####################################')
112 | print ('############Loading Model###########')
113 | print ('####################################')
114 | state_dict = torch.load(file)
115 | policy.load_state_dict(state_dict)
116 | else:
117 | print ('Error: Policy File Cannot Find')
118 | exit()
119 |
120 | else:
121 | policy = None
122 | policy_path = None
123 | opt = None
124 |
125 |
126 |
127 | try:
128 | enjoy(comm=comm, env=env, policy=policy, action_bound=action_bound)
129 | except KeyboardInterrupt:
130 | import traceback
131 | traceback.print_exc()
132 |
--------------------------------------------------------------------------------
/circle_world.py:
--------------------------------------------------------------------------------
1 | import time
2 | import rospy
3 | import copy
4 | import tf
5 | import numpy as np
6 |
7 | from geometry_msgs.msg import Twist, Pose
8 | from nav_msgs.msg import Odometry
9 | from sensor_msgs.msg import LaserScan
10 | from rosgraph_msgs.msg import Clock
11 | from std_srvs.srv import Empty
12 | from std_msgs.msg import Int8
13 | from model.utils import test_init_pose, test_goal_point
14 |
15 |
16 | class StageWorld():
17 | def __init__(self, beam_num, index, num_env):
18 | self.index = index
19 | self.num_env = num_env
20 | node_name = 'StageEnv_' + str(index)
21 | rospy.init_node(node_name, anonymous=None)
22 |
23 | self.beam_mum = beam_num
24 | self.laser_cb_num = 0
25 | self.scan = None
26 |
27 | # used in reset_world
28 | self.self_speed = [0.0, 0.0]
29 | self.step_goal = [0., 0.]
30 | self.step_r_cnt = 0.
31 |
32 | # used in generate goal point
33 | self.map_size = np.array([8., 8.], dtype=np.float32) # 20x20m
34 | self.goal_size = 0.5
35 |
36 | self.robot_value = 10.
37 | self.goal_value = 0.
38 |
39 | # -----------Publisher and Subscriber-------------
40 | cmd_vel_topic = 'robot_' + str(index) + '/cmd_vel'
41 | self.cmd_vel = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
42 |
43 | cmd_pose_topic = 'robot_' + str(index) + '/cmd_pose'
44 | self.cmd_pose = rospy.Publisher(cmd_pose_topic, Pose, queue_size=10)
45 |
46 | object_state_topic = 'robot_' + str(index) + '/base_pose_ground_truth'
47 | self.object_state_sub = rospy.Subscriber(object_state_topic, Odometry, self.ground_truth_callback)
48 |
49 | laser_topic = 'robot_' + str(index) + '/base_scan'
50 |
51 | self.laser_sub = rospy.Subscriber(laser_topic, LaserScan, self.laser_scan_callback)
52 |
53 | odom_topic = 'robot_' + str(index) + '/odom'
54 | self.odom_sub = rospy.Subscriber(odom_topic, Odometry, self.odometry_callback)
55 |
56 | crash_topic = 'robot_' + str(index) + '/is_crashed'
57 | self.check_crash = rospy.Subscriber(crash_topic, Int8, self.crash_callback)
58 |
59 |
60 | self.sim_clock = rospy.Subscriber('clock', Clock, self.sim_clock_callback)
61 |
62 | # -----------Service-------------------
63 | self.reset_stage = rospy.ServiceProxy('reset_positions', Empty)
64 |
65 | # # Wait until the first callback
66 | self.speed = None
67 | self.state = None
68 | self.speed_GT = None
69 | self.state_GT = None
70 | self.is_crashed = None
71 | while self.scan is None or self.speed is None or self.state is None\
72 | or self.speed_GT is None or self.state_GT is None or self.is_crashed is None:
73 | pass
74 |
75 |
76 | rospy.sleep(1.)
77 | # # What function to call when you ctrl + c
78 | # rospy.on_shutdown(self.shutdown)
79 |
80 |
81 | def ground_truth_callback(self, GT_odometry):
82 | Quaternious = GT_odometry.pose.pose.orientation
83 | Euler = tf.transformations.euler_from_quaternion([Quaternious.x, Quaternious.y, Quaternious.z, Quaternious.w])
84 | self.state_GT = [GT_odometry.pose.pose.position.x, GT_odometry.pose.pose.position.y, Euler[2]]
85 | v_x = GT_odometry.twist.twist.linear.x
86 | v_y = GT_odometry.twist.twist.linear.y
87 | v = np.sqrt(v_x**2 + v_y**2)
88 | self.speed_GT = [v, GT_odometry.twist.twist.angular.z]
89 |
90 | def laser_scan_callback(self, scan):
91 | self.scan_param = [scan.angle_min, scan.angle_max, scan.angle_increment, scan.time_increment,
92 | scan.scan_time, scan.range_min, scan.range_max]
93 | self.scan = np.array(scan.ranges)
94 | self.laser_cb_num += 1
95 |
96 |
97 | def odometry_callback(self, odometry):
98 | Quaternions = odometry.pose.pose.orientation
99 | Euler = tf.transformations.euler_from_quaternion([Quaternions.x, Quaternions.y, Quaternions.z, Quaternions.w])
100 | self.state = [odometry.pose.pose.position.x, odometry.pose.pose.position.y, Euler[2]]
101 | self.speed = [odometry.twist.twist.linear.x, odometry.twist.twist.angular.z]
102 |
103 | def sim_clock_callback(self, clock):
104 | self.sim_time = clock.clock.secs + clock.clock.nsecs / 1000000000.
105 |
106 | def crash_callback(self, flag):
107 | self.is_crashed = flag.data
108 |
109 | def get_self_stateGT(self):
110 | return self.state_GT
111 |
112 | def get_self_speedGT(self):
113 | return self.speed_GT
114 |
115 | def get_laser_observation(self):
116 | scan = copy.deepcopy(self.scan)
117 | scan[np.isnan(scan)] = 6.0
118 | scan[np.isinf(scan)] = 6.0
119 | raw_beam_num = len(scan)
120 | sparse_beam_num = self.beam_mum
121 | step = float(raw_beam_num) / sparse_beam_num
122 | sparse_scan_left = []
123 | index = 0.
124 | for x in xrange(int(sparse_beam_num / 2)):
125 | sparse_scan_left.append(scan[int(index)])
126 | index += step
127 | sparse_scan_right = []
128 | index = raw_beam_num - 1.
129 | for x in xrange(int(sparse_beam_num / 2)):
130 | sparse_scan_right.append(scan[int(index)])
131 | index -= step
132 | scan_sparse = np.concatenate((sparse_scan_left, sparse_scan_right[::-1]), axis=0)
133 | return scan_sparse / 6.0 - 0.5
134 |
135 |
136 | def get_self_speed(self):
137 | return self.speed
138 |
139 | def get_self_state(self):
140 | return self.state
141 |
142 | def get_crash_state(self):
143 | return self.is_crashed
144 |
145 | def get_sim_time(self):
146 | return self.sim_time
147 |
148 | def get_local_goal(self):
149 | [x, y, theta] = self.get_self_stateGT()
150 | [goal_x, goal_y] = self.goal_point
151 | local_x = (goal_x - x) * np.cos(theta) + (goal_y - y) * np.sin(theta)
152 | local_y = -(goal_x - x) * np.sin(theta) + (goal_y - y) * np.cos(theta)
153 | return [local_x, local_y]
154 |
155 | def reset_world(self):
156 | self.reset_stage()
157 | self.self_speed = [0.0, 0.0]
158 | self.step_goal = [0., 0.]
159 | self.step_r_cnt = 0.
160 | self.start_time = time.time()
161 | rospy.sleep(0.5)
162 |
163 |
164 | def generate_goal_point(self):
165 | self.goal_point = test_goal_point(self.index)
166 | self.pre_distance = 0
167 | self.distance = copy.deepcopy(self.pre_distance)
168 |
169 |
170 |
171 | def get_reward_and_terminate(self, t):
172 | terminate = False
173 | laser_scan = self.get_laser_observation()
174 | [x, y, theta] = self.get_self_stateGT()
175 | [v, w] = self.get_self_speedGT()
176 | self.pre_distance = copy.deepcopy(self.distance)
177 | self.distance = np.sqrt((self.goal_point[0] - x) ** 2 + (self.goal_point[1] - y) ** 2)
178 | reward_g = (self.pre_distance - self.distance) * 2.5
179 | reward_c = 0
180 | reward_w = 0
181 | result = 0
182 |
183 | is_crash = self.get_crash_state()
184 |
185 | if self.distance < self.goal_size:
186 | terminate = True
187 | reward_g = 15
188 | result = 'Reach Goal'
189 |
190 | if is_crash == 1:
191 | terminate = True
192 | reward_c = -15.
193 | result = 'Crashed'
194 |
195 | if np.abs(w) > 0.7:
196 | reward_w = -0.1 * np.abs(w)
197 |
198 | if t > 10000:
199 | terminate = True
200 | result = 'Time out'
201 | reward = reward_g + reward_c + reward_w
202 |
203 | return reward, terminate, result
204 |
205 | def reset_pose(self):
206 |
207 | reset_pose = test_init_pose(self.index)
208 | self.control_pose(reset_pose)
209 |
210 |
211 | def control_vel(self, action):
212 | move_cmd = Twist()
213 | move_cmd.linear.x = action[0]
214 | move_cmd.linear.y = 0.
215 | move_cmd.linear.z = 0.
216 | move_cmd.angular.x = 0.
217 | move_cmd.angular.y = 0.
218 | move_cmd.angular.z = action[1]
219 | self.cmd_vel.publish(move_cmd)
220 |
221 |
222 | def control_pose(self, pose):
223 | pose_cmd = Pose()
224 | assert len(pose)==3
225 | pose_cmd.position.x = pose[0]
226 | pose_cmd.position.y = pose[1]
227 | pose_cmd.position.z = 0
228 |
229 | qtn = tf.transformations.quaternion_from_euler(0, 0, pose[2], 'rxyz')
230 | pose_cmd.orientation.x = qtn[0]
231 | pose_cmd.orientation.y = qtn[1]
232 | pose_cmd.orientation.z = qtn[2]
233 | pose_cmd.orientation.w = qtn[3]
234 | self.cmd_pose.publish(pose_cmd)
235 |
236 |
237 | def generate_random_pose(self):
238 | [x_robot, y_robot, theta] = self.get_self_stateGT()
239 | x = np.random.uniform(9, 19)
240 | y = np.random.uniform(0, 1)
241 | if y <= 0.4:
242 | y = -(y * 10 + 1)
243 | else:
244 | y = -(y * 10 + 9)
245 | dis_goal = np.sqrt((x - x_robot) ** 2 + (y - y_robot) ** 2)
246 | while (dis_goal < 7) and not rospy.is_shutdown():
247 | x = np.random.uniform(9, 19)
248 | y = np.random.uniform(0, 1)
249 | if y <= 0.4:
250 | y = -(y * 10 + 1)
251 | else:
252 | y = -(y * 10 + 9)
253 | dis_goal = np.sqrt((x - x_robot) ** 2 + (y - y_robot) ** 2)
254 | theta = np.random.uniform(0, 2*np.pi)
255 | return [x, y, theta]
256 |
257 | def generate_random_goal(self):
258 | [x_robot, y_robot, theta] = self.get_self_stateGT()
259 | x = np.random.uniform(9, 19)
260 | y = np.random.uniform(0, 1)
261 | if y <= 0.4:
262 | y = -(y*10 + 1)
263 | else:
264 | y = -(y*10 + 9)
265 | dis_goal = np.sqrt((x - x_robot) ** 2 + (y - y_robot) ** 2)
266 | while (dis_goal < 7) and not rospy.is_shutdown():
267 | x = np.random.uniform(9, 19)
268 | y = np.random.uniform(0, 1)
269 | if y <= 0.4:
270 | y = -(y * 10 + 1)
271 | else:
272 | y = -(y * 10 + 9)
273 | dis_goal = np.sqrt((x - x_robot) ** 2 + (y - y_robot) ** 2)
274 | return [x, y]
275 |
276 |
277 |
278 |
279 |
280 |
281 |
--------------------------------------------------------------------------------
/doc/circle_test.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/doc/circle_test.gif
--------------------------------------------------------------------------------
/doc/stage2.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/doc/stage2.gif
--------------------------------------------------------------------------------
/model/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/model/__init__.py
--------------------------------------------------------------------------------
/model/net.py:
--------------------------------------------------------------------------------
1 | import math
2 | import numpy as np
3 | import torch
4 | import torch.nn as nn
5 | from torch.nn import init
6 | from torch.nn import functional as F
7 |
8 | from model.utils import log_normal_density
9 |
10 | class Flatten(nn.Module):
11 | def forward(self, input):
12 |
13 | return input.view(input.shape[0], 1, -1)
14 |
15 |
16 | class CNNPolicy(nn.Module):
17 | def __init__(self, frames, action_space):
18 | super(CNNPolicy, self).__init__()
19 | self.logstd = nn.Parameter(torch.zeros(action_space))
20 |
21 | self.act_fea_cv1 = nn.Conv1d(in_channels=frames, out_channels=32, kernel_size=5, stride=2, padding=1)
22 | self.act_fea_cv2 = nn.Conv1d(in_channels=32, out_channels=32, kernel_size=3, stride=2, padding=1)
23 | self.act_fc1 = nn.Linear(128*32, 256)
24 | self.act_fc2 = nn.Linear(256+2+2, 128)
25 | self.actor1 = nn.Linear(128, 1)
26 | self.actor2 = nn.Linear(128, 1)
27 |
28 |
29 | self.crt_fea_cv1 = nn.Conv1d(in_channels=frames, out_channels=32, kernel_size=5, stride=2, padding=1)
30 | self.crt_fea_cv2 = nn.Conv1d(in_channels=32, out_channels=32, kernel_size=3, stride=2, padding=1)
31 | self.crt_fc1 = nn.Linear(128*32, 256)
32 | self.crt_fc2 = nn.Linear(256+2+2, 128)
33 | self.critic = nn.Linear(128, 1)
34 |
35 |
36 |
37 | def forward(self, x, goal, speed):
38 | """
39 | returns value estimation, action, log_action_prob
40 | """
41 | # action
42 | a = F.relu(self.act_fea_cv1(x))
43 | a = F.relu(self.act_fea_cv2(a))
44 | a = a.view(a.shape[0], -1)
45 | a = F.relu(self.act_fc1(a))
46 |
47 | a = torch.cat((a, goal, speed), dim=-1)
48 | a = F.relu(self.act_fc2(a))
49 | mean1 = F.sigmoid(self.actor1(a))
50 | mean2 = F.tanh(self.actor2(a))
51 | mean = torch.cat((mean1, mean2), dim=-1)
52 |
53 | logstd = self.logstd.expand_as(mean)
54 | std = torch.exp(logstd)
55 | action = torch.normal(mean, std)
56 |
57 | # action prob on log scale
58 | logprob = log_normal_density(action, mean, std=std, log_std=logstd)
59 |
60 | # value
61 | v = F.relu(self.crt_fea_cv1(x))
62 | v = F.relu(self.crt_fea_cv2(v))
63 | v = v.view(v.shape[0], -1)
64 | v = F.relu(self.crt_fc1(v))
65 | v = torch.cat((v, goal, speed), dim=-1)
66 | v = F.relu(self.crt_fc2(v))
67 | v = self.critic(v)
68 |
69 |
70 | return v, action, logprob, mean
71 |
72 | def evaluate_actions(self, x, goal, speed, action):
73 | v, _, _, mean = self.forward(x, goal, speed)
74 | logstd = self.logstd.expand_as(mean)
75 | std = torch.exp(logstd)
76 | # evaluate
77 | logprob = log_normal_density(action, mean, log_std=logstd, std=std)
78 | dist_entropy = 0.5 + 0.5 * math.log(2 * math.pi) + logstd
79 | dist_entropy = dist_entropy.sum(-1).mean()
80 | return v, logprob, dist_entropy
81 |
82 |
83 | class MLPPolicy(nn.Module):
84 | def __init__(self, obs_space, action_space):
85 | super(MLPPolicy, self).__init__()
86 | # action network
87 | self.act_fc1 = nn.Linear(obs_space, 64)
88 | self.act_fc2 = nn.Linear(64, 128)
89 | self.mu = nn.Linear(128, action_space)
90 | self.mu.weight.data.mul_(0.1)
91 | # torch.log(std)
92 | self.logstd = nn.Parameter(torch.zeros(action_space))
93 |
94 | # value network
95 | self.value_fc1 = nn.Linear(obs_space, 64)
96 | self.value_fc2 = nn.Linear(64, 128)
97 | self.value_fc3 = nn.Linear(128, 1)
98 | self.value_fc3.weight.data.mul(0.1)
99 |
100 | def forward(self, x):
101 | """
102 | returns value estimation, action, log_action_prob
103 | """
104 | # action
105 | act = self.act_fc1(x)
106 | act = F.tanh(act)
107 | act = self.act_fc2(act)
108 | act = F.tanh(act)
109 | mean = self.mu(act) # N, num_actions
110 | logstd = self.logstd.expand_as(mean)
111 | std = torch.exp(logstd)
112 | action = torch.normal(mean, std)
113 |
114 | # value
115 | v = self.value_fc1(x)
116 | v = F.tanh(v)
117 | v = self.value_fc2(v)
118 | v = F.tanh(v)
119 | v = self.value_fc3(v)
120 |
121 | # action prob on log scale
122 | logprob = log_normal_density(action, mean, std=std, log_std=logstd)
123 | return v, action, logprob, mean
124 |
125 | def evaluate_actions(self, x, action):
126 | v, _, _, mean = self.forward(x)
127 | logstd = self.logstd.expand_as(mean)
128 | std = torch.exp(logstd)
129 | # evaluate
130 | logprob = log_normal_density(action, mean, log_std=logstd, std=std)
131 | dist_entropy = 0.5 + 0.5 * math.log(2 * math.pi) + logstd
132 | dist_entropy = dist_entropy.sum(-1).mean()
133 | return v, logprob, dist_entropy
134 |
135 |
136 | if __name__ == '__main__':
137 | from torch.autograd import Variable
138 |
139 | net = MLPPolicy(3, 2)
140 |
141 | observation = Variable(torch.randn(2, 3))
142 | v, action, logprob, mean = net.forward(observation)
143 | print(v)
144 |
145 |
--------------------------------------------------------------------------------
/model/ppo.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import logging
3 | import os
4 | from torch.autograd import Variable
5 | from torch.nn import functional as F
6 | import numpy as np
7 | import socket
8 | from torch.utils.data.sampler import BatchSampler, SubsetRandomSampler
9 |
10 | hostname = socket.gethostname()
11 | if not os.path.exists('./log/' + hostname):
12 | os.makedirs('./log/' + hostname)
13 | ppo_file = './log/' + hostname + '/ppo.log'
14 |
15 | logger_ppo = logging.getLogger('loggerppo')
16 | logger_ppo.setLevel(logging.INFO)
17 | ppo_file_handler = logging.FileHandler(ppo_file, mode='a')
18 | ppo_file_handler.setLevel(logging.INFO)
19 | logger_ppo.addHandler(ppo_file_handler)
20 |
21 |
22 | def transform_buffer(buff):
23 | s_batch, goal_batch, speed_batch, a_batch, r_batch, d_batch, l_batch, \
24 | v_batch = [], [], [], [], [], [], [], []
25 | s_temp, goal_temp, speed_temp = [], [], []
26 |
27 | for e in buff:
28 | for state in e[0]:
29 | s_temp.append(state[0])
30 | goal_temp.append(state[1])
31 | speed_temp.append(state[2])
32 | s_batch.append(s_temp)
33 | goal_batch.append(goal_temp)
34 | speed_batch.append(speed_temp)
35 | s_temp = []
36 | goal_temp = []
37 | speed_temp = []
38 |
39 | a_batch.append(e[1])
40 | r_batch.append(e[2])
41 | d_batch.append(e[3])
42 | l_batch.append(e[4])
43 | v_batch.append(e[5])
44 |
45 | s_batch = np.asarray(s_batch)
46 | goal_batch = np.asarray(goal_batch)
47 | speed_batch = np.asarray(speed_batch)
48 | a_batch = np.asarray(a_batch)
49 | r_batch = np.asarray(r_batch)
50 | d_batch = np.asarray(d_batch)
51 | l_batch = np.asarray(l_batch)
52 | v_batch = np.asarray(v_batch)
53 |
54 | return s_batch, goal_batch, speed_batch, a_batch, r_batch, d_batch, l_batch, v_batch
55 |
56 |
57 | def generate_action(env, state_list, policy, action_bound):
58 | if env.index == 0:
59 | s_list, goal_list, speed_list = [], [], []
60 | for i in state_list:
61 | s_list.append(i[0])
62 | goal_list.append(i[1])
63 | speed_list.append(i[2])
64 |
65 | s_list = np.asarray(s_list)
66 | goal_list = np.asarray(goal_list)
67 | speed_list = np.asarray(speed_list)
68 |
69 | s_list = Variable(torch.from_numpy(s_list)).float().cuda()
70 | goal_list = Variable(torch.from_numpy(goal_list)).float().cuda()
71 | speed_list = Variable(torch.from_numpy(speed_list)).float().cuda()
72 |
73 | v, a, logprob, mean = policy(s_list, goal_list, speed_list)
74 | v, a, logprob = v.data.cpu().numpy(), a.data.cpu().numpy(), logprob.data.cpu().numpy()
75 | scaled_action = np.clip(a, a_min=action_bound[0], a_max=action_bound[1])
76 | else:
77 | v = None
78 | a = None
79 | scaled_action = None
80 | logprob = None
81 |
82 | return v, a, logprob, scaled_action
83 |
84 | def generate_action_no_sampling(env, state_list, policy, action_bound):
85 | if env.index == 0:
86 | s_list, goal_list, speed_list = [], [], []
87 | for i in state_list:
88 | s_list.append(i[0])
89 | goal_list.append(i[1])
90 | speed_list.append(i[2])
91 |
92 | s_list = np.asarray(s_list)
93 | goal_list = np.asarray(goal_list)
94 | speed_list = np.asarray(speed_list)
95 |
96 | s_list = Variable(torch.from_numpy(s_list)).float().cuda()
97 | goal_list = Variable(torch.from_numpy(goal_list)).float().cuda()
98 | speed_list = Variable(torch.from_numpy(speed_list)).float().cuda()
99 |
100 | _, _, _, mean = policy(s_list, goal_list, speed_list)
101 | mean = mean.data.cpu().numpy()
102 | scaled_action = np.clip(mean, a_min=action_bound[0], a_max=action_bound[1])
103 | else:
104 | mean = None
105 | scaled_action = None
106 |
107 | return mean, scaled_action
108 |
109 |
110 |
111 | def calculate_returns(rewards, dones, last_value, values, gamma=0.99):
112 | num_step = rewards.shape[0]
113 | num_env = rewards.shape[1]
114 | returns = np.zeros((num_step + 1, num_env))
115 | returns[-1] = last_value
116 | dones = 1 - dones
117 | for i in reversed(range(num_step)):
118 | returns[i] = gamma * returns[i+1] * dones[i] + rewards[i]
119 | return returns
120 |
121 |
122 | def generate_train_data(rewards, gamma, values, last_value, dones, lam):
123 | num_step = rewards.shape[0]
124 | num_env = rewards.shape[1]
125 | values = list(values)
126 | values.append(last_value)
127 | values = np.asarray(values).reshape((num_step+1,num_env))
128 |
129 | targets = np.zeros((num_step, num_env))
130 | gae = np.zeros((num_env,))
131 |
132 | for t in range(num_step - 1, -1, -1):
133 | delta = rewards[t, :] + gamma * values[t + 1, :] * (1 - dones[t, :]) - values[t, :]
134 | gae = delta + gamma * lam * (1 - dones[t, :]) * gae
135 |
136 | targets[t, :] = gae + values[t, :]
137 |
138 | advs = targets - values[:-1, :]
139 | return targets, advs
140 |
141 |
142 |
143 | def ppo_update_stage1(policy, optimizer, batch_size, memory, epoch,
144 | coeff_entropy=0.02, clip_value=0.2,
145 | num_step=2048, num_env=12, frames=1, obs_size=24, act_size=4):
146 | obss, goals, speeds, actions, logprobs, targets, values, rewards, advs = memory
147 |
148 | advs = (advs - advs.mean()) / advs.std()
149 |
150 | obss = obss.reshape((num_step*num_env, frames, obs_size))
151 | goals = goals.reshape((num_step*num_env, 2))
152 | speeds = speeds.reshape((num_step*num_env, 2))
153 | actions = actions.reshape(num_step*num_env, act_size)
154 | logprobs = logprobs.reshape(num_step*num_env, 1)
155 | advs = advs.reshape(num_step*num_env, 1)
156 | targets = targets.reshape(num_step*num_env, 1)
157 |
158 | for update in range(epoch):
159 | sampler = BatchSampler(SubsetRandomSampler(list(range(advs.shape[0]))), batch_size=batch_size,
160 | drop_last=False)
161 | for i, index in enumerate(sampler):
162 | sampled_obs = Variable(torch.from_numpy(obss[index])).float().cuda()
163 | sampled_goals = Variable(torch.from_numpy(goals[index])).float().cuda()
164 | sampled_speeds = Variable(torch.from_numpy(speeds[index])).float().cuda()
165 |
166 | sampled_actions = Variable(torch.from_numpy(actions[index])).float().cuda()
167 | sampled_logprobs = Variable(torch.from_numpy(logprobs[index])).float().cuda()
168 | sampled_targets = Variable(torch.from_numpy(targets[index])).float().cuda()
169 | sampled_advs = Variable(torch.from_numpy(advs[index])).float().cuda()
170 |
171 |
172 | new_value, new_logprob, dist_entropy = policy.evaluate_actions(sampled_obs, sampled_goals, sampled_speeds, sampled_actions)
173 |
174 | sampled_logprobs = sampled_logprobs.view(-1, 1)
175 | ratio = torch.exp(new_logprob - sampled_logprobs)
176 |
177 | sampled_advs = sampled_advs.view(-1, 1)
178 | surrogate1 = ratio * sampled_advs
179 | surrogate2 = torch.clamp(ratio, 1 - clip_value, 1 + clip_value) * sampled_advs
180 | policy_loss = -torch.min(surrogate1, surrogate2).mean()
181 |
182 | sampled_targets = sampled_targets.view(-1, 1)
183 | value_loss = F.mse_loss(new_value, sampled_targets)
184 |
185 | loss = policy_loss + 20 * value_loss - coeff_entropy * dist_entropy
186 | optimizer.zero_grad()
187 | loss.backward()
188 | optimizer.step()
189 | info_p_loss, info_v_loss, info_entropy = float(policy_loss.detach().cpu().numpy()), \
190 | float(value_loss.detach().cpu().numpy()), float(
191 | dist_entropy.detach().cpu().numpy())
192 | logger_ppo.info('{}, {}, {}'.format(info_p_loss, info_v_loss, info_entropy))
193 |
194 | print('update')
195 |
196 |
197 | def ppo_update_stage2(policy, optimizer, batch_size, memory, filter_index, epoch,
198 | coeff_entropy=0.02, clip_value=0.2,
199 | num_step=2048, num_env=12, frames=1, obs_size=24, act_size=4):
200 | obss, goals, speeds, actions, logprobs, targets, values, rewards, advs = memory
201 |
202 | advs = (advs - advs.mean()) / advs.std()
203 |
204 | obss = obss.reshape((num_step*num_env, frames, obs_size))
205 | goals = goals.reshape((num_step*num_env, 2))
206 | speeds = speeds.reshape((num_step*num_env, 2))
207 | actions = actions.reshape(num_step*num_env, act_size)
208 | logprobs = logprobs.reshape(num_step*num_env, 1)
209 | advs = advs.reshape(num_step*num_env, 1)
210 | targets = targets.reshape(num_step*num_env, 1)
211 |
212 | obss = np.delete(obss, filter_index, 0)
213 | goals = np.delete(goals, filter_index, 0)
214 | speeds = np.delete(speeds, filter_index, 0)
215 | actions = np.delete(actions, filter_index, 0)
216 | logprobs = np.delete(logprobs, filter_index, 0)
217 | advs = np.delete(advs, filter_index, 0)
218 | targets = np.delete(targets, filter_index, 0)
219 |
220 |
221 | for update in range(epoch):
222 | sampler = BatchSampler(SubsetRandomSampler(list(range(advs.shape[0]))), batch_size=batch_size,
223 | drop_last=True)
224 | for i, index in enumerate(sampler):
225 | sampled_obs = Variable(torch.from_numpy(obss[index])).float().cuda()
226 | sampled_goals = Variable(torch.from_numpy(goals[index])).float().cuda()
227 | sampled_speeds = Variable(torch.from_numpy(speeds[index])).float().cuda()
228 |
229 | sampled_actions = Variable(torch.from_numpy(actions[index])).float().cuda()
230 | sampled_logprobs = Variable(torch.from_numpy(logprobs[index])).float().cuda()
231 | sampled_targets = Variable(torch.from_numpy(targets[index])).float().cuda()
232 | sampled_advs = Variable(torch.from_numpy(advs[index])).float().cuda()
233 |
234 |
235 | new_value, new_logprob, dist_entropy = policy.evaluate_actions(sampled_obs, sampled_goals, sampled_speeds, sampled_actions)
236 |
237 | sampled_logprobs = sampled_logprobs.view(-1, 1)
238 | ratio = torch.exp(new_logprob - sampled_logprobs)
239 |
240 | sampled_advs = sampled_advs.view(-1, 1)
241 | surrogate1 = ratio * sampled_advs
242 | surrogate2 = torch.clamp(ratio, 1 - clip_value, 1 + clip_value) * sampled_advs
243 | policy_loss = -torch.min(surrogate1, surrogate2).mean()
244 |
245 | sampled_targets = sampled_targets.view(-1, 1)
246 | value_loss = F.mse_loss(new_value, sampled_targets)
247 |
248 | loss = policy_loss + 20 * value_loss - coeff_entropy * dist_entropy
249 | optimizer.zero_grad()
250 | loss.backward()
251 | optimizer.step()
252 | info_p_loss, info_v_loss, info_entropy = float(policy_loss.detach().cpu().numpy()), \
253 | float(value_loss.detach().cpu().numpy()), float(
254 | dist_entropy.detach().cpu().numpy())
255 | logger_ppo.info('{}, {}, {}'.format(info_p_loss, info_v_loss, info_entropy))
256 |
257 |
258 |
259 | print('filter {} transitions; update'.format(len(filter_index)))
260 |
261 |
262 |
263 |
--------------------------------------------------------------------------------
/model/utils.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | import bisect
3 | import torch
4 | from torch.autograd import Variable
5 |
6 | def test_init_pose(index):
7 | init_pose_list = [[25.00, 0.00, np.pi], [24.80, 3.13, np.pi*26/25], [24.21, 6.22, np.pi*27/25], [23.24, 9.20, np.pi*28/25],
8 | [21.91, 12.04, np.pi*29/25], [20.23, 14.69, np.pi*30/25], [18.22, 17.11, np.pi*31/25], [15.94, 19.26, np.pi*32/25],
9 | [13.40, 21.11, np.pi*33/25], [10.64, 22.62, np.pi*34/25],
10 | [7.73, 23.78, np.pi*35/25], [4.68, 24.56, np.pi*36/25], [1.57, 24.95, np.pi*37/25], [-1.57, 24.95, np.pi*38/25],
11 | [-4.68, 24.56, np.pi*39/25], [-7.73, 23.78, np.pi*40/25],
12 | [-10.64, 22.62, np.pi*41/25], [-13.40, 21.11, np.pi*42/25], [-15.94, 19.26, np.pi*43/25], [-18.22, 17.11, np.pi*44/25],
13 | [-20.23, 14.69, np.pi*45/25], [-21.91, 12.04, np.pi*46/25],
14 | [-23.24, 9.20, np.pi*47/25], [-24.21, 6.22, np.pi*48/25], [-24.80, 3.13, np.pi*49/25],[-25.00, -0.00, np.pi*50/25],
15 | [-24.80, -3.13, np.pi*51/25], [-24.21, -6.22, np.pi*52/25], [-23.24, -9.20, np.pi*53/25],
16 | [-21.91, -12.04, np.pi*54/25], [-20.23, -14.69, np.pi*55/25],
17 | [-18.22, -17.11, np.pi*56/25], [-15.94, -19.26, np.pi*57/25], [-13.40, -21.11, np.pi*58/25],
18 | [-10.64, -22.62, np.pi*59/25], [-7.73, -23.78, np.pi*60/25],
19 | [-4.68, -24.56, np.pi*61/25], [-1.57, -24.95, np.pi*62/25], [1.57, -24.95, np.pi*63/25],
20 | [4.68, -24.56, np.pi*64/25], [7.73, -23.78, np.pi*65/25], [10.64, -22.62, np.pi*66/25],
21 | [13.40, -21.11, np.pi*67/25], [15.94, -19.26, np.pi*68/25], [18.22, -17.11, np.pi*69/25],
22 | [20.23, -14.69, np.pi*70/25], [21.91, -12.04, np.pi*71/25], [23.24, -9.20, np.pi*72/25],
23 | [24.21, -6.22, np.pi*73/25], [24.80, -3.13, np.pi*74/25]
24 | ]
25 | return init_pose_list[index]
26 |
27 | def test_goal_point(index):
28 | goal_list = [[-25.00, -0.00], [-24.80, -3.13], [-24.21, -6.22], [-23.24, -9.20], [-21.91, -12.04], [-20.23, -14.69],
29 | [-18.22, -17.11], [-15.94, -19.26], [-13.40, -21.11], [-10.64, -22.62], [-7.73, -23.78],
30 | [-4.68, -24.56], [-1.57, -24.95], [1.57, -24.95], [4.68, -24.56], [7.73, -23.78], [10.64, -22.62],
31 | [13.40, -21.11], [15.94, -19.26], [18.22, -17.11], [20.23, -14.69], [21.91, -12.04], [23.24, -9.20],
32 | [24.21, -6.22], [24.80, -3.13], [25.00, 0.00], [24.80, 3.13], [24.21, 6.22], [23.24, 9.20],
33 | [21.91, 12.04], [20.23, 14.69], [18.22, 17.11], [15.94, 19.26], [13.40, 21.11], [10.64, 22.62],
34 | [7.73, 23.78], [4.68, 24.56], [1.57, 24.95], [-1.57, 24.95], [-4.68, 24.56], [-7.73, 23.78],
35 | [-10.64, 22.62], [-13.40, 21.11], [-15.94, 19.26], [-18.22, 17.11], [-20.23, 14.69], [-21.91, 12.04],
36 | [-23.24, 9.20], [-24.21, 6.22], [-24.80, 3.13]
37 | ]
38 | return goal_list[index]
39 |
40 |
41 | def get_init_pose(index):
42 | init_pose_list = [[-7.00, 11.50, np.pi], [-7.00, 9.50, np.pi], [-18.00, 11.50, 0.00], [-18.00, 9.50, 0.00],
43 | [-12.50, 17.00, np.pi*3/2], [-12.50, 4.00, np.pi/2], [-2.00, 16.00, -np.pi/2], [0.00, 16.00, -np.pi/2],
44 | [3.00, 16.00, -np.pi/2], [5.00, 16.00, -np.pi/2], [10.00, 4.00, np.pi/2], [12.00, 4.00, np.pi/2],
45 | [14.00, 4.00, np.pi/2], [16.00, 4.00, np.pi/2], [18.00, 4.00, np.pi/2], [-2.5, -2.5, 0.00],
46 | [-0.5, -2.5, 0.00], [3.5, -2.5, np.pi], [5.5, -2.5, np.pi], [-2.5, -18.5, np.pi/2],
47 | [-0.5, -18.5, np.pi/2], [1.5, -18.5, np.pi/2], [3.5, -18.5, np.pi/2], [5.5, -18.5, np.pi/2],
48 | [-6.00, -10.00, np.pi], [-7.15, -6.47, np.pi*6/5], [-10.15, -4.29, np.pi*7/5], [-13.85, -4.29, np.pi*8/5],
49 | [-16.85, -6.47, np.pi*9/5], [-18.00, -10.00, np.pi*2], [-16.85, -13.53, np.pi*11/5], [-13.85, -15.71, np.pi*12/5],
50 | [-10.15, -15.71, np.pi*13/5], [-7.15, -13.53, np.pi*14/5], [10.00, -17.00, np.pi/2], [12.00, -17.00, np.pi/2],
51 | [14.00, -17.00, np.pi/2], [16.00, -17.00, np.pi/2], [18.00, -17.00, np.pi/2], [10.00, -2.00, -np.pi/2],
52 | [12.00, -2.00, -np.pi/2], [14.00, -2.00, -np.pi/2], [16.00, -2.00, -np.pi/2], [18.00, -2.00, -np.pi/2]]
53 | return init_pose_list[index]
54 |
55 | def get_goal_point(index):
56 | goal_list = [[-18.0, 11.5], [-18.0, 9.5], [-7.0, 11.5], [-7.0, 9.5], [-12.5, 4.0], [-12.5, 17.0],
57 | [-2.0, 3.0], [0.0, 3.0], [3.0, 3.0], [5.0, 3.0], [10.0, 10.0], [12.0, 10.0],
58 | [14.0, 10.0], [16.0, 10.0], [18.0, 10.0], [3.5, -2.5], [5.5, -2.5], [-2.5, -2.5],
59 | [-0.5, -2.5], [-2.5, -5.5], [-0.5, -5.5], [1.5, -5.5], [3.5, -5.5], [5.5, -5.5],
60 | [-18.0, -10.0], [-16.85, -13.53], [-13.85, -15.71], [-10.15, -15.71], [-7.15, -13.53], [-6.00, -10.00],
61 | [-7.15, -6.47], [-10.15, -4.29], [-13.85, -4.29], [-16.85, -6.47],
62 | ]
63 | return goal_list[index]
64 |
65 | def get_filter_index(d_list):
66 | filter_index = []
67 | filter_flag = 0
68 | step = d_list.shape[0]
69 | num_env = d_list.shape[1]
70 | for i in range(num_env):
71 | for j in range(step):
72 | if d_list[j, i] == True:
73 | filter_flag += 1
74 | else:
75 | filter_flag = 0
76 | if filter_flag >= 2:
77 | filter_index.append(num_env*j + i)
78 | return filter_index
79 |
80 |
81 | def get_group_terminal(terminal_list, index):
82 | group_terminal = False
83 | refer = [0, 6, 10, 15, 19, 24, 34, 44]
84 | r = bisect.bisect(refer, index)
85 | if reduce(lambda x, y: x * y, terminal_list[refer[r-1]:refer[r]]) == 1:
86 | group_terminal = True
87 | return group_terminal
88 |
89 |
90 | def log_normal_density(x, mean, log_std, std):
91 | """returns guassian density given x on log scale"""
92 |
93 | variance = std.pow(2)
94 | log_density = -(x - mean).pow(2) / (2 * variance) - 0.5 *\
95 | np.log(2 * np.pi) - log_std # num_env * frames * act_size
96 | log_density = log_density.sum(dim=-1, keepdim=True) # num_env * frames * 1
97 | return log_density
98 |
99 |
100 |
101 | class RunningMeanStd(object):
102 | # https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#algorithm_Parallel
103 | def __init__(self, epsilon=1e-4, shape=()): # shape (1, 1, 84, 84)
104 | self.mean = np.zeros(shape, 'float64')
105 | self.var = np.ones(shape, 'float64')
106 | self.count = epsilon
107 |
108 | def update(self, x):
109 | batch_mean = np.mean(x, axis=0)
110 | batch_var = np.var(x, axis=0)
111 | batch_count = x.shape[0]
112 | self.update_from_moments(batch_mean, batch_var, batch_count)
113 |
114 | def update_from_moments(self, batch_mean, batch_var, batch_count):
115 | delta = batch_mean - self.mean
116 | tot_count = self.count + batch_count
117 |
118 | new_mean = self.mean + delta * batch_count / tot_count
119 | m_a = self.var * (self.count)
120 | m_b = batch_var * (batch_count)
121 | M2 = m_a + m_b + np.square(delta) * self.count * batch_count / (self.count + batch_count)
122 | new_var = M2 / (self.count + batch_count)
123 |
124 | new_count = batch_count + self.count
125 |
126 | self.mean = new_mean
127 | self.var = new_var
128 | self.count = new_count
129 |
--------------------------------------------------------------------------------
/policy/stage1_1.pth:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/policy/stage1_1.pth
--------------------------------------------------------------------------------
/policy/stage1_2.pth:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/policy/stage1_2.pth
--------------------------------------------------------------------------------
/policy/stage2.pth:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/policy/stage2.pth
--------------------------------------------------------------------------------
/ppo_stage1.py:
--------------------------------------------------------------------------------
1 | import os
2 | import logging
3 | import sys
4 | import socket
5 | import numpy as np
6 | import rospy
7 | import torch
8 | import torch.nn as nn
9 | from mpi4py import MPI
10 |
11 | from torch.optim import Adam
12 | from collections import deque
13 |
14 | from model.net import MLPPolicy, CNNPolicy
15 | from stage_world1 import StageWorld
16 | from model.ppo import ppo_update_stage1, generate_train_data
17 | from model.ppo import generate_action
18 | from model.ppo import transform_buffer
19 |
20 |
21 |
22 | MAX_EPISODES = 5000
23 | LASER_BEAM = 512
24 | LASER_HIST = 3
25 | HORIZON = 128
26 | GAMMA = 0.99
27 | LAMDA = 0.95
28 | BATCH_SIZE = 1024
29 | EPOCH = 2
30 | COEFF_ENTROPY = 5e-4
31 | CLIP_VALUE = 0.1
32 | NUM_ENV = 24
33 | OBS_SIZE = 512
34 | ACT_SIZE = 2
35 | LEARNING_RATE = 5e-5
36 |
37 |
38 |
39 | def run(comm, env, policy, policy_path, action_bound, optimizer):
40 |
41 | # rate = rospy.Rate(5)
42 | buff = []
43 | global_update = 0
44 | global_step = 0
45 |
46 |
47 | if env.index == 0:
48 | env.reset_world()
49 |
50 |
51 | for id in range(MAX_EPISODES):
52 | env.reset_pose()
53 |
54 | env.generate_goal_point()
55 | terminal = False
56 | ep_reward = 0
57 | step = 1
58 |
59 | obs = env.get_laser_observation()
60 | obs_stack = deque([obs, obs, obs])
61 | goal = np.asarray(env.get_local_goal())
62 | speed = np.asarray(env.get_self_speed())
63 | state = [obs_stack, goal, speed]
64 |
65 | while not terminal and not rospy.is_shutdown():
66 | state_list = comm.gather(state, root=0)
67 |
68 |
69 | # generate actions at rank==0
70 | v, a, logprob, scaled_action=generate_action(env=env, state_list=state_list,
71 | policy=policy, action_bound=action_bound)
72 |
73 | # execute actions
74 | real_action = comm.scatter(scaled_action, root=0)
75 | env.control_vel(real_action)
76 |
77 | # rate.sleep()
78 | rospy.sleep(0.001)
79 |
80 | # get informtion
81 | r, terminal, result = env.get_reward_and_terminate(step)
82 | ep_reward += r
83 | global_step += 1
84 |
85 |
86 | # get next state
87 | s_next = env.get_laser_observation()
88 | left = obs_stack.popleft()
89 | obs_stack.append(s_next)
90 | goal_next = np.asarray(env.get_local_goal())
91 | speed_next = np.asarray(env.get_self_speed())
92 | state_next = [obs_stack, goal_next, speed_next]
93 |
94 | if global_step % HORIZON == 0:
95 | state_next_list = comm.gather(state_next, root=0)
96 | last_v, _, _, _ = generate_action(env=env, state_list=state_next_list, policy=policy,
97 | action_bound=action_bound)
98 | # add transitons in buff and update policy
99 | r_list = comm.gather(r, root=0)
100 | terminal_list = comm.gather(terminal, root=0)
101 |
102 | if env.index == 0:
103 | buff.append((state_list, a, r_list, terminal_list, logprob, v))
104 | if len(buff) > HORIZON - 1:
105 | s_batch, goal_batch, speed_batch, a_batch, r_batch, d_batch, l_batch, v_batch = \
106 | transform_buffer(buff=buff)
107 | t_batch, advs_batch = generate_train_data(rewards=r_batch, gamma=GAMMA, values=v_batch,
108 | last_value=last_v, dones=d_batch, lam=LAMDA)
109 | memory = (s_batch, goal_batch, speed_batch, a_batch, l_batch, t_batch, v_batch, r_batch, advs_batch)
110 | ppo_update_stage1(policy=policy, optimizer=optimizer, batch_size=BATCH_SIZE, memory=memory,
111 | epoch=EPOCH, coeff_entropy=COEFF_ENTROPY, clip_value=CLIP_VALUE, num_step=HORIZON,
112 | num_env=NUM_ENV, frames=LASER_HIST,
113 | obs_size=OBS_SIZE, act_size=ACT_SIZE)
114 |
115 | buff = []
116 | global_update += 1
117 |
118 | step += 1
119 | state = state_next
120 |
121 |
122 | if env.index == 0:
123 | if global_update != 0 and global_update % 20 == 0:
124 | torch.save(policy.state_dict(), policy_path + '/Stage1_{}'.format(global_update))
125 | logger.info('########################## model saved when update {} times#########'
126 | '################'.format(global_update))
127 | distance = np.sqrt((env.goal_point[0] - env.init_pose[0])**2 + (env.goal_point[1]-env.init_pose[1])**2)
128 |
129 | logger.info('Env %02d, Goal (%05.1f, %05.1f), Episode %05d, setp %03d, Reward %-5.1f, Distance %05.1f, %s' % \
130 | (env.index, env.goal_point[0], env.goal_point[1], id + 1, step, ep_reward, distance, result))
131 | logger_cal.info(ep_reward)
132 |
133 |
134 |
135 |
136 |
137 | if __name__ == '__main__':
138 |
139 | # config log
140 | hostname = socket.gethostname()
141 | if not os.path.exists('./log/' + hostname):
142 | os.makedirs('./log/' + hostname)
143 | output_file = './log/' + hostname + '/output.log'
144 | cal_file = './log/' + hostname + '/cal.log'
145 |
146 | # config log
147 | logger = logging.getLogger('mylogger')
148 | logger.setLevel(logging.INFO)
149 |
150 | file_handler = logging.FileHandler(output_file, mode='a')
151 | file_handler.setLevel(logging.INFO)
152 | file_handler.setFormatter(logging.Formatter("%(asctime)s - %(levelname)s - %(message)s"))
153 | stdout_handler = logging.StreamHandler(sys.stdout)
154 | stdout_handler.setLevel(logging.INFO)
155 | logger.addHandler(file_handler)
156 | logger.addHandler(stdout_handler)
157 |
158 | logger_cal = logging.getLogger('loggercal')
159 | logger_cal.setLevel(logging.INFO)
160 | cal_f_handler = logging.FileHandler(cal_file, mode='a')
161 | file_handler.setLevel(logging.INFO)
162 | logger_cal.addHandler(cal_f_handler)
163 |
164 | comm = MPI.COMM_WORLD
165 | rank = comm.Get_rank()
166 | size = comm.Get_size()
167 |
168 | env = StageWorld(512, index=rank, num_env=NUM_ENV)
169 | reward = None
170 | action_bound = [[0, -1], [1, 1]]
171 |
172 | # torch.manual_seed(1)
173 | # np.random.seed(1)
174 | if rank == 0:
175 | policy_path = 'policy'
176 | # policy = MLPPolicy(obs_size, act_size)
177 | policy = CNNPolicy(frames=LASER_HIST, action_space=2)
178 | policy.cuda()
179 | opt = Adam(policy.parameters(), lr=LEARNING_RATE)
180 | mse = nn.MSELoss()
181 |
182 | if not os.path.exists(policy_path):
183 | os.makedirs(policy_path)
184 |
185 | file = policy_path + '/stage1_2.pth'
186 | if os.path.exists(file):
187 | logger.info('####################################')
188 | logger.info('############Loading Model###########')
189 | logger.info('####################################')
190 | state_dict = torch.load(file)
191 | policy.load_state_dict(state_dict)
192 | else:
193 | logger.info('#####################################')
194 | logger.info('############Start Training###########')
195 | logger.info('#####################################')
196 | else:
197 | policy = None
198 | policy_path = None
199 | opt = None
200 |
201 | try:
202 | run(comm=comm, env=env, policy=policy, policy_path=policy_path, action_bound=action_bound, optimizer=opt)
203 | except KeyboardInterrupt:
204 | pass
--------------------------------------------------------------------------------
/ppo_stage2.py:
--------------------------------------------------------------------------------
1 | import os
2 | import logging
3 | import sys
4 | import numpy as np
5 | import rospy
6 | import torch
7 | import socket
8 | import torch.nn as nn
9 | from mpi4py import MPI
10 |
11 | from torch.optim import Adam
12 | from torch.autograd import Variable
13 | from collections import deque
14 |
15 | from model.net import MLPPolicy, CNNPolicy
16 | from stage_world2 import StageWorld
17 | from model.ppo import ppo_update_stage2, generate_train_data
18 | from model.ppo import generate_action, transform_buffer
19 | from model.utils import get_group_terminal, get_filter_index
20 |
21 |
22 | MAX_EPISODES = 5000
23 | LASER_BEAM = 512
24 | LASER_HIST = 3
25 | HORIZON = 128
26 | GAMMA = 0.99
27 | LAMDA = 0.95
28 | BATCH_SIZE = 512
29 | EPOCH = 4
30 | COEFF_ENTROPY = 5e-4
31 | CLIP_VALUE = 0.1
32 | NUM_ENV = 44
33 | OBS_SIZE = 512
34 | ACT_SIZE = 2
35 | LEARNING_RATE = 5e-5
36 |
37 |
38 |
39 | def run(comm, env, policy, policy_path, action_bound, optimizer):
40 | rate = rospy.Rate(40)
41 | buff = []
42 | global_update = 0
43 | global_step = 0
44 |
45 | if env.index == 0:
46 | env.reset_world()
47 |
48 |
49 | for id in range(MAX_EPISODES):
50 | env.reset_pose()
51 |
52 | env.generate_goal_point()
53 | group_terminal = False
54 | ep_reward = 0
55 | liveflag = True
56 | step = 1
57 |
58 | obs = env.get_laser_observation()
59 | obs_stack = deque([obs, obs, obs])
60 | goal = np.asarray(env.get_local_goal())
61 | speed = np.asarray(env.get_self_speed())
62 | state = [obs_stack, goal, speed]
63 |
64 | while not group_terminal and not rospy.is_shutdown():
65 | state_list = comm.gather(state, root=0)
66 |
67 | # generate actions at rank==0
68 | v, a, logprob, scaled_action=generate_action(env=env, state_list=state_list,
69 | policy=policy, action_bound=action_bound)
70 | # execute actions
71 | real_action = comm.scatter(scaled_action, root=0)
72 | if liveflag == True:
73 | env.control_vel(real_action)
74 | # rate.sleep()
75 | rospy.sleep(0.001)
76 | # get informtion
77 | r, terminal, result = env.get_reward_and_terminate(step)
78 | step += 1
79 |
80 |
81 | if liveflag == True:
82 | ep_reward += r
83 | if terminal == True:
84 | liveflag = False
85 |
86 | global_step += 1
87 |
88 | # get next state
89 | s_next = env.get_laser_observation()
90 | left = obs_stack.popleft()
91 | obs_stack.append(s_next)
92 | goal_next = np.asarray(env.get_local_goal())
93 | speed_next = np.asarray(env.get_self_speed())
94 | state_next = [obs_stack, goal_next, speed_next]
95 |
96 |
97 | if global_step % HORIZON == 0:
98 | state_next_list = comm.gather(state_next, root=0)
99 | last_v, _, _, _ = generate_action(env=env, state_list=state_next_list, policy=policy,
100 | action_bound=action_bound)
101 | # add transitons in buff and update policy
102 | r_list = comm.gather(r, root=0)
103 | terminal_list = comm.gather(terminal, root=0)
104 |
105 | terminal_list = comm.bcast(terminal_list, root=0)
106 | group_terminal = get_group_terminal(terminal_list, env.index)
107 | if env.index == 0:
108 | buff.append((state_list, a, r_list, terminal_list, logprob, v))
109 | if len(buff) > HORIZON - 1:
110 | s_batch, goal_batch, speed_batch, a_batch, r_batch, d_batch, l_batch, v_batch = \
111 | transform_buffer(buff=buff)
112 | filter_index = get_filter_index(d_batch)
113 | # print len(filter_index)
114 | t_batch, advs_batch = generate_train_data(rewards=r_batch, gamma=GAMMA, values=v_batch,
115 | last_value=last_v, dones=d_batch, lam=LAMDA)
116 | memory = (s_batch, goal_batch, speed_batch, a_batch, l_batch, t_batch, v_batch, r_batch, advs_batch)
117 | ppo_update_stage2(policy=policy, optimizer=optimizer, batch_size=BATCH_SIZE, memory=memory, filter_index=filter_index,
118 | epoch=EPOCH, coeff_entropy=COEFF_ENTROPY, clip_value=CLIP_VALUE, num_step=HORIZON,
119 | num_env=NUM_ENV, frames=LASER_HIST,
120 | obs_size=OBS_SIZE, act_size=ACT_SIZE)
121 |
122 | buff = []
123 | global_update += 1
124 |
125 |
126 | state = state_next
127 |
128 |
129 |
130 | if env.index == 0:
131 | if global_update != 0 and global_update % 20 == 0:
132 | torch.save(policy.state_dict(), policy_path + '/stage2_{}.pth'.format(global_update))
133 | logger.info('########################## model saved when update {} times#########'
134 | '################'.format(global_update))
135 |
136 | logger.info('Env %02d, Goal (%05.1f, %05.1f), Episode %05d, setp %03d, Reward %-5.1f, %s,' % \
137 | (env.index, env.goal_point[0], env.goal_point[1], id, step-1, ep_reward, result))
138 | logger_cal.info(ep_reward)
139 |
140 |
141 |
142 |
143 |
144 |
145 | if __name__ == '__main__':
146 |
147 | # config log
148 | hostname = socket.gethostname()
149 | if not os.path.exists('./log/' + hostname):
150 | os.makedirs('./log/' + hostname)
151 | output_file = './log/' + hostname + '/output.log'
152 | cal_file = './log/' + hostname + '/cal.log'
153 |
154 | # config log
155 | logger = logging.getLogger('mylogger')
156 | logger.setLevel(logging.INFO)
157 |
158 | file_handler = logging.FileHandler(output_file, mode='a')
159 | file_handler.setLevel(logging.INFO)
160 | file_handler.setFormatter(logging.Formatter("%(asctime)s - %(levelname)s - %(message)s"))
161 | stdout_handler = logging.StreamHandler(sys.stdout)
162 | stdout_handler.setLevel(logging.INFO)
163 | logger.addHandler(file_handler)
164 | logger.addHandler(stdout_handler)
165 |
166 | logger_cal = logging.getLogger('loggercal')
167 | logger_cal.setLevel(logging.INFO)
168 | cal_f_handler = logging.FileHandler(cal_file, mode='a')
169 | file_handler.setLevel(logging.INFO)
170 | logger_cal.addHandler(cal_f_handler)
171 |
172 |
173 | comm = MPI.COMM_WORLD
174 | rank = comm.Get_rank()
175 | size = comm.Get_size()
176 |
177 | env = StageWorld(512, index=rank, num_env=NUM_ENV)
178 | reward = None
179 | action_bound = [[0, -1], [1, 1]]
180 | # torch.manual_seed(1)
181 | # np.random.seed(1)
182 |
183 | if rank == 0:
184 | policy_path = 'policy'
185 | # policy = MLPPolicy(obs_size, act_size)
186 | policy = CNNPolicy(frames=LASER_HIST, action_space=2)
187 | policy.cuda()
188 | opt = Adam(policy.parameters(), lr=LEARNING_RATE)
189 | mse = nn.MSELoss()
190 |
191 | if not os.path.exists(policy_path):
192 | os.makedirs(policy_path)
193 |
194 | file = policy_path + '/stage2.pth'
195 | if os.path.exists(file):
196 | logger.info('####################################')
197 | logger.info('############Loading Model###########')
198 | logger.info('####################################')
199 | state_dict = torch.load(file)
200 | policy.load_state_dict(state_dict)
201 | else:
202 | logger.info('#####################################')
203 | logger.info('############Start Training###########')
204 | logger.info('#####################################')
205 | else:
206 | policy = None
207 | policy_path = None
208 | opt = None
209 |
210 |
211 | try:
212 | run(comm=comm, env=env, policy=policy, policy_path=policy_path, action_bound=action_bound, optimizer=opt)
213 | except KeyboardInterrupt:
214 | import traceback
215 | traceback.print_exc()
216 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/.gitignore:
--------------------------------------------------------------------------------
1 | build
2 | .svn
3 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/.travis.yml:
--------------------------------------------------------------------------------
1 |
2 | sudo: required
3 | dist: trusty
4 | # Force travis to use its minimal image with default Python settings
5 | language: generic
6 | compiler:
7 | - gcc
8 | env:
9 | global:
10 | - CATKIN_WS=~/catkin_ws
11 | - CATKIN_WS_SRC=${CATKIN_WS}/src
12 | matrix:
13 | - CI_ROS_DISTRO="indigo"
14 | - CI_ROS_DISTRO="jade"
15 | install:
16 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
17 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
18 | - sudo apt-get update -qq
19 | - sudo apt-get install -qq -y python-rosdep python-catkin-tools
20 | - sudo rosdep init
21 | - rosdep update
22 | # Use rosdep to install all dependencies (including ROS itself)
23 | - rosdep install --from-paths ./ -i -y --rosdistro $CI_ROS_DISTRO
24 | script:
25 | - source /opt/ros/$CI_ROS_DISTRO/setup.bash
26 | - mkdir -p $CATKIN_WS_SRC
27 | - ln -s $TRAVIS_BUILD_DIR $CATKIN_WS_SRC
28 | - cd $CATKIN_WS
29 | - catkin init
30 | # Enable install space
31 | - catkin config --install
32 | # Build [and Install] packages
33 | - catkin build --no-status -vi --no-notify -DCMAKE_BUILD_TYPE=Release
34 | # Build tests
35 | - catkin build --no-status -vi --no-notify --make-args tests
36 | # Run tests
37 | - catkin run_tests
38 | # Assert tests all passed
39 | - catkin_test_results ./build
40 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/CHANGELOG.rst:
--------------------------------------------------------------------------------
1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2 | Changelog for package stage_ros
3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4 |
5 | 1.7.5 (2015-09-16)
6 | ------------------
7 | * Removed all references to FLTK/Fluid and use the upstream CMake config file instead.
8 | * Added ``reset_positions`` service to stage (adds dependency on ``std_srvs``).
9 | * Contributors: Aurélien Ballier, Daniel Claes, Scott K Logan, William Woodall
10 |
11 | 1.7.4 (2015-03-04)
12 | ------------------
13 | * Added missing -ldl flag on newer versions of Ubuntu
14 | * Contributors: William Woodall
15 |
16 | 1.7.3 (2015-01-26)
17 | ------------------
18 | * Split up ``fltk`` dep into ``libfltk-dev`` and ``fluid``, only ``run_depend``'ing on fluid.
19 | * Now supports multiple robots with multiple sensors.
20 | * Fixed a bug on systems that cannot populate FLTK_INCLUDE_DIRS.
21 | * Updated topurg model from "laser" to "ranger".
22 | * Added -u option to use name property of position models as its namespace instead of "robot_0", "robot_1", etc.
23 | * Contributors: Gustavo Velasco Hernández, Gustavo Velasco-Hernández, Pablo Urcola, Wayne Chang, William Woodall
24 |
25 | 1.7.2 (2013-09-19)
26 | ------------------
27 | * Changed default GUI window size to 600x400
28 | * Added velocity to ground truth odometry
29 | * Fixed tf (yaw component) for the base_link->camera transform.
30 | * Fixed ground truth pose coordinate system
31 |
32 | 1.7.1 (2013-08-30)
33 | ------------------
34 | * Fixing warnings
35 | * Small fixes
36 | * Added RGB+3D-sensor interface (Primesense/Kinect/Xtion).
37 | * Publishes CameraInfo, depth image, RGBA image, tf (takes world-file pantilt paremeter into account)
38 | * Supports the "old" configuration (laser+odom) as well as camera+odom, laser+camera+odom and odom-only.
39 | Fixed laser transform height (previously was hardcoded at 0.15, now it takes robot height into account).
40 | * Introduced changes from https://github.com/rtv/Stage/issues/34 with some changes (does not require lasers to be present and works without cameras).
41 |
42 | 1.7.0 (2013-06-27 18:15:07 -0700)
43 | ---------------------------------
44 | - Initial move over from old repository: https://code.ros.org/svn/ros-pkg/stacks/stage
45 | - Catkinized
46 | - Stage itself is released as a third party package now
47 | - Had to disable velocities in the output odometry as Stage no longer implements it internally.
48 | - Updated rostest
49 | - Updated rviz configurations
50 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(stage_ros_add_pose_and_crash)
3 |
4 | find_package(catkin REQUIRED
5 | COMPONENTS
6 | geometry_msgs
7 | nav_msgs
8 | roscpp
9 | sensor_msgs
10 | std_msgs
11 | std_srvs
12 | tf
13 | )
14 |
15 | find_package(Boost REQUIRED COMPONENTS system thread)
16 |
17 | find_package(stage REQUIRED)
18 |
19 | include_directories(
20 | ${catkin_INCLUDE_DIRS}
21 | ${Boost_INCLUDE_DIRS}
22 | ${STAGE_INCLUDE_DIRS}
23 | )
24 |
25 | catkin_package()
26 |
27 | # Declare a cpp executable
28 | add_executable(stageros src/stageros.cpp)
29 | set(${PROJECT_NAME}_extra_libs "")
30 | if(UNIX AND NOT APPLE)
31 | set(${PROJECT_NAME}_extra_libs dl)
32 | endif()
33 | target_link_libraries(stageros
34 | ${catkin_LIBRARIES}
35 | ${Boost_LIBRARIES}
36 | ${STAGE_LIBRARIES}
37 | ${${PROJECT_NAME}_extra_libs}
38 | )
39 | if(catkin_EXPORTED_TARGETS)
40 | add_dependencies(stageros ${catkin_EXPORTED_TARGETS})
41 | endif()
42 |
43 | ## Install
44 |
45 | install(PROGRAMS scripts/upgrade-world.sh
46 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
47 |
48 | install(TARGETS stageros
49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
52 | )
53 |
54 | install(DIRECTORY rviz
55 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
56 |
57 | install(DIRECTORY world
58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
59 |
60 | ## Tests
61 |
62 | if(CATKIN_ENABLE_TESTING)
63 | find_package(rostest REQUIRED)
64 | add_rostest(test/hztest.xml)
65 | add_rostest(test/cmdpose_tests.xml)
66 | endif()
67 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/launch/maptest.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 |
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 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/launch/maptest2.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 |
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 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | stage_ros_add_pose_and_crash
4 | 1.7.5
5 | This package provides ROS specific hooks for stage
6 |
7 | William Woodall
8 |
9 | BSD
10 |
11 | http://ros.org/wiki/stage_ros
12 | https://github.com/ros-simulation/stage_ros
13 | https://github.com/ros-simulation/stage_ros/issues
14 |
15 | Brian Gerky
16 |
17 | catkin
18 |
19 | boost
20 | geometry_msgs
21 | nav_msgs
22 | roscpp
23 | rostest
24 | sensor_msgs
25 | stage
26 | std_msgs
27 | std_srvs
28 | tf
29 |
30 | boost
31 | geometry_msgs
32 | nav_msgs
33 | roscpp
34 | sensor_msgs
35 | stage
36 | std_msgs
37 | std_srvs
38 | tf
39 |
40 | rospy
41 |
42 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/rviz/stage.rviz:
--------------------------------------------------------------------------------
1 | Panels:
2 | - Class: rviz/Displays
3 | Help Height: 78
4 | Name: Displays
5 | Property Tree Widget:
6 | Expanded:
7 | - /Global Options1
8 | - /TF1/Frames1
9 | Splitter Ratio: 0.5
10 | Tree Height: 479
11 | - Class: rviz/Selection
12 | Name: Selection
13 | - Class: rviz/Tool Properties
14 | Expanded:
15 | - /2D Pose Estimate1
16 | - /2D Nav Goal1
17 | - /Publish Point1
18 | Name: Tool Properties
19 | Splitter Ratio: 0.588679
20 | - Class: rviz/Views
21 | Expanded:
22 | - /Current View1
23 | Name: Views
24 | Splitter Ratio: 0.5
25 | - Class: rviz/Time
26 | Experimental: false
27 | Name: Time
28 | SyncMode: 0
29 | SyncSource: LaserScan
30 | Visualization Manager:
31 | Class: ""
32 | Displays:
33 | - Alpha: 0.5
34 | Cell Size: 1
35 | Class: rviz/Grid
36 | Color: 160; 160; 164
37 | Enabled: true
38 | Line Style:
39 | Line Width: 0.03
40 | Value: Lines
41 | Name: Grid
42 | Normal Cell Count: 0
43 | Offset:
44 | X: 0
45 | Y: 0
46 | Z: 0
47 | Plane: XY
48 | Plane Cell Count: 10
49 | Reference Frame:
50 | Value: true
51 | - Class: rviz/TF
52 | Enabled: true
53 | Frame Timeout: 15
54 | Frames:
55 | /base_footprint:
56 | Value: true
57 | /base_laser_link:
58 | Value: true
59 | /base_link:
60 | Value: true
61 | /odom:
62 | Value: true
63 | All Enabled: false
64 | Marker Scale: 1
65 | Name: TF
66 | Show Arrows: false
67 | Show Axes: true
68 | Show Names: false
69 | Tree:
70 | /odom:
71 | /base_footprint:
72 | /base_link:
73 | /base_laser_link:
74 | {}
75 | Update Interval: 0
76 | Value: true
77 | - Alpha: 1
78 | Autocompute Intensity Bounds: true
79 | Autocompute Value Bounds:
80 | Max Value: 10
81 | Min Value: -10
82 | Value: true
83 | Axis: Z
84 | Channel Name: intensity
85 | Class: rviz/LaserScan
86 | Color: 255; 255; 255
87 | Color Transformer: Intensity
88 | Decay Time: 0
89 | Enabled: true
90 | Max Color: 255; 255; 255
91 | Max Intensity: 1
92 | Min Color: 0; 0; 0
93 | Min Intensity: 1
94 | Name: LaserScan
95 | Position Transformer: XYZ
96 | Queue Size: 10
97 | Selectable: true
98 | Size (Pixels): 3
99 | Size (m): 0.01
100 | Style: Flat Squares
101 | Topic: /base_scan
102 | Use Fixed Frame: true
103 | Use rainbow: true
104 | Value: true
105 | Enabled: true
106 | Global Options:
107 | Background Color: 48; 48; 48
108 | Fixed Frame: /odom
109 | Name: root
110 | Tools:
111 | - Class: rviz/Interact
112 | Hide Inactive Objects: true
113 | - Class: rviz/MoveCamera
114 | - Class: rviz/Select
115 | - Class: rviz/FocusCamera
116 | - Class: rviz/Measure
117 | - Class: rviz/SetInitialPose
118 | Topic: /initialpose
119 | - Class: rviz/SetGoal
120 | Topic: /move_base_simple/goal
121 | - Class: rviz/PublishPoint
122 | Single click: true
123 | Topic: /clicked_point
124 | Value: true
125 | Views:
126 | Current:
127 | Class: rviz/Orbit
128 | Distance: 5.68654
129 | Focal Point:
130 | X: 0
131 | Y: 0
132 | Z: 0
133 | Name: Current View
134 | Near Clip Distance: 0.01
135 | Pitch: 0.529796
136 | Target Frame:
137 | Value: Orbit (rviz)
138 | Yaw: 3.2654
139 | Saved: ~
140 | Window Geometry:
141 | Displays:
142 | collapsed: false
143 | Height: 756
144 | Hide Left Dock: false
145 | Hide Right Dock: false
146 | QMainWindow State: 000000ff00000000fd00000004000000000000013c0000026dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c0000026d000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000026dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002c0000026d000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650100000000000004fc0000020b00fffffffb0000000800540069006d00650100000000000004500000000000000000000002af0000026d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
147 | Selection:
148 | collapsed: false
149 | Time:
150 | collapsed: false
151 | Tool Properties:
152 | collapsed: false
153 | Views:
154 | collapsed: false
155 | Width: 1276
156 | X: 60
157 | Y: 22
158 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/scripts/upgrade-world.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | USAGE="USAGE: upgrade-world.sh in.world out.world"
4 |
5 | if [ $# != 2 ]; then
6 | echo $USAGE
7 | exit 1
8 | fi
9 |
10 | cat < $1 | sed 's/size3/size/' | sed 's/origin3/origin/' | sed 's/.*interval_real.*//' | sed 's/.*range_min.*//' | sed 's/.*center.*//' | sed 's/.*gui_movemask.*//' | sed 's/\(.*pose *\[\)\([^ ]*\) *\([^ ]*\) *\([^ ]*\) *\(\].*\)/\1 \2 \3 0 \4 \5/' > $2
11 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/src/stageros.cpp:
--------------------------------------------------------------------------------
1 | /*
2 | * stageros
3 | * Copyright (c) 2008, Willow Garage, Inc.
4 | *
5 | * This program is free software; you can redistribute it and/or modify
6 | * it under the terms of the GNU General Public License as published by
7 | * the Free Software Foundation; either version 2 of the License, or
8 | * (at your option) any later version.
9 | *
10 | * This program is distributed in the hope that it will be useful,
11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 | * GNU General Public License for more details.
14 | *
15 | * You should have received a copy of the GNU General Public License
16 | * along with this program; if not, write to the Free Software
17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 | */
19 | /**
20 |
21 | @mainpage
22 |
23 | @htmlinclude manifest.html
24 | **/
25 |
26 | #include
27 | #include
28 | #include
29 |
30 | #include
31 | #include
32 | #include
33 | #include
34 |
35 |
36 | // libstage
37 | #include
38 |
39 | // roscpp
40 | #include
41 | #include
42 | #include
43 | #include
44 | #include
45 | #include
46 | #include
47 | #include
48 | #include
49 | #include
50 | #include
51 |
52 | #include
53 | #include
54 | #include "tf/LinearMath/Transform.h"
55 | #include
56 |
57 | #include "tf/transform_broadcaster.h"
58 |
59 | #define USAGE "stageros "
60 | #define IMAGE "image"
61 | #define DEPTH "depth"
62 | #define CAMERA_INFO "camera_info"
63 | #define ODOM "odom"
64 | #define BASE_SCAN "base_scan"
65 | #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
66 | #define CMD_VEL "cmd_vel"
67 | #define POSE "cmd_pose"
68 | #define POSESTAMPED "cmd_pose_stamped"
69 | #define IS_CRASHED "is_crashed"
70 |
71 | // Our node
72 | class StageNode
73 | {
74 | private:
75 |
76 | // roscpp-related bookkeeping
77 | ros::NodeHandle n_;
78 |
79 | // A mutex to lock access to fields that are used in message callbacks
80 | boost::mutex msg_lock;
81 |
82 | // The models that we're interested in
83 | std::vector cameramodels;
84 | std::vector lasermodels;
85 | std::vector positionmodels;
86 |
87 | //a structure representing a robot inthe simulator
88 | struct StageRobot
89 | {
90 | //stage related models
91 | Stg::ModelPosition* positionmodel; //one position
92 | std::vector cameramodels; //multiple cameras per position
93 | std::vector lasermodels; //multiple rangers per position
94 |
95 | //ros publishers
96 | ros::Publisher odom_pub; //one odom
97 | ros::Publisher ground_truth_pub; //one ground truth
98 | ros::Publisher stall_pub;
99 |
100 | std::vector image_pubs; //multiple images
101 | std::vector depth_pubs; //multiple depths
102 | std::vector camera_pubs; //multiple cameras
103 | std::vector laser_pubs; //multiple lasers
104 |
105 | ros::Subscriber cmdvel_sub; //one cmd_vel subscriber
106 | ros::Subscriber pose_sub;
107 | ros::Subscriber posestamped_sub;
108 | };
109 |
110 | std::vector robotmodels_;
111 |
112 | // Used to remember initial poses for soft reset
113 | std::vector initial_poses;
114 | ros::ServiceServer reset_srv_;
115 |
116 | ros::Publisher clock_pub_;
117 |
118 | bool isDepthCanonical;
119 | bool use_model_names;
120 |
121 | // A helper function that is executed for each stage model. We use it
122 | // to search for models of interest.
123 | static void ghfunc(Stg::Model* mod, StageNode* node);
124 |
125 | static bool s_update(Stg::World* world, StageNode* node)
126 | {
127 | node->WorldCallback();
128 | // We return false to indicate that we want to be called again (an
129 | // odd convention, but that's the way that Stage works).
130 | return false;
131 | }
132 |
133 | // Appends the given robot ID to the given message name. If omitRobotID
134 | // is true, an unaltered copy of the name is returned.
135 | const char *mapName(const char *name, size_t robotID, Stg::Model* mod) const;
136 | const char *mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;
137 |
138 | tf::TransformBroadcaster tf;
139 |
140 | // Last time that we received a velocity command
141 | ros::Time base_last_cmd;
142 | ros::Duration base_watchdog_timeout;
143 |
144 | // Current simulation time
145 | ros::Time sim_time;
146 |
147 | // Last time we saved global position (for velocity calculation).
148 | ros::Time base_last_globalpos_time;
149 | // Last published global pose of each robot
150 | std::vector base_last_globalpos;
151 |
152 | public:
153 | // Constructor; stage itself needs argc/argv. fname is the .world file
154 | // that stage should load.
155 | StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names);
156 | ~StageNode();
157 |
158 | // Subscribe to models of interest. Currently, we find and subscribe
159 | // to the first 'laser' model and the first 'position' model. Returns
160 | // 0 on success (both models subscribed), -1 otherwise.
161 | int SubscribeModels();
162 |
163 | // Our callback
164 | void WorldCallback();
165 |
166 | // Do one update of the world. May pause if the next update time
167 | // has not yet arrived.
168 | bool UpdateWorld();
169 |
170 | // Message callback for a cmd_vel message, which set velocities.
171 | void cmdvelReceived(int idx, const boost::shared_ptr& msg);
172 |
173 | // Message callback for a cmd_pose message, which sets positions.
174 | void poseReceived(int idx, const boost::shared_ptr& msg);
175 |
176 | // Message callback for a cmd_pose_stamped message, which sets positions
177 | // with a timestamp (e.g., from rviz).
178 | void poseStampedReceived(int idx, const boost::shared_ptr& msg);
179 |
180 | // Service callback for soft reset
181 | bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
182 |
183 | // The main simulator object
184 | Stg::World* world;
185 | };
186 |
187 | // since stageros is single-threaded, this is OK. revisit if that changes!
188 | const char *
189 | StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
190 | {
191 | //ROS_INFO("Robot %lu: Device %s", robotID, name);
192 | bool umn = this->use_model_names;
193 |
194 | if ((positionmodels.size() > 1 ) || umn)
195 | {
196 | static char buf[100];
197 | std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
198 |
199 | if ((found==std::string::npos) && umn)
200 | {
201 | snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
202 | }
203 | else
204 | {
205 | snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
206 | }
207 |
208 | return buf;
209 | }
210 | else
211 | return name;
212 | }
213 |
214 | const char *
215 | StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const
216 | {
217 | //ROS_INFO("Robot %lu: Device %s:%lu", robotID, name, deviceID);
218 | bool umn = this->use_model_names;
219 |
220 | if ((positionmodels.size() > 1 ) || umn)
221 | {
222 | static char buf[100];
223 | std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
224 |
225 | if ((found==std::string::npos) && umn)
226 | {
227 | snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
228 | }
229 | else
230 | {
231 | snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
232 | }
233 |
234 | return buf;
235 | }
236 | else
237 | {
238 | static char buf[100];
239 | snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID);
240 | return buf;
241 | }
242 | }
243 |
244 | void
245 | StageNode::ghfunc(Stg::Model* mod, StageNode* node)
246 | {
247 | if (dynamic_cast(mod))
248 | node->lasermodels.push_back(dynamic_cast(mod));
249 | if (dynamic_cast(mod)) {
250 | Stg::ModelPosition * p = dynamic_cast(mod);
251 | // remember initial poses
252 | node->positionmodels.push_back(p);
253 | node->initial_poses.push_back(p->GetGlobalPose());
254 | }
255 | if (dynamic_cast(mod))
256 | node->cameramodels.push_back(dynamic_cast(mod));
257 | }
258 |
259 |
260 | bool
261 | StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
262 | {
263 | ROS_INFO("Resetting stage!");
264 | for (size_t r = 0; r < this->positionmodels.size(); r++) {
265 | this->positionmodels[r]->SetPose(this->initial_poses[r]);
266 | this->positionmodels[r]->SetStall(false);
267 | }
268 | return true;
269 | }
270 |
271 |
272 | void
273 | StageNode::cmdvelReceived(int idx, const boost::shared_ptr& msg)
274 | {
275 | boost::mutex::scoped_lock lock(msg_lock);
276 | this->positionmodels[idx]->SetSpeed(msg->linear.x,
277 | msg->linear.y,
278 | msg->angular.z);
279 | this->base_last_cmd = this->sim_time;
280 | }
281 |
282 | void
283 | StageNode::poseReceived(int idx, const boost::shared_ptr& msg)
284 | {
285 | boost::mutex::scoped_lock lock(msg_lock);
286 | Stg::Pose pose;
287 |
288 | double roll, pitch, yaw;
289 | tf::Matrix3x3 m(tf::Quaternion(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w));
290 | m.getRPY(roll, pitch, yaw);
291 | pose.x = msg->position.x;
292 | pose.y = msg->position.y;
293 | pose.z = 0;
294 | pose.a = yaw;
295 | this->positionmodels[idx]->SetPose(pose);
296 | }
297 |
298 | void
299 | StageNode::poseStampedReceived(int idx, const boost::shared_ptr& msg)
300 | {
301 | // Create a shared pointer to the raw pose and pass it through.
302 | // Note that we ignore the header because we're not supporting setting of
303 | // poses in an arbitrary frame. Every pose is interpreted to be in Stage's
304 | // world frame (which isn't represented anywhere as a tf frame).
305 | geometry_msgs::Pose* pose = new geometry_msgs::Pose;
306 | *pose = msg->pose;
307 | boost::shared_ptr pose_ptr(pose);
308 | this->poseReceived(idx, pose_ptr);
309 | }
310 |
311 | StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names)
312 | {
313 | this->use_model_names = use_model_names;
314 | this->sim_time.fromSec(0.0);
315 | this->base_last_cmd.fromSec(0.0);
316 | double t;
317 | ros::NodeHandle localn("~");
318 | if(!localn.getParam("base_watchdog_timeout", t))
319 | t = 0.2;
320 | this->base_watchdog_timeout.fromSec(t);
321 |
322 | if(!localn.getParam("is_depth_canonical", isDepthCanonical))
323 | isDepthCanonical = true;
324 |
325 |
326 | // We'll check the existence of the world file, because libstage doesn't
327 | // expose its failure to open it. Could go further with checks (e.g., is
328 | // it readable by this user).
329 | struct stat s;
330 | if(stat(fname, &s) != 0)
331 | {
332 | ROS_FATAL("The world file %s does not exist.", fname);
333 | ROS_BREAK();
334 | }
335 |
336 | // initialize libstage
337 | Stg::Init( &argc, &argv );
338 |
339 | if(gui)
340 | this->world = new Stg::WorldGui(600, 400, "Stage (ROS)");
341 | else
342 | this->world = new Stg::World();
343 |
344 | // Apparently an Update is needed before the Load to avoid crashes on
345 | // startup on some systems.
346 | // As of Stage 4.1.1, this update call causes a hang on start.
347 | //this->UpdateWorld();
348 | this->world->Load(fname);
349 |
350 | // We add our callback here, after the Update, so avoid our callback
351 | // being invoked before we're ready.
352 | this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this);
353 |
354 | this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this);
355 | }
356 |
357 |
358 | // Subscribe to models of interest. Currently, we find and subscribe
359 | // to the first 'laser' model and the first 'position' model. Returns
360 | // 0 on success (both models subscribed), -1 otherwise.
361 | //
362 | // Eventually, we should provide a general way to map stage models onto ROS
363 | // topics, similar to Player .cfg files.
364 | int
365 | StageNode::SubscribeModels()
366 | {
367 | n_.setParam("/use_sim_time", true);
368 |
369 | for (size_t r = 0; r < this->positionmodels.size(); r++)
370 | {
371 | StageRobot* new_robot = new StageRobot;
372 | new_robot->positionmodel = this->positionmodels[r];
373 | new_robot->positionmodel->Subscribe();
374 |
375 |
376 | for (size_t s = 0; s < this->lasermodels.size(); s++)
377 | {
378 | if (this->lasermodels[s] and this->lasermodels[s]->Parent() == new_robot->positionmodel)
379 | {
380 | new_robot->lasermodels.push_back(this->lasermodels[s]);
381 | this->lasermodels[s]->Subscribe();
382 | }
383 | }
384 |
385 | for (size_t s = 0; s < this->cameramodels.size(); s++)
386 | {
387 | if (this->cameramodels[s] and this->cameramodels[s]->Parent() == new_robot->positionmodel)
388 | {
389 | new_robot->cameramodels.push_back(this->cameramodels[s]);
390 | this->cameramodels[s]->Subscribe();
391 | }
392 | }
393 |
394 | ROS_INFO("Found %lu laser devices and %lu cameras in robot %lu", new_robot->lasermodels.size(), new_robot->cameramodels.size(), r);
395 |
396 | new_robot->odom_pub = n_.advertise(mapName(ODOM, r, static_cast(new_robot->positionmodel)), 10);
397 | new_robot->stall_pub = n_.advertise(mapName(IS_CRASHED, r, static_cast(new_robot->positionmodel)), 10);
398 | new_robot->ground_truth_pub = n_.advertise(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast(new_robot->positionmodel)), 10);
399 | new_robot->cmdvel_sub = n_.subscribe(mapName(CMD_VEL, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1));
400 | new_robot->pose_sub = n_.subscribe(mapName(POSE, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseReceived, this, r, _1));
401 |
402 | new_robot->posestamped_sub = n_.subscribe(mapName(POSESTAMPED, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseStampedReceived, this, r, _1));
403 |
404 | for (size_t s = 0; s < new_robot->lasermodels.size(); ++s)
405 | {
406 | if (new_robot->lasermodels.size() == 1)
407 | new_robot->laser_pubs.push_back(n_.advertise(mapName(BASE_SCAN, r, static_cast(new_robot->positionmodel)), 10));
408 | else
409 | new_robot->laser_pubs.push_back(n_.advertise(mapName(BASE_SCAN, r, s, static_cast(new_robot->positionmodel)), 10));
410 |
411 | }
412 |
413 | for (size_t s = 0; s < new_robot->cameramodels.size(); ++s)
414 | {
415 | if (new_robot->cameramodels.size() == 1)
416 | {
417 | new_robot->image_pubs.push_back(n_.advertise(mapName(IMAGE, r, static_cast(new_robot->positionmodel)), 10));
418 | new_robot->depth_pubs.push_back(n_.advertise(mapName(DEPTH, r, static_cast(new_robot->positionmodel)), 10));
419 | new_robot->camera_pubs.push_back(n_.advertise(mapName(CAMERA_INFO, r, static_cast(new_robot->positionmodel)), 10));
420 | }
421 | else
422 | {
423 | new_robot->image_pubs.push_back(n_.advertise(mapName(IMAGE, r, s, static_cast(new_robot->positionmodel)), 10));
424 | new_robot->depth_pubs.push_back(n_.advertise(mapName(DEPTH, r, s, static_cast(new_robot->positionmodel)), 10));
425 | new_robot->camera_pubs.push_back(n_.advertise(mapName(CAMERA_INFO, r, s, static_cast(new_robot->positionmodel)), 10));
426 | }
427 | }
428 |
429 | this->robotmodels_.push_back(new_robot);
430 | }
431 | clock_pub_ = n_.advertise("/clock", 10);
432 |
433 | // advertising reset service
434 | reset_srv_ = n_.advertiseService("reset_positions", &StageNode::cb_reset_srv, this);
435 |
436 | return(0);
437 | }
438 |
439 | StageNode::~StageNode()
440 | {
441 | for (std::vector::iterator r = this->robotmodels_.begin(); r != this->robotmodels_.end(); ++r)
442 | delete *r;
443 | }
444 |
445 | bool
446 | StageNode::UpdateWorld()
447 | {
448 | return this->world->UpdateAll();
449 | }
450 |
451 | void
452 | StageNode::WorldCallback()
453 | {
454 | boost::mutex::scoped_lock lock(msg_lock);
455 |
456 | this->sim_time.fromSec(world->SimTimeNow() / 1e6);
457 | // We're not allowed to publish clock==0, because it used as a special
458 | // value in parts of ROS, #4027.
459 | if(this->sim_time.sec == 0 && this->sim_time.nsec == 0)
460 | {
461 | ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0");
462 | return;
463 | }
464 |
465 | // TODO make this only affect one robot if necessary
466 | if((this->base_watchdog_timeout.toSec() > 0.0) &&
467 | ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
468 | {
469 | for (size_t r = 0; r < this->positionmodels.size(); r++)
470 | this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
471 | }
472 |
473 | //loop on the robot models
474 | for (size_t r = 0; r < this->robotmodels_.size(); ++r)
475 | {
476 | StageRobot const * robotmodel = this->robotmodels_[r];
477 |
478 | //loop on the laser devices for the current robot
479 | for (size_t s = 0; s < robotmodel->lasermodels.size(); ++s)
480 | {
481 | Stg::ModelRanger const* lasermodel = robotmodel->lasermodels[s];
482 | const std::vector& sensors = lasermodel->GetSensors();
483 |
484 | if( sensors.size() > 1 )
485 | ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." );
486 |
487 | // for now we access only the zeroth sensor of the ranger - good
488 | // enough for most laser models that have a single beam origin
489 | const Stg::ModelRanger::Sensor& sensor = sensors[0];
490 |
491 | if( sensor.ranges.size() )
492 | {
493 | // Translate into ROS message format and publish
494 | sensor_msgs::LaserScan msg;
495 | msg.angle_min = -sensor.fov/2.0;
496 | msg.angle_max = +sensor.fov/2.0;
497 | msg.angle_increment = sensor.fov/(double)(sensor.sample_count-1);
498 | msg.range_min = sensor.range.min;
499 | msg.range_max = sensor.range.max;
500 | msg.ranges.resize(sensor.ranges.size());
501 | msg.intensities.resize(sensor.intensities.size());
502 |
503 | for(unsigned int i = 0; i < sensor.ranges.size(); i++)
504 | {
505 | msg.ranges[i] = sensor.ranges[i];
506 | msg.intensities[i] = (uint8_t)sensor.intensities[i];
507 | }
508 |
509 | if (robotmodel->lasermodels.size() > 1)
510 | msg.header.frame_id = mapName("base_laser_link", r, s, static_cast(robotmodel->positionmodel));
511 | else
512 | msg.header.frame_id = mapName("base_laser_link", r, static_cast(robotmodel->positionmodel));
513 |
514 | msg.header.stamp = sim_time;
515 | robotmodel->laser_pubs[s].publish(msg);
516 | }
517 |
518 | // Also publish the base->base_laser_link Tx. This could eventually move
519 | // into being retrieved from the param server as a static Tx.
520 | Stg::Pose lp = lasermodel->GetPose();
521 | tf::Quaternion laserQ;
522 | laserQ.setRPY(0.0, 0.0, lp.a);
523 | tf::Transform txLaser = tf::Transform(laserQ, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z + lp.z));
524 |
525 | if (robotmodel->lasermodels.size() > 1)
526 | tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
527 | mapName("base_link", r, static_cast(robotmodel->positionmodel)),
528 | mapName("base_laser_link", r, s, static_cast(robotmodel->positionmodel))));
529 | else
530 | tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
531 | mapName("base_link", r, static_cast(robotmodel->positionmodel)),
532 | mapName("base_laser_link", r, static_cast(robotmodel->positionmodel))));
533 | }
534 |
535 | //the position of the robot
536 | tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(),
537 | sim_time,
538 | mapName("base_footprint", r, static_cast(robotmodel->positionmodel)),
539 | mapName("base_link", r, static_cast(robotmodel->positionmodel))));
540 |
541 | // Get latest odometry data
542 | // Translate into ROS message format and publish
543 | nav_msgs::Odometry odom_msg;
544 | odom_msg.pose.pose.position.x = robotmodel->positionmodel->est_pose.x;
545 | odom_msg.pose.pose.position.y = robotmodel->positionmodel->est_pose.y;
546 | odom_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robotmodel->positionmodel->est_pose.a);
547 | Stg::Velocity v = robotmodel->positionmodel->GetVelocity();
548 | odom_msg.twist.twist.linear.x = v.x;
549 | odom_msg.twist.twist.linear.y = v.y;
550 | odom_msg.twist.twist.angular.z = v.a;
551 |
552 | //@todo Publish stall on a separate topic when one becomes available
553 | //this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
554 | //
555 | odom_msg.header.frame_id = mapName("odom", r, static_cast(robotmodel->positionmodel));
556 | odom_msg.header.stamp = sim_time;
557 |
558 | robotmodel->odom_pub.publish(odom_msg);
559 |
560 | //Publish stall on separate topic
561 | //ROS_INFO("Stalled status: %d", robotmodel->positionmodel->Stalled());
562 | std_msgs::Int8 stall_status;
563 | stall_status.data = robotmodel->positionmodel->Stalled();
564 | robotmodel->stall_pub.publish(stall_status);
565 |
566 | // broadcast odometry transform
567 | tf::Quaternion odomQ;
568 | tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, odomQ);
569 | tf::Transform txOdom(odomQ, tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0));
570 | tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
571 | mapName("odom", r, static_cast(robotmodel->positionmodel)),
572 | mapName("base_footprint", r, static_cast(robotmodel->positionmodel))));
573 |
574 | // Also publish the ground truth pose and velocity
575 | Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose();
576 | tf::Quaternion q_gpose;
577 | q_gpose.setRPY(0.0, 0.0, gpose.a);
578 | tf::Transform gt(q_gpose, tf::Point(gpose.x, gpose.y, 0.0));
579 | // Velocity is 0 by default and will be set only if there is previous pose and time delta>0
580 | Stg::Velocity gvel(0,0,0,0);
581 | if (this->base_last_globalpos.size()>r){
582 | Stg::Pose prevpose = this->base_last_globalpos.at(r);
583 | double dT = (this->sim_time-this->base_last_globalpos_time).toSec();
584 | if (dT>0)
585 | gvel = Stg::Velocity(
586 | (gpose.x - prevpose.x)/dT,
587 | (gpose.y - prevpose.y)/dT,
588 | (gpose.z - prevpose.z)/dT,
589 | Stg::normalize(gpose.a - prevpose.a)/dT
590 | );
591 | this->base_last_globalpos.at(r) = gpose;
592 | }else //There are no previous readings, adding current pose...
593 | this->base_last_globalpos.push_back(gpose);
594 |
595 | nav_msgs::Odometry ground_truth_msg;
596 | ground_truth_msg.pose.pose.position.x = gt.getOrigin().x();
597 | ground_truth_msg.pose.pose.position.y = gt.getOrigin().y();
598 | ground_truth_msg.pose.pose.position.z = gt.getOrigin().z();
599 | ground_truth_msg.pose.pose.orientation.x = gt.getRotation().x();
600 | ground_truth_msg.pose.pose.orientation.y = gt.getRotation().y();
601 | ground_truth_msg.pose.pose.orientation.z = gt.getRotation().z();
602 | ground_truth_msg.pose.pose.orientation.w = gt.getRotation().w();
603 | ground_truth_msg.twist.twist.linear.x = gvel.x;
604 | ground_truth_msg.twist.twist.linear.y = gvel.y;
605 | ground_truth_msg.twist.twist.linear.z = gvel.z;
606 | ground_truth_msg.twist.twist.angular.z = gvel.a;
607 |
608 | ground_truth_msg.header.frame_id = mapName("odom", r, static_cast(robotmodel->positionmodel));
609 | ground_truth_msg.header.stamp = sim_time;
610 |
611 | robotmodel->ground_truth_pub.publish(ground_truth_msg);
612 |
613 | //cameras
614 | for (size_t s = 0; s < robotmodel->cameramodels.size(); ++s)
615 | {
616 | Stg::ModelCamera* cameramodel = robotmodel->cameramodels[s];
617 | // Get latest image data
618 | // Translate into ROS message format and publish
619 | if (robotmodel->image_pubs[s].getNumSubscribers() > 0 && cameramodel->FrameColor())
620 | {
621 | sensor_msgs::Image image_msg;
622 |
623 | image_msg.height = cameramodel->getHeight();
624 | image_msg.width = cameramodel->getWidth();
625 | image_msg.encoding = "rgba8";
626 | //this->imageMsgs[r].is_bigendian="";
627 | image_msg.step = image_msg.width*4;
628 | image_msg.data.resize(image_msg.width * image_msg.height * 4);
629 |
630 | memcpy(&(image_msg.data[0]), cameramodel->FrameColor(), image_msg.width * image_msg.height * 4);
631 |
632 | //invert the opengl weirdness
633 | int height = image_msg.height - 1;
634 | int linewidth = image_msg.width*4;
635 |
636 | char* temp = new char[linewidth];
637 | for (int y = 0; y < (height+1)/2; y++)
638 | {
639 | memcpy(temp,&image_msg.data[y*linewidth],linewidth);
640 | memcpy(&(image_msg.data[y*linewidth]),&(image_msg.data[(height-y)*linewidth]),linewidth);
641 | memcpy(&(image_msg.data[(height-y)*linewidth]),temp,linewidth);
642 | }
643 |
644 | if (robotmodel->cameramodels.size() > 1)
645 | image_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel));
646 | else
647 | image_msg.header.frame_id = mapName("camera", r,static_cast(robotmodel->positionmodel));
648 | image_msg.header.stamp = sim_time;
649 |
650 | robotmodel->image_pubs[s].publish(image_msg);
651 | }
652 |
653 | //Get latest depth data
654 | //Translate into ROS message format and publish
655 | //Skip if there are no subscribers
656 | if (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth())
657 | {
658 | sensor_msgs::Image depth_msg;
659 | depth_msg.height = cameramodel->getHeight();
660 | depth_msg.width = cameramodel->getWidth();
661 | depth_msg.encoding = this->isDepthCanonical?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
662 | //this->depthMsgs[r].is_bigendian="";
663 | int sz = this->isDepthCanonical?sizeof(float):sizeof(uint16_t);
664 | size_t len = depth_msg.width * depth_msg.height;
665 | depth_msg.step = depth_msg.width * sz;
666 | depth_msg.data.resize(len*sz);
667 |
668 | //processing data according to REP118
669 | if (this->isDepthCanonical){
670 | double nearClip = cameramodel->getCamera().nearClip();
671 | double farClip = cameramodel->getCamera().farClip();
672 | memcpy(&(depth_msg.data[0]),cameramodel->FrameDepth(),len*sz);
673 | float * data = (float*)&(depth_msg.data[0]);
674 | for (size_t i=0;i=farClip)
678 | data[i] = INFINITY;
679 | }
680 | else{
681 | int nearClip = (int)(cameramodel->getCamera().nearClip() * 1000);
682 | int farClip = (int)(cameramodel->getCamera().farClip() * 1000);
683 | for (size_t i=0;iFrameDepth()[i]*1000);
685 | if (v<=nearClip || v>=farClip) v = 0;
686 | ((uint16_t*)&(depth_msg.data[0]))[i] = (uint16_t) ((v<=nearClip || v>=farClip) ? 0 : v );
687 | }
688 | }
689 |
690 | //invert the opengl weirdness
691 | int height = depth_msg.height - 1;
692 | int linewidth = depth_msg.width*sz;
693 |
694 | char* temp = new char[linewidth];
695 | for (int y = 0; y < (height+1)/2; y++)
696 | {
697 | memcpy(temp,&depth_msg.data[y*linewidth],linewidth);
698 | memcpy(&(depth_msg.data[y*linewidth]),&(depth_msg.data[(height-y)*linewidth]),linewidth);
699 | memcpy(&(depth_msg.data[(height-y)*linewidth]),temp,linewidth);
700 | }
701 |
702 | if (robotmodel->cameramodels.size() > 1)
703 | depth_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel));
704 | else
705 | depth_msg.header.frame_id = mapName("camera", r, static_cast(robotmodel->positionmodel));
706 | depth_msg.header.stamp = sim_time;
707 | robotmodel->depth_pubs[s].publish(depth_msg);
708 | }
709 |
710 | //sending camera's tf and info only if image or depth topics are subscribed to
711 | if ((robotmodel->image_pubs[s].getNumSubscribers()>0 && cameramodel->FrameColor())
712 | || (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth()))
713 | {
714 |
715 | Stg::Pose lp = cameramodel->GetPose();
716 | tf::Quaternion Q; Q.setRPY(
717 | (cameramodel->getCamera().pitch()*M_PI/180.0)-M_PI,
718 | 0.0,
719 | lp.a+(cameramodel->getCamera().yaw()*M_PI/180.0) - robotmodel->positionmodel->GetPose().a
720 | );
721 |
722 | tf::Transform tr = tf::Transform(Q, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z+lp.z));
723 |
724 | if (robotmodel->cameramodels.size() > 1)
725 | tf.sendTransform(tf::StampedTransform(tr, sim_time,
726 | mapName("base_link", r, static_cast(robotmodel->positionmodel)),
727 | mapName("camera", r, s, static_cast(robotmodel->positionmodel))));
728 | else
729 | tf.sendTransform(tf::StampedTransform(tr, sim_time,
730 | mapName("base_link", r, static_cast(robotmodel->positionmodel)),
731 | mapName("camera", r, static_cast(robotmodel->positionmodel))));
732 |
733 | sensor_msgs::CameraInfo camera_msg;
734 | if (robotmodel->cameramodels.size() > 1)
735 | camera_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel));
736 | else
737 | camera_msg.header.frame_id = mapName("camera", r, static_cast(robotmodel->positionmodel));
738 | camera_msg.header.stamp = sim_time;
739 | camera_msg.height = cameramodel->getHeight();
740 | camera_msg.width = cameramodel->getWidth();
741 |
742 | double fx,fy,cx,cy;
743 | cx = camera_msg.width / 2.0;
744 | cy = camera_msg.height / 2.0;
745 | double fovh = cameramodel->getCamera().horizFov()*M_PI/180.0;
746 | double fovv = cameramodel->getCamera().vertFov()*M_PI/180.0;
747 | //double fx_ = 1.43266615300557*this->cameramodels[r]->getWidth()/tan(fovh);
748 | //double fy_ = 1.43266615300557*this->cameramodels[r]->getHeight()/tan(fovv);
749 | fx = cameramodel->getWidth()/(2*tan(fovh/2));
750 | fy = cameramodel->getHeight()/(2*tan(fovv/2));
751 |
752 | //ROS_INFO("fx=%.4f,%.4f; fy=%.4f,%.4f", fx, fx_, fy, fy_);
753 |
754 |
755 | camera_msg.D.resize(4, 0.0);
756 |
757 | camera_msg.K[0] = fx;
758 | camera_msg.K[2] = cx;
759 | camera_msg.K[4] = fy;
760 | camera_msg.K[5] = cy;
761 | camera_msg.K[8] = 1.0;
762 |
763 | camera_msg.R[0] = 1.0;
764 | camera_msg.R[4] = 1.0;
765 | camera_msg.R[8] = 1.0;
766 |
767 | camera_msg.P[0] = fx;
768 | camera_msg.P[2] = cx;
769 | camera_msg.P[5] = fy;
770 | camera_msg.P[6] = cy;
771 | camera_msg.P[10] = 1.0;
772 |
773 | robotmodel->camera_pubs[s].publish(camera_msg);
774 |
775 | }
776 |
777 | }
778 | }
779 |
780 | this->base_last_globalpos_time = this->sim_time;
781 | rosgraph_msgs::Clock clock_msg;
782 | clock_msg.clock = sim_time;
783 | this->clock_pub_.publish(clock_msg);
784 | }
785 |
786 | int
787 | main(int argc, char** argv)
788 | {
789 | if( argc < 2 )
790 | {
791 | puts(USAGE);
792 | exit(-1);
793 | }
794 |
795 | ros::init(argc, argv, "stageros");
796 |
797 | bool gui = true;
798 | bool use_model_names = false;
799 | for(int i=0;i<(argc-1);i++)
800 | {
801 | if(!strcmp(argv[i], "-g"))
802 | gui = false;
803 | if(!strcmp(argv[i], "-u"))
804 | use_model_names = true;
805 | }
806 |
807 | StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names);
808 |
809 | if(sn.SubscribeModels() != 0)
810 | exit(-1);
811 |
812 | boost::thread t = boost::thread(boost::bind(&ros::spin));
813 |
814 | // New in Stage 4.1.1: must Start() the world.
815 | sn.world->Start();
816 |
817 | // TODO: get rid of this fixed-duration sleep, using some Stage builtin
818 | // PauseUntilNextUpdate() functionality.
819 | ros::WallRate r(10.0);
820 | while(ros::ok() && !sn.world->TestQuit())
821 | {
822 | if(gui)
823 | Fl::wait(r.expectedCycleTime().toSec());
824 | else
825 | {
826 | sn.UpdateWorld();
827 | r.sleep();
828 | }
829 | }
830 | t.join();
831 |
832 | exit(0);
833 | }
834 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/test/cmdpose_tests.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python
2 | # Software License Agreement (BSD License)
3 | #
4 | # Copyright (c) 2015, Open Source Robotics Foundation, Inc.
5 | # All rights reserved.
6 | #
7 | # Redistribution and use in source and binary forms, with or without
8 | # modification, are permitted provided that the following conditions
9 | # are met:
10 | #
11 | # * Redistributions of source code must retain the above copyright
12 | # notice, this list of conditions and the following disclaimer.
13 | # * Redistributions in binary form must reproduce the above
14 | # copyright notice, this list of conditions and the following
15 | # disclaimer in the documentation and/or other materials provided
16 | # with the distribution.
17 | # * Neither the name of Willow Garage, Inc. nor the names of its
18 | # contributors may be used to endorse or promote products derived
19 | # from this software without specific prior written permission.
20 | #
21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 | # POSSIBILITY OF SUCH DAMAGE.
33 | #
34 | # Author: Brian Gerkey
35 |
36 | import sys
37 | import threading
38 | import time
39 | import unittest
40 |
41 | from geometry_msgs.msg import Pose, PoseStamped, Twist
42 | from nav_msgs.msg import Odometry
43 | import rospy
44 | import rostest
45 | import tf.transformations
46 |
47 | class TestStageRos(unittest.TestCase):
48 |
49 | def __init__(self, *args):
50 | unittest.TestCase.__init__(self, *args)
51 | rospy.init_node('pose_tester', anonymous=True)
52 |
53 | def _base_pose_ground_truth_sub(self, msg):
54 | self.base_pose_ground_truth = msg
55 |
56 | def _odom_sub(self, msg):
57 | self.odom = msg
58 |
59 | def setUp(self):
60 | self.odom = None
61 | self.base_pose_ground_truth = None
62 | self.done = False
63 | self.odom_sub = rospy.Subscriber('odom', Odometry, self._odom_sub)
64 | self.base_pose_ground_truth_sub = rospy.Subscriber(
65 | 'base_pose_ground_truth', Odometry, self._base_pose_ground_truth_sub)
66 | # Make sure we get base_pose_ground_truth
67 | while self.base_pose_ground_truth is None:
68 | time.sleep(0.1)
69 | # Make sure we get odom and the robot is stopped (not still moving
70 | # from the previous test). We can count on stage to return true zeros.
71 | while (self.odom is None or
72 | self.odom.twist.twist.linear.x != 0.0 or
73 | self.odom.twist.twist.linear.y != 0.0 or
74 | self.odom.twist.twist.linear.z != 0.0 or
75 | self.odom.twist.twist.angular.x != 0.0 or
76 | self.odom.twist.twist.angular.y != 0.0 or
77 | self.odom.twist.twist.angular.z != 0.0):
78 | time.sleep(0.1)
79 |
80 | def _pub_thread(self, pub, msg):
81 | while not self.done:
82 | pub.publish(msg)
83 | time.sleep(0.05)
84 |
85 | # Test that, if we command the robot to drive forward for a while, that it does
86 | # so.
87 | def test_cmdvel_x(self):
88 | odom0 = self.odom
89 | pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
90 | twist = Twist()
91 | twist.linear.x = 1.0
92 | # Make a thread to repeatedly publish (to overcome Stage's watchdog)
93 | t = threading.Thread(target=self._pub_thread, args=[pub, twist])
94 | t.start()
95 | time.sleep(3.0)
96 | odom1 = self.odom
97 | self.done = True
98 | t.join()
99 | # Now we expect the robot's odometric pose to differ in X but nothing
100 | # else
101 | self.assertGreater(odom1.header.stamp, odom0.header.stamp)
102 | self.assertNotAlmostEqual(odom1.pose.pose.position.x, odom0.pose.pose.position.x)
103 | self.assertAlmostEqual(odom1.pose.pose.position.y, odom0.pose.pose.position.y)
104 | self.assertAlmostEqual(odom1.pose.pose.position.z, odom0.pose.pose.position.z)
105 | self.assertAlmostEqual(odom1.pose.pose.orientation.x, odom0.pose.pose.orientation.x)
106 | self.assertAlmostEqual(odom1.pose.pose.orientation.y, odom0.pose.pose.orientation.y)
107 | self.assertAlmostEqual(odom1.pose.pose.orientation.z, odom0.pose.pose.orientation.z)
108 | self.assertAlmostEqual(odom1.pose.pose.orientation.w, odom0.pose.pose.orientation.w)
109 |
110 | # Test that, if we command the robot to turn in place for a while, that it does
111 | # so.
112 | def test_cmdvel_yaw(self):
113 | odom0 = self.odom
114 | pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
115 | twist = Twist()
116 | twist.angular.z = 0.25
117 | # Make a thread to repeatedly publish (to overcome Stage's watchdog)
118 | t = threading.Thread(target=self._pub_thread, args=[pub, twist])
119 | t.start()
120 | time.sleep(3.0)
121 | odom1 = self.odom
122 | self.done = True
123 | t.join()
124 | # Now we expect the robot's odometric pose to differ in yaw (which will
125 | # appear in the quaternion elements z and w) and not elsewhere
126 | self.assertGreater(odom1.header.stamp, odom0.header.stamp)
127 | self.assertAlmostEqual(odom1.pose.pose.position.x, odom0.pose.pose.position.x)
128 | self.assertAlmostEqual(odom1.pose.pose.position.y, odom0.pose.pose.position.y)
129 | self.assertAlmostEqual(odom1.pose.pose.position.z, odom0.pose.pose.position.z)
130 | self.assertAlmostEqual(odom1.pose.pose.orientation.x, odom0.pose.pose.orientation.x)
131 | self.assertAlmostEqual(odom1.pose.pose.orientation.y, odom0.pose.pose.orientation.y)
132 | self.assertNotAlmostEqual(odom1.pose.pose.orientation.z, odom0.pose.pose.orientation.z)
133 | self.assertNotAlmostEqual(odom1.pose.pose.orientation.w, odom0.pose.pose.orientation.w)
134 |
135 | # Test that, if we command the robot to jump to a pose, it does so.
136 | def test_pose(self):
137 | pub = rospy.Publisher('cmd_pose', Pose, queue_size=1)
138 | while pub.get_num_connections() == 0:
139 | time.sleep(0.1)
140 | pose = Pose()
141 | pose.position.x = 42.0
142 | pose.position.y = -42.0
143 | pose.position.z = 142.0
144 | roll = 0.2
145 | pitch = -0.3
146 | yaw = 0.9
147 | q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
148 | pose.orientation.x = q[0]
149 | pose.orientation.y = q[1]
150 | pose.orientation.z = q[2]
151 | pose.orientation.w = q[3]
152 | pub.publish(pose)
153 | time.sleep(3.0)
154 | # Now we expect the robot's ground truth pose to be what we told, except
155 | # for z, roll, and pitch, which should all be zero (Stage is 2-D, after all).
156 | bpgt = self.base_pose_ground_truth
157 | self.assertAlmostEqual(bpgt.pose.pose.position.x, pose.position.x)
158 | self.assertAlmostEqual(bpgt.pose.pose.position.y, pose.position.y)
159 | self.assertEqual(bpgt.pose.pose.position.z, 0.0)
160 | q = [bpgt.pose.pose.orientation.x,
161 | bpgt.pose.pose.orientation.y,
162 | bpgt.pose.pose.orientation.z,
163 | bpgt.pose.pose.orientation.w]
164 | e = tf.transformations.euler_from_quaternion(q)
165 | self.assertEqual(e[0], 0.0)
166 | self.assertEqual(e[1], 0.0)
167 | self.assertAlmostEqual(e[2], yaw)
168 |
169 | # Test that, if we command the robot to jump to a pose (with a header), it does so.
170 | def test_pose_stamped(self):
171 | pub = rospy.Publisher('cmd_pose_stamped', PoseStamped, queue_size=1)
172 | while pub.get_num_connections() == 0:
173 | time.sleep(0.1)
174 | ps = PoseStamped()
175 | ps.header.frame_id = 'ignored_value'
176 | ps.header.stamp = rospy.Time.now()
177 | ps.pose.position.x = -42.0
178 | ps.pose.position.y = 42.0
179 | ps.pose.position.z = -142.0
180 | roll = -0.2
181 | pitch = 0.3
182 | yaw = -0.9
183 | q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
184 | ps.pose.orientation.x = q[0]
185 | ps.pose.orientation.y = q[1]
186 | ps.pose.orientation.z = q[2]
187 | ps.pose.orientation.w = q[3]
188 | pub.publish(ps)
189 | time.sleep(3.0)
190 | # Now we expect the robot's ground truth pose to be what we told, except
191 | # for z, roll, and pitch, which should all be zero (Stage is 2-D, after all).
192 | bpgt = self.base_pose_ground_truth
193 | self.assertAlmostEqual(bpgt.pose.pose.position.x, ps.pose.position.x)
194 | self.assertAlmostEqual(bpgt.pose.pose.position.y, ps.pose.position.y)
195 | self.assertEqual(bpgt.pose.pose.position.z, 0.0)
196 | q = [bpgt.pose.pose.orientation.x,
197 | bpgt.pose.pose.orientation.y,
198 | bpgt.pose.pose.orientation.z,
199 | bpgt.pose.pose.orientation.w]
200 | e = tf.transformations.euler_from_quaternion(q)
201 | self.assertEqual(e[0], 0.0)
202 | self.assertEqual(e[1], 0.0)
203 | self.assertAlmostEqual(e[2], yaw)
204 |
205 | NAME = 'stage_ros'
206 | if __name__ == '__main__':
207 | rostest.unitrun('stage_ros', NAME, TestStageRos, sys.argv)
208 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/test/cmdpose_tests.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/test/hztest.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
14 |
15 |
18 |
19 |
20 |
21 |
22 |
29 |
30 |
31 |
32 |
33 |
34 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/world/Obstacles.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/stage_ros-add_pose_and_crash/world/Obstacles.jpg
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/world/Obstacles.world:
--------------------------------------------------------------------------------
1 | define block model
2 | (
3 | size [0.5 0.5 0.5]
4 | gui_nose 0
5 | )
6 |
7 | define topurg ranger
8 | (
9 | sensor(
10 | range [ 0.02 5.60 ]
11 | fov 240.00
12 | samples 660
13 | )
14 | # generic model properties
15 | color "black"
16 | size [ 0.05 0.05 0.1 ]
17 | )
18 |
19 | define pr2 position
20 | (
21 | size [0.455 0.381 0.237]
22 | origin [0 0 0 0]
23 | gui_nose 1
24 | drive "omni"
25 | topurg(pose [ 0.1 0 0.287 0 ])
26 | velocity_bounds [-100 100 -100 100 -100 100 -90 90 ]
27 | acceleration_bounds [-1 1 -1 1 -1 1 -90 90]
28 |
29 | )
30 |
31 | define floorplan model
32 | (
33 | # sombre, sensible, artistic
34 | color "gray30"
35 |
36 | # most maps will need a bounding box
37 | boundary 1
38 |
39 | gui_nose 0
40 | gui_grid 0
41 |
42 | gui_outline 0
43 | gripper_return 0
44 | fiducial_return 0
45 | ranger_return 1
46 | )
47 |
48 | # set the resolution of the underlying raytrace model in meters
49 | resolution 0.02
50 |
51 | interval_sim 100 # simulation timestep in milliseconds
52 | speedup 3
53 |
54 | window
55 | (
56 | size [ 745.000 448.000 ]
57 |
58 | rotate [ 0.000 0.000 ]
59 | scale 28.806
60 | )
61 |
62 | # load an environment bitmap
63 | floorplan
64 | (
65 | name "willow"
66 | bitmap "./Obstacles.jpg"
67 | size [8.0 8.0 1.0]
68 | pose [ 0.000 0.000 0 00.000 ]
69 | )
70 |
71 | # throw in a robot
72 | pr2( pose [ 0.000 0.000 0 00.000 ] name "pr2" color "blue")
73 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/world/rink.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/stage_ros-add_pose_and_crash/world/rink.png
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/world/simple.world:
--------------------------------------------------------------------------------
1 | show_clock 0
2 | show_clock_interval 10000
3 | resolution 0.2
4 | threads 4
5 | speedup 1
6 |
7 |
8 | define sicklaser ranger
9 | (
10 | sensor(
11 | pose [ 0 0 0.1 0 ]
12 | fov 180
13 | range [ 0.0 6.0 ]
14 | samples 512
15 | )
16 | color "random"
17 | block(
18 | points 4
19 | point[0] [0 0]
20 | point[1] [0 1]
21 | point[2] [1 1]
22 | point[3] [1 0]
23 | z [0 0.21]
24 | )
25 | )
26 |
27 |
28 | define floorplan model
29 | (
30 | color "gray30"
31 | boundary 1
32 |
33 | gui_nose 0
34 | gui_grid 0
35 | gui_move 1
36 | gui_outline 0
37 | gripper_return 0
38 | fiducial_return 0
39 | ranger_return 1
40 | obstacle_return 1
41 | )
42 |
43 | floorplan
44 | (
45 | name "blank"
46 | size [20.000 20.000 0.800]
47 | pose [0.000 0.000 0.000 0.000]
48 | bitmap "rink.png"
49 | )
50 |
51 |
52 |
53 | window
54 | (
55 | size [1550 1550]
56 |
57 | # Camera options
58 | scale 35
59 | center [0 0]
60 | rotate [ 0.000 0.000 ]
61 |
62 |
63 | # GUI options
64 | show_data 1
65 | show_flags 1
66 | show_blocks 1
67 | show_clock 1
68 | show_footprints 1
69 | show_grid 1
70 | show_trailarrows 0
71 | show_trailrise 0
72 | show_trailfast 0
73 | show_occupancy 0
74 |
75 | )
76 |
77 |
78 |
79 |
80 | define agent position
81 | (
82 | # actual size
83 | size [0.44 0.38 0.22] # sizes from MobileRobots' web site
84 |
85 | localization "gps"
86 | # the pioneer's center of rotation is offset from its center of area
87 | origin [0 0 0 0]
88 |
89 | # draw a nose on the robot so we can see which way it points
90 | gui_nose 1
91 |
92 | color "random"
93 | drive "diff" # Differential steering model.
94 | obstacle_return 1 # Can hit things.
95 | ranger_return 0.5 # reflects sonar beams
96 | blob_return 1 # Seen by blobfinders
97 | fiducial_return 1 # Seen as "1" fiducial finders
98 | sicklaser(
99 | pose [ 0 0 0 0 ]
100 | )
101 |
102 | # [ xmin xmax ymin ymax zmin zmax amin amax ]
103 | # velocity_bounds [-0.5 0.5 0 0 0 0 -90.0 90.0 ]
104 | # acceleration_bounds [-0.5 0.5 0 0 0 0 -90 90.0 ]
105 | )
106 |
107 | agent( pose [8.00 0.00 0.00 180.00])
108 | agent( pose [7.73 2.07 0.00 195.00])
109 | agent( pose [6.93 4.00 0.00 210.00])
110 | agent( pose [5.66 5.66 0.00 225.00])
111 | agent( pose [4.00 6.93 0.00 240.00])
112 | agent( pose [2.07 7.73 0.00 255.00])
113 | agent( pose [-0.00 8.00 0.00 270.00])
114 | agent( pose [-2.07 7.73 0.00 285.00])
115 | agent( pose [-4.00 6.93 0.00 300.00])
116 | agent( pose [-5.66 5.66 0.00 315.00])
117 | agent( pose [-6.93 4.00 0.00 330.00])
118 | agent( pose [-7.73 2.07 0.00 345.00])
119 | agent( pose [-8.00 -0.00 0.00 360.00])
120 | agent( pose [-7.73 -2.07 0.00 375.00])
121 | agent( pose [-6.93 -4.00 0.00 390.00])
122 | agent( pose [-5.66 -5.66 0.00 405.00])
123 | agent( pose [-4.00 -6.93 0.00 420.00])
124 | agent( pose [-2.07 -7.73 0.00 435.00])
125 | agent( pose [0.00 -8.00 0.00 450.00])
126 | agent( pose [2.07 -7.73 0.00 465.00])
127 | agent( pose [4.00 -6.93 0.00 480.00])
128 | agent( pose [5.66 -5.66 0.00 495.00])
129 | agent( pose [6.93 -4.00 0.00 510.00])
130 | agent( pose [7.73 -2.07 0.00 525.00])
131 |
132 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/world/willow-erratic.world:
--------------------------------------------------------------------------------
1 | define block model
2 | (
3 | size [0.5 0.5 0.5]
4 | gui_nose 0
5 | )
6 |
7 | define topurg ranger
8 | (
9 | sensor(
10 | range [ 0.0 30.0 ]
11 | fov 270.25
12 | samples 1081
13 | )
14 |
15 | # generic model properties
16 | color "black"
17 | size [ 0.05 0.05 0.1 ]
18 | )
19 |
20 | define erratic position
21 | (
22 | #size [0.415 0.392 0.25]
23 | size [0.35 0.35 0.25]
24 | origin [-0.05 0 0 0]
25 | gui_nose 1
26 | drive "diff"
27 | topurg(pose [ 0.050 0.000 0 0.000 ])
28 | )
29 |
30 | define floorplan model
31 | (
32 | # sombre, sensible, artistic
33 | color "gray30"
34 |
35 | # most maps will need a bounding box
36 | boundary 1
37 |
38 | gui_nose 0
39 | gui_grid 0
40 |
41 | gui_outline 0
42 | gripper_return 0
43 | fiducial_return 0
44 | laser_return 1
45 | )
46 |
47 | # set the resolution of the underlying raytrace model in meters
48 | resolution 0.02
49 |
50 | interval_sim 100 # simulation timestep in milliseconds
51 |
52 |
53 | window
54 | (
55 | size [ 745.000 448.000 ]
56 |
57 | rotate [ 0.000 -1.560 ]
58 | scale 28.806
59 | )
60 |
61 | # load an environment bitmap
62 | floorplan
63 | (
64 | name "willow"
65 | bitmap "willow-full.pgm"
66 | size [54.0 58.7 0.5]
67 | pose [ -29.350 27.000 0 90.000 ]
68 | )
69 |
70 | # throw in a robot
71 | erratic( pose [ -11.277 23.266 0 180.000 ] name "era" color "blue")
72 | block( pose [ -13.924 25.020 0 180.000 ] color "red")
73 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/world/willow-four-erratics-multisensor.world:
--------------------------------------------------------------------------------
1 | define block model
2 | (
3 | size [0.5 0.5 0.5]
4 | gui_nose 0
5 | )
6 |
7 | define topurg ranger
8 | (
9 | sensor(
10 | range [ 0.0 30.0 ]
11 | fov 270.25
12 | samples 1081
13 | )
14 |
15 | # generic model properties
16 | color "black"
17 | size [ 0.05 0.05 0.1 ]
18 | )
19 |
20 | define mycamera camera
21 | (
22 | range [ 0.2 8.0 ]
23 | resolution [ 100 100 ]
24 | fov [ 70 40 ]
25 | pantilt [ 0 0 ]
26 | alwayson 1
27 | )
28 |
29 | define erratic position
30 | (
31 | #size [0.415 0.392 0.25]
32 | size [0.35 0.35 0.25]
33 | origin [-0.05 0 0 0]
34 | gui_nose 1
35 | drive "diff"
36 | topurg(pose [ 0.050 0.000 0 0.000 ])
37 | topurg(pose [ -0.050 0.000 0 180.000 ])
38 | mycamera(pose [ 0 0 0 90.0 ])
39 | mycamera(pose [ 0 0 0 -90.0 ])
40 | )
41 |
42 | define floorplan model
43 | (
44 | # sombre, sensible, artistic
45 | color "gray30"
46 |
47 | # most maps will need a bounding box
48 | boundary 1
49 |
50 | gui_nose 0
51 | gui_grid 0
52 |
53 | gui_outline 0
54 | gripper_return 0
55 | fiducial_return 0
56 | laser_return 1
57 | )
58 |
59 | # set the resolution of the underlying raytrace model in meters
60 | resolution 0.02
61 |
62 | interval_sim 100 # simulation timestep in milliseconds
63 |
64 |
65 | window
66 | (
67 | size [ 745.000 448.000 ]
68 |
69 | rotate [ 0.000 -1.560 ]
70 | scale 28.806
71 | )
72 |
73 | # load an environment bitmap
74 | floorplan
75 | (
76 | name "willow"
77 | bitmap "willow-full.pgm"
78 | size [54.0 58.7 0.5]
79 | pose [ -29.350 27.000 0 90.000 ]
80 | )
81 |
82 | # throw in two robots
83 | erratic( pose [ -11.277 23.266 0 180.000 ] name "era" color "blue")
84 | erratic( pose [ -13.277 22.266 0 180.000 ] name "era2" color "blue")
85 | erratic( pose [ -13.277 23.266 0 180.000 ] name "era3" color "blue")
86 | erratic( pose [ -11.277 22.266 0 180.000 ] name "era4" color "blue")
87 | block( pose [ -13.924 25.020 0 180.000 ] color "red")
88 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/world/willow-four-erratics.world:
--------------------------------------------------------------------------------
1 | define block model
2 | (
3 | size [0.5 0.5 0.5]
4 | gui_nose 0
5 | )
6 |
7 | define topurg ranger
8 | (
9 | sensor(
10 | range [ 0.0 30.0 ]
11 | fov 270.25
12 | samples 1081
13 | )
14 |
15 | # generic model properties
16 | color "black"
17 | size [ 0.05 0.05 0.1 ]
18 | )
19 |
20 | define erratic position
21 | (
22 | #size [0.415 0.392 0.25]
23 | size [0.35 0.35 0.25]
24 | origin [-0.05 0 0 0]
25 | gui_nose 1
26 | drive "diff"
27 | topurg(pose [ 0.050 0.000 0 0.000 ])
28 | )
29 |
30 | define floorplan model
31 | (
32 | # sombre, sensible, artistic
33 | color "gray30"
34 |
35 | # most maps will need a bounding box
36 | boundary 1
37 |
38 | gui_nose 0
39 | gui_grid 0
40 |
41 | gui_outline 0
42 | gripper_return 0
43 | fiducial_return 0
44 | laser_return 1
45 | )
46 |
47 | # set the resolution of the underlying raytrace model in meters
48 | resolution 0.02
49 |
50 | interval_sim 100 # simulation timestep in milliseconds
51 |
52 |
53 | window
54 | (
55 | size [ 745.000 448.000 ]
56 |
57 | rotate [ 0.000 -1.560 ]
58 | scale 28.806
59 | )
60 |
61 | # load an environment bitmap
62 | floorplan
63 | (
64 | name "willow"
65 | bitmap "willow-full.pgm"
66 | size [54.0 58.7 0.5]
67 | pose [ -29.350 27.000 0 90.000 ]
68 | )
69 |
70 | # throw in two robots
71 | erratic( pose [ -11.277 23.266 0 180.000 ] name "era" color "blue")
72 | erratic( pose [ -13.277 22.266 0 180.000 ] name "era2" color "blue")
73 | erratic( pose [ -13.277 23.266 0 180.000 ] name "era3" color "blue")
74 | erratic( pose [ -11.277 22.266 0 180.000 ] name "era4" color "blue")
75 | block( pose [ -13.924 25.020 0 180.000 ] color "red")
76 |
--------------------------------------------------------------------------------
/stage_ros-add_pose_and_crash/world/willow-full.pgm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/stage_ros-add_pose_and_crash/world/willow-full.pgm
--------------------------------------------------------------------------------
/stage_world1.py:
--------------------------------------------------------------------------------
1 | import time
2 | import rospy
3 | import copy
4 | import tf
5 | import numpy as np
6 |
7 |
8 | from geometry_msgs.msg import Twist, Pose
9 | from nav_msgs.msg import Odometry
10 | from sensor_msgs.msg import LaserScan
11 | from rosgraph_msgs.msg import Clock
12 | from std_srvs.srv import Empty
13 | from std_msgs.msg import Int8
14 |
15 |
16 | class StageWorld():
17 | def __init__(self, beam_num, index, num_env):
18 | self.index = index
19 | self.num_env = num_env
20 | node_name = 'StageEnv_' + str(index)
21 | rospy.init_node(node_name, anonymous=None)
22 |
23 | self.beam_mum = beam_num
24 | self.laser_cb_num = 0
25 | self.scan = None
26 |
27 | # used in reset_world
28 | self.self_speed = [0.0, 0.0]
29 | self.step_goal = [0., 0.]
30 | self.step_r_cnt = 0.
31 |
32 | # used in generate goal point
33 | self.map_size = np.array([8., 8.], dtype=np.float32) # 20x20m
34 | self.goal_size = 0.5
35 |
36 | self.robot_value = 10.
37 | self.goal_value = 0.
38 | # self.reset_pose = None
39 |
40 | self.init_pose = None
41 |
42 |
43 |
44 | # for get reward and terminate
45 | self.stop_counter = 0
46 |
47 | # -----------Publisher and Subscriber-------------
48 | cmd_vel_topic = 'robot_' + str(index) + '/cmd_vel'
49 | self.cmd_vel = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
50 |
51 | cmd_pose_topic = 'robot_' + str(index) + '/cmd_pose'
52 | self.cmd_pose = rospy.Publisher(cmd_pose_topic, Pose, queue_size=2)
53 |
54 | object_state_topic = 'robot_' + str(index) + '/base_pose_ground_truth'
55 | self.object_state_sub = rospy.Subscriber(object_state_topic, Odometry, self.ground_truth_callback)
56 |
57 | laser_topic = 'robot_' + str(index) + '/base_scan'
58 |
59 | self.laser_sub = rospy.Subscriber(laser_topic, LaserScan, self.laser_scan_callback)
60 |
61 | odom_topic = 'robot_' + str(index) + '/odom'
62 | self.odom_sub = rospy.Subscriber(odom_topic, Odometry, self.odometry_callback)
63 |
64 | crash_topic = 'robot_' + str(index) + '/is_crashed'
65 | self.check_crash = rospy.Subscriber(crash_topic, Int8, self.crash_callback)
66 |
67 |
68 | self.sim_clock = rospy.Subscriber('clock', Clock, self.sim_clock_callback)
69 |
70 | # -----------Service-------------------
71 | self.reset_stage = rospy.ServiceProxy('reset_positions', Empty)
72 |
73 |
74 | # # Wait until the first callback
75 | self.speed = None
76 | self.state = None
77 | self.speed_GT = None
78 | self.state_GT = None
79 | while self.scan is None or self.speed is None or self.state is None\
80 | or self.speed_GT is None or self.state_GT is None:
81 | pass
82 |
83 | rospy.sleep(1.)
84 | # # What function to call when you ctrl + c
85 | # rospy.on_shutdown(self.shutdown)
86 |
87 |
88 | def ground_truth_callback(self, GT_odometry):
89 | Quaternious = GT_odometry.pose.pose.orientation
90 | Euler = tf.transformations.euler_from_quaternion([Quaternious.x, Quaternious.y, Quaternious.z, Quaternious.w])
91 | self.state_GT = [GT_odometry.pose.pose.position.x, GT_odometry.pose.pose.position.y, Euler[2]]
92 | v_x = GT_odometry.twist.twist.linear.x
93 | v_y = GT_odometry.twist.twist.linear.y
94 | v = np.sqrt(v_x**2 + v_y**2)
95 | self.speed_GT = [v, GT_odometry.twist.twist.angular.z]
96 |
97 | def laser_scan_callback(self, scan):
98 | self.scan_param = [scan.angle_min, scan.angle_max, scan.angle_increment, scan.time_increment,
99 | scan.scan_time, scan.range_min, scan.range_max]
100 | self.scan = np.array(scan.ranges)
101 | self.laser_cb_num += 1
102 |
103 |
104 | def odometry_callback(self, odometry):
105 | Quaternions = odometry.pose.pose.orientation
106 | Euler = tf.transformations.euler_from_quaternion([Quaternions.x, Quaternions.y, Quaternions.z, Quaternions.w])
107 | self.state = [odometry.pose.pose.position.x, odometry.pose.pose.position.y, Euler[2]]
108 | self.speed = [odometry.twist.twist.linear.x, odometry.twist.twist.angular.z]
109 |
110 | def sim_clock_callback(self, clock):
111 | self.sim_time = clock.clock.secs + clock.clock.nsecs / 1000000000.
112 |
113 | def crash_callback(self, flag):
114 | self.is_crashed = flag.data
115 |
116 | def get_self_stateGT(self):
117 | return self.state_GT
118 |
119 | def get_self_speedGT(self):
120 | return self.speed_GT
121 |
122 | def get_laser_observation(self):
123 | scan = copy.deepcopy(self.scan)
124 | scan[np.isnan(scan)] = 6.0
125 | scan[np.isinf(scan)] = 6.0
126 | raw_beam_num = len(scan)
127 | sparse_beam_num = self.beam_mum
128 | step = float(raw_beam_num) / sparse_beam_num
129 | sparse_scan_left = []
130 | index = 0.
131 | for x in xrange(int(sparse_beam_num / 2)):
132 | sparse_scan_left.append(scan[int(index)])
133 | index += step
134 | sparse_scan_right = []
135 | index = raw_beam_num - 1.
136 | for x in xrange(int(sparse_beam_num / 2)):
137 | sparse_scan_right.append(scan[int(index)])
138 | index -= step
139 | scan_sparse = np.concatenate((sparse_scan_left, sparse_scan_right[::-1]), axis=0)
140 | return scan_sparse / 6.0 - 0.5
141 |
142 |
143 | def get_self_speed(self):
144 | return self.speed
145 |
146 | def get_self_state(self):
147 | return self.state
148 |
149 | def get_crash_state(self):
150 | return self.is_crashed
151 |
152 | def get_sim_time(self):
153 | return self.sim_time
154 |
155 | def get_local_goal(self):
156 | [x, y, theta] = self.get_self_stateGT()
157 | [goal_x, goal_y] = self.goal_point
158 | local_x = (goal_x - x) * np.cos(theta) + (goal_y - y) * np.sin(theta)
159 | local_y = -(goal_x - x) * np.sin(theta) + (goal_y - y) * np.cos(theta)
160 | return [local_x, local_y]
161 |
162 | def reset_world(self):
163 | self.reset_stage()
164 | self.self_speed = [0.0, 0.0]
165 | self.step_goal = [0., 0.]
166 | self.step_r_cnt = 0.
167 | self.start_time = time.time()
168 | rospy.sleep(0.5)
169 |
170 |
171 | def generate_goal_point(self):
172 | [x_g, y_g] = self.generate_random_goal()
173 | self.goal_point = [x_g, y_g]
174 | [x, y] = self.get_local_goal()
175 |
176 | self.pre_distance = np.sqrt(x ** 2 + y ** 2)
177 | self.distance = copy.deepcopy(self.pre_distance)
178 |
179 |
180 | def get_reward_and_terminate(self, t):
181 | terminate = False
182 | laser_scan = self.get_laser_observation()
183 | [x, y, theta] = self.get_self_stateGT()
184 | [v, w] = self.get_self_speedGT()
185 | self.pre_distance = copy.deepcopy(self.distance)
186 | self.distance = np.sqrt((self.goal_point[0] - x) ** 2 + (self.goal_point[1] - y) ** 2)
187 | reward_g = (self.pre_distance - self.distance) * 2.5
188 | reward_c = 0
189 | reward_w = 0
190 | result = 0
191 | is_crash = self.get_crash_state()
192 |
193 | if self.distance < self.goal_size:
194 | terminate = True
195 | reward_g = 15
196 | result = 'Reach Goal'
197 |
198 | if is_crash == 1:
199 | terminate = True
200 | reward_c = -15.
201 | result = 'Crashed'
202 |
203 | if np.abs(w) > 1.05:
204 | reward_w = -0.1 * np.abs(w)
205 |
206 | if t > 150:
207 | terminate = True
208 | result = 'Time out'
209 | reward = reward_g + reward_c + reward_w
210 |
211 | return reward, terminate, result
212 |
213 | def reset_pose(self):
214 | random_pose = self.generate_random_pose()
215 | rospy.sleep(0.01)
216 | self.control_pose(random_pose)
217 | [x_robot, y_robot, theta] = self.get_self_stateGT()
218 |
219 | # start_time = time.time()
220 | while np.abs(random_pose[0] - x_robot) > 0.2 or np.abs(random_pose[1] - y_robot) > 0.2:
221 | [x_robot, y_robot, theta] = self.get_self_stateGT()
222 | self.control_pose(random_pose)
223 | rospy.sleep(0.01)
224 |
225 |
226 | def control_vel(self, action):
227 | move_cmd = Twist()
228 | move_cmd.linear.x = action[0]
229 | move_cmd.linear.y = 0.
230 | move_cmd.linear.z = 0.
231 | move_cmd.angular.x = 0.
232 | move_cmd.angular.y = 0.
233 | move_cmd.angular.z = action[1]
234 | self.cmd_vel.publish(move_cmd)
235 |
236 |
237 | def control_pose(self, pose):
238 | pose_cmd = Pose()
239 | assert len(pose)==3
240 | pose_cmd.position.x = pose[0]
241 | pose_cmd.position.y = pose[1]
242 | pose_cmd.position.z = 0
243 |
244 | qtn = tf.transformations.quaternion_from_euler(0, 0, pose[2], 'rxyz')
245 | pose_cmd.orientation.x = qtn[0]
246 | pose_cmd.orientation.y = qtn[1]
247 | pose_cmd.orientation.z = qtn[2]
248 | pose_cmd.orientation.w = qtn[3]
249 | self.cmd_pose.publish(pose_cmd)
250 |
251 | def generate_random_pose(self):
252 | x = np.random.uniform(-9, 9)
253 | y = np.random.uniform(-9, 9)
254 | dis = np.sqrt(x ** 2 + y ** 2)
255 | while (dis > 9) and not rospy.is_shutdown():
256 | x = np.random.uniform(-9, 9)
257 | y = np.random.uniform(-9, 9)
258 | dis = np.sqrt(x ** 2 + y ** 2)
259 | theta = np.random.uniform(0, 2 * np.pi)
260 | return [x, y, theta]
261 |
262 | def generate_random_goal(self):
263 | self.init_pose = self.get_self_stateGT()
264 | x = np.random.uniform(-9, 9)
265 | y = np.random.uniform(-9, 9)
266 | dis_origin = np.sqrt(x ** 2 + y ** 2)
267 | dis_goal = np.sqrt((x - self.init_pose[0]) ** 2 + (y - self.init_pose[1]) ** 2)
268 | while (dis_origin > 9 or dis_goal > 10 or dis_goal < 8) and not rospy.is_shutdown():
269 | x = np.random.uniform(-9, 9)
270 | y = np.random.uniform(-9, 9)
271 | dis_origin = np.sqrt(x ** 2 + y ** 2)
272 | dis_goal = np.sqrt((x - self.init_pose[0]) ** 2 + (y - self.init_pose[1]) ** 2)
273 |
274 | return [x, y]
275 |
276 |
277 |
278 |
279 |
280 |
--------------------------------------------------------------------------------
/stage_world2.py:
--------------------------------------------------------------------------------
1 | import time
2 | import rospy
3 | import copy
4 | import tf
5 | import numpy as np
6 |
7 | from geometry_msgs.msg import Twist, Pose
8 | from nav_msgs.msg import Odometry
9 | from sensor_msgs.msg import LaserScan
10 | from rosgraph_msgs.msg import Clock
11 | from std_srvs.srv import Empty
12 | from std_msgs.msg import Int8
13 | from model.utils import get_init_pose, get_goal_point
14 |
15 |
16 | class StageWorld():
17 | def __init__(self, beam_num, index, num_env):
18 | self.index = index
19 | self.num_env = num_env
20 | node_name = 'StageEnv_' + str(index)
21 | rospy.init_node(node_name, anonymous=None)
22 |
23 | self.beam_mum = beam_num
24 | self.laser_cb_num = 0
25 | self.scan = None
26 |
27 | # used in reset_world
28 | self.self_speed = [0.0, 0.0]
29 | self.step_goal = [0., 0.]
30 | self.step_r_cnt = 0.
31 |
32 | # used in generate goal point
33 | self.map_size = np.array([8., 8.], dtype=np.float32) # 20x20m
34 | self.goal_size = 0.5
35 |
36 | self.robot_value = 10.
37 | self.goal_value = 0.
38 |
39 |
40 | # -----------Publisher and Subscriber-------------
41 | cmd_vel_topic = 'robot_' + str(index) + '/cmd_vel'
42 | self.cmd_vel = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
43 |
44 | cmd_pose_topic = 'robot_' + str(index) + '/cmd_pose'
45 | self.cmd_pose = rospy.Publisher(cmd_pose_topic, Pose, queue_size=10)
46 |
47 | object_state_topic = 'robot_' + str(index) + '/base_pose_ground_truth'
48 | self.object_state_sub = rospy.Subscriber(object_state_topic, Odometry, self.ground_truth_callback)
49 |
50 | laser_topic = 'robot_' + str(index) + '/base_scan'
51 |
52 | self.laser_sub = rospy.Subscriber(laser_topic, LaserScan, self.laser_scan_callback)
53 |
54 | odom_topic = 'robot_' + str(index) + '/odom'
55 | self.odom_sub = rospy.Subscriber(odom_topic, Odometry, self.odometry_callback)
56 |
57 | crash_topic = 'robot_' + str(index) + '/is_crashed'
58 | self.check_crash = rospy.Subscriber(crash_topic, Int8, self.crash_callback)
59 |
60 |
61 | self.sim_clock = rospy.Subscriber('clock', Clock, self.sim_clock_callback)
62 |
63 | # -----------Service-------------------
64 | self.reset_stage = rospy.ServiceProxy('reset_positions', Empty)
65 |
66 | # # Wait until the first callback
67 | self.speed = None
68 | self.state = None
69 | self.speed_GT = None
70 | self.state_GT = None
71 | self.is_crashed = None
72 | while self.scan is None or self.speed is None or self.state is None\
73 | or self.speed_GT is None or self.state_GT is None or self.is_crashed is None:
74 | pass
75 |
76 | rospy.sleep(1.)
77 | # # What function to call when you ctrl + c
78 | # rospy.on_shutdown(self.shutdown)
79 |
80 |
81 | def ground_truth_callback(self, GT_odometry):
82 | Quaternious = GT_odometry.pose.pose.orientation
83 | Euler = tf.transformations.euler_from_quaternion([Quaternious.x, Quaternious.y, Quaternious.z, Quaternious.w])
84 | self.state_GT = [GT_odometry.pose.pose.position.x, GT_odometry.pose.pose.position.y, Euler[2]]
85 | v_x = GT_odometry.twist.twist.linear.x
86 | v_y = GT_odometry.twist.twist.linear.y
87 | v = np.sqrt(v_x**2 + v_y**2)
88 | self.speed_GT = [v, GT_odometry.twist.twist.angular.z]
89 |
90 | def laser_scan_callback(self, scan):
91 | self.scan_param = [scan.angle_min, scan.angle_max, scan.angle_increment, scan.time_increment,
92 | scan.scan_time, scan.range_min, scan.range_max]
93 | self.scan = np.array(scan.ranges)
94 | self.laser_cb_num += 1
95 |
96 |
97 | def odometry_callback(self, odometry):
98 | Quaternions = odometry.pose.pose.orientation
99 | Euler = tf.transformations.euler_from_quaternion([Quaternions.x, Quaternions.y, Quaternions.z, Quaternions.w])
100 | self.state = [odometry.pose.pose.position.x, odometry.pose.pose.position.y, Euler[2]]
101 | self.speed = [odometry.twist.twist.linear.x, odometry.twist.twist.angular.z]
102 |
103 | def sim_clock_callback(self, clock):
104 | self.sim_time = clock.clock.secs + clock.clock.nsecs / 1000000000.
105 |
106 | def crash_callback(self, flag):
107 | self.is_crashed = flag.data
108 |
109 | def get_self_stateGT(self):
110 | return self.state_GT
111 |
112 | def get_self_speedGT(self):
113 | return self.speed_GT
114 |
115 | def get_laser_observation(self):
116 | scan = copy.deepcopy(self.scan)
117 | scan[np.isnan(scan)] = 6.0
118 | scan[np.isinf(scan)] = 6.0
119 | raw_beam_num = len(scan)
120 | sparse_beam_num = self.beam_mum
121 | step = float(raw_beam_num) / sparse_beam_num
122 | sparse_scan_left = []
123 | index = 0.
124 | for x in xrange(int(sparse_beam_num / 2)):
125 | sparse_scan_left.append(scan[int(index)])
126 | index += step
127 | sparse_scan_right = []
128 | index = raw_beam_num - 1.
129 | for x in xrange(int(sparse_beam_num / 2)):
130 | sparse_scan_right.append(scan[int(index)])
131 | index -= step
132 | scan_sparse = np.concatenate((sparse_scan_left, sparse_scan_right[::-1]), axis=0)
133 | return scan_sparse / 6.0 - 0.5
134 |
135 |
136 | def get_self_speed(self):
137 | return self.speed
138 |
139 | def get_self_state(self):
140 | return self.state
141 |
142 | def get_crash_state(self):
143 | return self.is_crashed
144 |
145 | def get_sim_time(self):
146 | return self.sim_time
147 |
148 | def get_local_goal(self):
149 | [x, y, theta] = self.get_self_stateGT()
150 | [goal_x, goal_y] = self.goal_point
151 | local_x = (goal_x - x) * np.cos(theta) + (goal_y - y) * np.sin(theta)
152 | local_y = -(goal_x - x) * np.sin(theta) + (goal_y - y) * np.cos(theta)
153 | return [local_x, local_y]
154 |
155 | def reset_world(self):
156 | self.reset_stage()
157 | self.self_speed = [0.0, 0.0]
158 | self.step_goal = [0., 0.]
159 | self.step_r_cnt = 0.
160 | self.start_time = time.time()
161 | rospy.sleep(0.5)
162 |
163 |
164 | def generate_goal_point(self):
165 | if self.index > 33 and self.index < 44:
166 | self.goal_point = self.generate_random_goal()
167 | else:
168 | self.goal_point = get_goal_point(self.index)
169 |
170 | self.pre_distance = 0
171 | self.distance = copy.deepcopy(self.pre_distance)
172 |
173 |
174 |
175 | def get_reward_and_terminate(self, t):
176 | terminate = False
177 | laser_scan = self.get_laser_observation()
178 | laser_min = np.amin(laser_scan)
179 | [x, y, theta] = self.get_self_stateGT()
180 | [v, w] = self.get_self_speedGT()
181 | self.pre_distance = copy.deepcopy(self.distance)
182 | self.distance = np.sqrt((self.goal_point[0] - x) ** 2 + (self.goal_point[1] - y) ** 2)
183 | reward_g = (self.pre_distance - self.distance) * 2.5
184 | reward_c = 0
185 | reward_w = 0
186 | result = 0
187 |
188 | is_crash = self.get_crash_state()
189 |
190 | if self.distance < self.goal_size:
191 | terminate = True
192 | reward_g = 15
193 | result = 'Reach Goal'
194 |
195 | if is_crash == 1:
196 | terminate = True
197 | reward_c = -15.
198 | result = 'Crashed'
199 |
200 | if np.abs(w) > 1.05:
201 | reward_w = -0.1 * np.abs(w)
202 |
203 | if t > 200:
204 | terminate = True
205 | result = 'Time out'
206 | reward = reward_g + reward_c + reward_w
207 |
208 | return reward, terminate, result
209 |
210 | def reset_pose(self):
211 | if self.index > 33 and self.index < 44:
212 | reset_pose = self.generate_random_pose()
213 | else:
214 | reset_pose = get_init_pose(self.index)
215 | rospy.sleep(0.05)
216 | self.control_pose(reset_pose)
217 | [x_robot, y_robot, theta] = self.get_self_stateGT()
218 |
219 | while np.abs(reset_pose[0] - x_robot) > 0.2 or np.abs(reset_pose[1] - y_robot) > 0.2:
220 | [x_robot, y_robot, theta] = self.get_self_stateGT()
221 | rospy.sleep(0.05)
222 |
223 |
224 | def control_vel(self, action):
225 | move_cmd = Twist()
226 | move_cmd.linear.x = action[0]
227 | move_cmd.linear.y = 0.
228 | move_cmd.linear.z = 0.
229 | move_cmd.angular.x = 0.
230 | move_cmd.angular.y = 0.
231 | move_cmd.angular.z = action[1]
232 | self.cmd_vel.publish(move_cmd)
233 |
234 |
235 | def control_pose(self, pose):
236 | pose_cmd = Pose()
237 | assert len(pose)==3
238 | pose_cmd.position.x = pose[0]
239 | pose_cmd.position.y = pose[1]
240 | pose_cmd.position.z = 0
241 |
242 | qtn = tf.transformations.quaternion_from_euler(0, 0, pose[2], 'rxyz')
243 | pose_cmd.orientation.x = qtn[0]
244 | pose_cmd.orientation.y = qtn[1]
245 | pose_cmd.orientation.z = qtn[2]
246 | pose_cmd.orientation.w = qtn[3]
247 | self.cmd_pose.publish(pose_cmd)
248 |
249 |
250 | def generate_random_pose(self):
251 | [x_robot, y_robot, theta] = self.get_self_stateGT()
252 | x = np.random.uniform(9, 19)
253 | y = np.random.uniform(0, 1)
254 | if y <= 0.4:
255 | y = -(y * 10 + 1)
256 | else:
257 | y = -(y * 10 + 9)
258 | dis_goal = np.sqrt((x - x_robot) ** 2 + (y - y_robot) ** 2)
259 | while (dis_goal < 7) and not rospy.is_shutdown():
260 | x = np.random.uniform(9, 19)
261 | y = np.random.uniform(0, 1)
262 | if y <= 0.4:
263 | y = -(y * 10 + 1)
264 | else:
265 | y = -(y * 10 + 9)
266 | dis_goal = np.sqrt((x - x_robot) ** 2 + (y - y_robot) ** 2)
267 | theta = np.random.uniform(0, 2*np.pi)
268 | return [x, y, theta]
269 |
270 | def generate_random_goal(self):
271 | [x_robot, y_robot, theta] = self.get_self_stateGT()
272 | x = np.random.uniform(9, 19)
273 | y = np.random.uniform(0, 1)
274 | if y <= 0.4:
275 | y = -(y*10 + 1)
276 | else:
277 | y = -(y*10 + 9)
278 | dis_goal = np.sqrt((x - x_robot) ** 2 + (y - y_robot) ** 2)
279 | while (dis_goal < 7) and not rospy.is_shutdown():
280 | x = np.random.uniform(9, 19)
281 | y = np.random.uniform(0, 1)
282 | if y <= 0.4:
283 | y = -(y * 10 + 1)
284 | else:
285 | y = -(y * 10 + 9)
286 | dis_goal = np.sqrt((x - x_robot) ** 2 + (y - y_robot) ** 2)
287 | return [x, y]
288 |
289 |
290 |
291 |
292 |
293 |
--------------------------------------------------------------------------------
/worlds/circle.world:
--------------------------------------------------------------------------------
1 | show_clock 0
2 | show_clock_interval 10000
3 | resolution 0.01
4 | threads 4
5 | speedup 1
6 |
7 | define sicklaser ranger
8 | (
9 | sensor(
10 | pose [ 0 0 0.1 0 ]
11 | fov 180
12 | range [ 0.0 6.0 ]
13 | samples 512
14 | )
15 | color "random"
16 | block(
17 | points 4
18 | point[0] [0 0]
19 | point[1] [0 1]
20 | point[2] [1 1]
21 | point[3] [1 0]
22 | z [0 0.21]
23 | )
24 | )
25 |
26 |
27 | define floorplan model
28 | (
29 | color "gray30"
30 | boundary 1
31 |
32 | gui_nose 0
33 | gui_grid 0
34 | gui_move 0
35 | gui_outline 0
36 | gripper_return 0
37 | fiducial_return 0
38 | ranger_return 1
39 | obstacle_return 1
40 | )
41 |
42 | floorplan
43 | (
44 | name "blank"
45 | bitmap "rink.png"
46 | size [60.000 60.000 0.800]
47 | pose [0.000 0.000 0.000 0.000]
48 |
49 | )
50 |
51 |
52 |
53 | window
54 | (
55 | size [1550 1550]
56 |
57 | # Camera options
58 | scale 35
59 | center [0 0]
60 | rotate [ 0.000 0.000 ]
61 |
62 |
63 | # GUI options
64 | show_data 1
65 | show_flags 1
66 | show_blocks 1
67 | show_clock 1
68 | show_footprints 1
69 | show_grid 1
70 | show_trailarrows 0
71 | show_trailrise 0
72 | show_trailfast 0
73 | show_occupancy 0
74 |
75 | )
76 |
77 |
78 |
79 |
80 | define agent position
81 | (
82 | # actual size
83 | size [0.44 0.38 0.22] # sizes from MobileRobots' web site
84 |
85 | # the pioneer's center of rotation is offset from its center of area
86 | origin [0 0 0 0]
87 |
88 | # draw a nose on the robot so we can see which way it points
89 | gui_nose 1
90 |
91 | color "random"
92 | drive "diff" # Differential steering model.
93 | obstacle_return 1 # Can hit things.
94 | ranger_return 0.5 # reflects sonar beams
95 | blob_return 1 # Seen by blobfinders
96 | fiducial_return 1 # Seen as "1" fiducial finders
97 | sicklaser(
98 | pose [ 0 0 0 0 ]
99 | )
100 |
101 | # [ xmin xmax ymin ymax zmin zmax amin amax ]
102 | # velocity_bounds [-0.5 0.5 0 0 0 0 -90.0 90.0 ]
103 | # acceleration_bounds [-0.5 0.5 0 0 0 0 -90 90.0 ]
104 | )
105 |
106 | agent( pose [25.00 0.00 0.00 180.00])
107 | agent( pose [24.80 3.13 0.00 187.20])
108 | agent( pose [24.21 6.22 0.00 194.40])
109 | agent( pose [23.24 9.20 0.00 201.60])
110 | agent( pose [21.91 12.04 0.00 208.80])
111 | agent( pose [20.23 14.69 0.00 216.00])
112 | agent( pose [18.22 17.11 0.00 223.20])
113 | agent( pose [15.94 19.26 0.00 230.40])
114 | agent( pose [13.40 21.11 0.00 237.60])
115 | agent( pose [10.64 22.62 0.00 244.80])
116 | agent( pose [7.73 23.78 0.00 252.00])
117 | agent( pose [4.68 24.56 0.00 259.20])
118 | agent( pose [1.57 24.95 0.00 266.40])
119 | agent( pose [-1.57 24.95 0.00 273.60])
120 | agent( pose [-4.68 24.56 0.00 280.80])
121 | agent( pose [-7.73 23.78 0.00 288.00])
122 | agent( pose [-10.64 22.62 0.00 295.20])
123 | agent( pose [-13.40 21.11 0.00 302.40])
124 | agent( pose [-15.94 19.26 0.00 309.60])
125 | agent( pose [-18.22 17.11 0.00 316.80])
126 | agent( pose [-20.23 14.69 0.00 324.00])
127 | agent( pose [-21.91 12.04 0.00 331.20])
128 | agent( pose [-23.24 9.20 0.00 338.40])
129 | agent( pose [-24.21 6.22 0.00 345.60])
130 | agent( pose [-24.80 3.13 0.00 352.80])
131 | agent( pose [-25.00 -0.00 0.00 360.00])
132 | agent( pose [-24.80 -3.13 0.00 367.20])
133 | agent( pose [-24.21 -6.22 0.00 374.40])
134 | agent( pose [-23.24 -9.20 0.00 381.60])
135 | agent( pose [-21.91 -12.04 0.00 388.80])
136 | agent( pose [-20.23 -14.69 0.00 396.00])
137 | agent( pose [-18.22 -17.11 0.00 403.20])
138 | agent( pose [-15.94 -19.26 0.00 410.40])
139 | agent( pose [-13.40 -21.11 0.00 417.60])
140 | agent( pose [-10.64 -22.62 0.00 424.80])
141 | agent( pose [-7.73 -23.78 0.00 432.00])
142 | agent( pose [-4.68 -24.56 0.00 439.20])
143 | agent( pose [-1.57 -24.95 0.00 446.40])
144 | agent( pose [1.57 -24.95 0.00 453.60])
145 | agent( pose [4.68 -24.56 0.00 460.80])
146 | agent( pose [7.73 -23.78 0.00 468.00])
147 | agent( pose [10.64 -22.62 0.00 475.20])
148 | agent( pose [13.40 -21.11 0.00 482.40])
149 | agent( pose [15.94 -19.26 0.00 489.60])
150 | agent( pose [18.22 -17.11 0.00 496.80])
151 | agent( pose [20.23 -14.69 0.00 504.00])
152 | agent( pose [21.91 -12.04 0.00 511.20])
153 | agent( pose [23.24 -9.20 0.00 518.40])
154 | agent( pose [24.21 -6.22 0.00 525.60])
155 | agent( pose [24.80 -3.13 0.00 532.80])
156 |
157 |
--------------------------------------------------------------------------------
/worlds/rink.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/worlds/rink.png
--------------------------------------------------------------------------------
/worlds/stage1.world:
--------------------------------------------------------------------------------
1 | show_clock 0
2 | show_clock_interval 10000
3 | resolution 0.2
4 | threads 16
5 | speedup 1
6 |
7 |
8 | define sicklaser ranger
9 | (
10 | sensor(
11 | pose [ 0 0 0.1 0 ]
12 | fov 180
13 | range [ 0.0 6.0 ]
14 | samples 512
15 | )
16 | color "random"
17 | block(
18 | points 4
19 | point[0] [0 0]
20 | point[1] [0 1]
21 | point[2] [1 1]
22 | point[3] [1 0]
23 | z [0 0.21]
24 | )
25 | )
26 |
27 |
28 | define floorplan model
29 | (
30 | color "gray30"
31 | boundary 1
32 |
33 | gui_nose 0
34 | gui_grid 0
35 | gui_move 1
36 | gui_outline 0
37 | gripper_return 0
38 | fiducial_return 0
39 | ranger_return 1
40 | obstacle_return 1
41 | )
42 |
43 | floorplan
44 | (
45 | name "blank"
46 | size [20.000 20.000 0.800]
47 | pose [0.000 0.000 0.000 0.000]
48 | bitmap "rink.png"
49 | )
50 |
51 |
52 |
53 | window
54 | (
55 | size [1550 1550]
56 |
57 | # Camera options
58 | scale 35
59 | center [0 0]
60 | rotate [ 0.000 0.000 ]
61 |
62 |
63 | # GUI options
64 | show_data 1
65 | show_flags 1
66 | show_blocks 1
67 | show_clock 1
68 | show_footprints 1
69 | show_grid 1
70 | show_trailarrows 0
71 | show_trailrise 0
72 | show_trailfast 0
73 | show_occupancy 0
74 |
75 | )
76 |
77 |
78 |
79 |
80 | define agent position
81 | (
82 | # actual size
83 | size [0.44 0.38 0.22] # sizes from MobileRobots' web site
84 |
85 | localization "gps"
86 | # the pioneer's center of rotation is offset from its center of area
87 | origin [0 0 0 0]
88 |
89 | # draw a nose on the robot so we can see which way it points
90 | gui_nose 1
91 |
92 | color "random"
93 | drive "diff" # Differential steering model.
94 | obstacle_return 1 # Can hit things.
95 | ranger_return 0.5 # reflects sonar beams
96 | blob_return 1 # Seen by blobfinders
97 | fiducial_return 1 # Seen as "1" fiducial finders
98 | sicklaser(
99 | pose [ 0 0 0 0 ]
100 | )
101 |
102 | # [ xmin xmax ymin ymax zmin zmax amin amax ]
103 | # velocity_bounds [-0.5 0.5 0 0 0 0 -90.0 90.0 ]
104 | # acceleration_bounds [-0.5 0.5 0 0 0 0 -90 90.0 ]
105 | )
106 |
107 | agent( pose [8.00 0.00 0.00 180.00])
108 | agent( pose [7.73 2.07 0.00 195.00])
109 | agent( pose [6.93 4.00 0.00 210.00])
110 | agent( pose [5.66 5.66 0.00 225.00])
111 | agent( pose [4.00 6.93 0.00 240.00])
112 | agent( pose [2.07 7.73 0.00 255.00])
113 | agent( pose [-0.00 8.00 0.00 270.00])
114 | agent( pose [-2.07 7.73 0.00 285.00])
115 | agent( pose [-4.00 6.93 0.00 300.00])
116 | agent( pose [-5.66 5.66 0.00 315.00])
117 | agent( pose [-6.93 4.00 0.00 330.00])
118 | agent( pose [-7.73 2.07 0.00 345.00])
119 | agent( pose [-8.00 -0.00 0.00 360.00])
120 | agent( pose [-7.73 -2.07 0.00 375.00])
121 | agent( pose [-6.93 -4.00 0.00 390.00])
122 | agent( pose [-5.66 -5.66 0.00 405.00])
123 | agent( pose [-4.00 -6.93 0.00 420.00])
124 | agent( pose [-2.07 -7.73 0.00 435.00])
125 | agent( pose [0.00 -8.00 0.00 450.00])
126 | agent( pose [2.07 -7.73 0.00 465.00])
127 | agent( pose [4.00 -6.93 0.00 480.00])
128 | agent( pose [5.66 -5.66 0.00 495.00])
129 | agent( pose [6.93 -4.00 0.00 510.00])
130 | agent( pose [7.73 -2.07 0.00 525.00])
131 |
132 |
--------------------------------------------------------------------------------
/worlds/stage2.world:
--------------------------------------------------------------------------------
1 | show_clock 0
2 | show_clock_interval 10000
3 | resolution 0.2
4 | threads 4
5 | speedup 20
6 |
7 | define sicklaser ranger
8 | (
9 | sensor(
10 | pose [ 0 0 0.1 0 ]
11 | fov 180
12 | range [ 0.0 6.0 ]
13 | samples 512
14 | )
15 | color "random"
16 | block(
17 | points 4
18 | point[0] [0 0]
19 | point[1] [0 1]
20 | point[2] [1 1]
21 | point[3] [1 0]
22 | z [0 0.21]
23 | )
24 | )
25 |
26 |
27 | define floorplan model
28 | (
29 | color "gray30"
30 | boundary 1
31 |
32 | gui_nose 0
33 | gui_grid 0
34 | gui_move 1
35 | gui_outline 0
36 | gripper_return 0
37 | fiducial_return 0
38 | ranger_return 1
39 | obstacle_return 1
40 | )
41 |
42 | floorplan
43 | (
44 | name "blank"
45 | size [40.000 40.000 0.800]
46 | pose [0.000 0.000 0.000 0.000]
47 | bitmap "testenv.png"
48 | )
49 |
50 |
51 |
52 | window
53 | (
54 | size [1550 1550]
55 |
56 | # Camera options
57 | scale 35
58 | center [0 0]
59 | rotate [ 0.000 0.000 ]
60 |
61 |
62 | # GUI options
63 | show_data 1
64 | show_flags 1
65 | show_blocks 1
66 | show_clock 1
67 | show_footprints 0
68 | show_grid 1
69 | show_trailarrows 0
70 | show_trailrise 0
71 | show_trailfast 0
72 | show_occupancy 0
73 |
74 | )
75 |
76 |
77 |
78 |
79 | define agent position
80 | (
81 | # actual size
82 | size [0.44 0.38 0.22] # sizes from MobileRobots' web site
83 |
84 | # the pioneer's center of rotation is offset from its center of area
85 | origin [0 0 0 0]
86 |
87 | # draw a nose on the robot so we can see which way it points
88 | gui_nose 1
89 |
90 | color "random"
91 | drive "diff" # Differential steering model.
92 | obstacle_return 1 # Can hit things.
93 | ranger_return 0.5 # reflects sonar beams
94 | blob_return 1 # Seen by blobfinders
95 | fiducial_return 1 # Seen as "1" fiducial finders
96 | sicklaser(
97 | pose [ 0 0 0 0 ]
98 | )
99 |
100 | # [ xmin xmax ymin ymax zmin zmax amin amax ]
101 | # velocity_bounds [-0.5 0.5 0 0 0 0 -90.0 90.0 ]
102 | # acceleration_bounds [-0.5 0.5 0 0 0 0 -90 90.0 ]
103 | )
104 |
105 | define obstacle position
106 | (
107 | color "random"
108 | ranger_return 1
109 | obstacle_return 1
110 |
111 | )
112 |
113 | agent( pose [-7.00 11.50 0.00 180.00])
114 | agent( pose [-7.00 9.50 0.00 180.00])
115 | agent( pose [-18.00 11.50 0.00 0.00])
116 | agent( pose [-18.00 9.50 0.00 0.00])
117 | agent( pose [-12.50 17.00 0.00 270.00])
118 | agent( pose [-12.50 4.00 0.00 90.00])
119 |
120 | agent( pose [-2.00 16.00 0.00 -90.00])
121 | agent( pose [0.00 16.00 0.00 -90.00])
122 | agent( pose [3.00 16.00 0.00 -90.00])
123 | agent( pose [5.00 16.00 0.00 -90.00])
124 |
125 | agent( pose [10.00 4.00 0.00 90.00])
126 | agent( pose [12.00 4.00 0.00 90.00])
127 | agent( pose [14.00 4.00 0.00 90.00])
128 | agent( pose [16.00 4.00 0.00 90.00])
129 | agent( pose [18.00 4.00 0.00 90.00])
130 |
131 | agent( pose [-2.5 -2.5 0.00 0.00])
132 | agent( pose [-0.5 -2.5 0.00 0.00])
133 | agent( pose [3.5 -2.5 0.00 180.00])
134 | agent( pose [5.5 -2.5 0.00 180.00])
135 |
136 | agent( pose [-2.5 -18.5 0.00 90.00])
137 | agent( pose [-0.5 -18.5 0.00 90.00])
138 | agent( pose [1.5 -18.5 0.00 90.00])
139 | agent( pose [3.5 -18.5 0.00 90.00])
140 | agent( pose [5.5 -18.5 0.00 90.00])
141 |
142 |
143 | agent( pose [-6.00 -10.00 0.00 180.00])
144 | agent( pose [-7.15 -6.47 0.00 216.00])
145 | agent( pose [-10.15 -4.29 0.00 252.00])
146 | agent( pose [-13.85 -4.29 0.00 288.00])
147 | agent( pose [-16.85 -6.47 0.00 324.00])
148 | agent( pose [-18.00 -10.00 0.00 360.00])
149 | agent( pose [-16.85 -13.53 0.00 396.00])
150 | agent( pose [-13.85 -15.71 0.00 432.00])
151 | agent( pose [-10.15 -15.71 0.00 468.00])
152 | agent( pose [-7.15 -13.53 0.00 504.00])
153 |
154 |
155 | agent( pose [10.00 -17.00 0.00 90.00])
156 | agent( pose [12.00 -17.00 0.00 90.00])
157 | agent( pose [14.00 -17.00 0.00 90.00])
158 | agent( pose [16.00 -17.00 0.00 90.00])
159 | agent( pose [18.00 -17.00 0.00 90.00])
160 |
161 | agent( pose [10.00 -2.00 0.00 -90.00])
162 | agent( pose [12.00 -2.00 0.00 -90.00])
163 | agent( pose [14.00 -2.00 0.00 -90.00])
164 | agent( pose [16.00 -2.00 0.00 -90.00])
165 | agent( pose [18.00 -2.00 0.00 -90.00])
166 |
167 |
168 |
169 | obstacle( pose [12 -9 0.00 0]
170 | size [0.7 0.7 0.8]
171 | block(
172 | points 6
173 | point[5] [0 0]
174 | point[4] [0 1]
175 | point[3] [0.75 1]
176 | point[2] [1 0.75]
177 | point[1] [1 0.25]
178 | point[0] [0.75 0]
179 | z [0 1]
180 | )
181 | )
182 |
183 | obstacle( pose [16 -7 0.00 0]
184 | size [0.7 0.7 0.8]
185 | block(
186 | points 16
187 | point[0] [ 0.225 0.000 ]
188 | point[1] [ 0.208 0.086 ]
189 | point[2] [ 0.159 0.159 ]
190 | point[3] [ 0.086 0.208 ]
191 | point[4] [ 0.000 0.225 ]
192 | point[5] [ -0.086 0.208 ]
193 | point[6] [ -0.159 0.159 ]
194 | point[7] [ -0.208 0.086 ]
195 | point[8] [ -0.225 0.000 ]
196 | point[9] [ -0.208 -0.086 ]
197 | point[10] [ -0.159 -0.159 ]
198 | point[11] [ -0.086 -0.208 ]
199 | point[12] [ -0.000 -0.225 ]
200 | point[13] [ 0.086 -0.208 ]
201 | point[14] [ 0.159 -0.159 ]
202 | point[15] [ 0.208 -0.086 ]
203 | z [0 0.1]
204 | )
205 | )
206 |
207 |
208 |
209 | obstacle( pose [17 -9 0.00 0]
210 | size [0.7 0.7 0.8]
211 | block(
212 | points 4
213 | point[0] [ -0.02 -0.077 ]
214 | point[1] [ 0.078 -0.077 ]
215 | point[2] [ 0.078 0.077 ]
216 | point[3] [ -0.02 0.077 ]
217 | z [0 0.02 ]
218 | )
219 | )
220 |
221 | obstacle( pose [14 -11 0.00 0]
222 | size [0.7 0.7 0.8]
223 | block( points 6
224 | point[0] [-3 -1.5 ]
225 | point[1] [-3 0.5 ]
226 | point[2] [-2 1.5 ]
227 | point[3] [+2 1.5 ]
228 | point[4] [+3 0.5 ]
229 | point[5] [+3 -1.5 ]
230 | z [0 0.05]
231 | )
232 | )
233 |
234 | obstacle( pose [10 -10 0.00 0]
235 | size [0.7 0.7 0.8]
236 | block( points 6
237 | point[0] [-3 -1.5 ]
238 | point[1] [-3 0.5 ]
239 | point[2] [-2 1.5 ]
240 | point[3] [+2 0.7 ]
241 | point[4] [+3 0.5 ]
242 | point[5] [+3 0 ]
243 | z [0 0.05]
244 | )
245 | )
246 |
247 | obstacle( pose [13 -7 0.00 0]
248 | size [0.7 0.7 0.8]
249 | block( points 6
250 | point[0] [0 -1.5 ]
251 | point[1] [0 0.5 ]
252 | point[2] [0.5 -0.5 ]
253 | point[3] [1 0.4 ]
254 | point[4] [2 -0.5 ]
255 | point[5] [0.5 -1 ]
256 | z [0 0.05]
257 | )
258 | )
259 | obstacle( pose [10 -7 0.00 0]
260 | size [0.7 0.7 0.8]
261 | block( points 5
262 | point[0] [0 0 ]
263 | point[1] [0.3 0.5 ]
264 | point[2] [0 1 ]
265 | point[3] [0.4 0.7]
266 | point[4] [0.8 1.2 ]
267 | point[4] [0.5 0]
268 |
269 | z [0 0.05]
270 | )
271 | )
272 |
273 | obstacle( pose [18 -11 0.00 0]
274 | size [0.7 0.7 0.8]
275 | block( points 6
276 | point[0] [-3 -1.5 ]
277 | point[1] [-3 0.5 ]
278 | point[2] [-2 1.5 ]
279 | point[3] [+2 0.7 ]
280 | point[4] [+3 0.5 ]
281 | point[5] [+3 0 ]
282 | z [0 0.05]
283 | )
284 | )
285 |
286 | obstacle( pose [15 -9 0.00 0]
287 | size [0.7 0.7 0.8]
288 | block( points 6
289 | point[0] [0 -1.5 ]
290 | point[1] [0 0.5 ]
291 | point[2] [0.5 -0.5 ]
292 | point[3] [1 0.4 ]
293 | point[4] [2 -0.5 ]
294 | point[5] [0.5 -1 ]
295 | z [0 0.05]
296 | )
297 | )
--------------------------------------------------------------------------------
/worlds/testenv.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Acmece/rl-collision-avoidance/c7cb93372ca5a16b5f32419b89d2f0302c4e9dc1/worlds/testenv.png
--------------------------------------------------------------------------------