├── .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 | ![](./doc/stage2.gif) | ![](./doc/circle_test.gif) 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 --------------------------------------------------------------------------------