Sakthi Sivaraman / cse_190_pa2

Download as
Browse Code ยป

Commit 7e34b4734cd7d2a7972997597357886eb2cd0401

Authored by Sakthi Sivaraman
1 parent ec0adf64c5
Exists in master

Added new move_lists & laser model

Showing 9 changed files with 394 additions and 3 deletions Side-by-side Diff

map/stage1.world View file @ 7e34b47
  1 +#Laser Ranger Properties
  2 +define topurg ranger
  3 +(
  4 + #Laser Sensor
  5 + #range => min & max ranger of a laser scan
  6 + #fov => angle of laser spread
  7 + #samples => number of laser scans
  8 +
  9 + sensor(
  10 + range [ 0.0 200.0 ]
  11 + fov 360
  12 + samples 100
  13 + )
  14 +
  15 + color "black"
  16 + size [ 0.05 0.05 0.1 ]
  17 +)
  18 +
  19 +#Robot Properties
  20 +define robot position
  21 +(
  22 + #DONT CHANGE SIZE, lot of random issues pop up if you try to increase it
  23 + size [0.35 0.35 0.25]
  24 + origin [0 0 0 0]
  25 + gui_nose 1
  26 + #Differential Turn
  27 + drive "diff"
  28 + #Min-Max velocities for x,y,z,theta
  29 + velocity_bounds [-40 40 -1 1 -1 1 -360 360 ]
  30 + acceleration_bounds [-1 1 -1 1 -1 1 -360 360]
  31 + #Laser sensor placed on the robot
  32 + topurg(pose [ 0.000 0.000 0 0.000 ])
  33 +)
  34 +
  35 +# throw in a robot in the map
  36 +robot( pose [ 125.0 90.0 0.0 0.0 ] name "robot" color "red")
  37 +
  38 +#FloorPlan Properties
  39 +define floorplan model
  40 +(
  41 + # sombre, sensible, artistic
  42 + color "gray30"
  43 +
  44 + # most maps will need a bounding box
  45 + boundary 1
  46 +
  47 + gui_nose 0
  48 + gui_grid 0
  49 +
  50 + gui_outline 0
  51 + gripper_return 0
  52 + fiducial_return 0
  53 + laser_return 1
  54 +)
  55 +
  56 +# set the resolution of the underlying raytrace model in meters
  57 +resolution 0.02
  58 +interval_sim 100 # simulation timestep in milliseconds
  59 +
  60 +#Window Properties
  61 +window
  62 +(
  63 + size [ 612.000 484.000 ]
  64 +
  65 + rotate [ 0.000 0.0 ]
  66 + scale 2.358
  67 +)
  68 +
  69 +# load an environment bitmap
  70 +floorplan
  71 +(
  72 + name "tutorial"
  73 + bitmap "MAP.png"
  74 + size [259.0 194.0 0.5]
  75 + pose [ 129.5 97.0 0.0 0.0 ]
  76 +)
map/stage2.world View file @ 7e34b47
  1 +#Laser Ranger Properties
  2 +define topurg ranger
  3 +(
  4 + #Laser Sensor
  5 + #range => min & max ranger of a laser scan
  6 + #fov => angle of laser spread
  7 + #samples => number of laser scans
  8 +
  9 + sensor(
  10 + range [ 0.0 200.0 ]
  11 + fov 360
  12 + samples 100
  13 + )
  14 +
  15 + color "black"
  16 + size [ 0.05 0.05 0.1 ]
  17 +)
  18 +
  19 +#Robot Properties
  20 +define robot position
  21 +(
  22 + #DONT CHANGE SIZE, lot of random issues pop up if you try to increase it
  23 + size [0.35 0.35 0.25]
  24 + origin [0 0 0 0]
  25 + gui_nose 1
  26 + #Differential Turn
  27 + drive "diff"
  28 + #Min-Max velocities for x,y,z,theta
  29 + velocity_bounds [-40 40 -1 1 -1 1 -360 360 ]
  30 + acceleration_bounds [-1 1 -1 1 -1 1 -360 360]
  31 + #Laser sensor placed on the robot
  32 + topurg(pose [ 0.000 0.000 0 0.000 ])
  33 +)
  34 +
  35 +# throw in a robot in the map
  36 +robot( pose [ 225.0 15.0 0.0 0.0 ] name "robot" color "red")
  37 +
  38 +#FloorPlan Properties
  39 +define floorplan model
  40 +(
  41 + # sombre, sensible, artistic
  42 + color "gray30"
  43 +
  44 + # most maps will need a bounding box
  45 + boundary 1
  46 +
  47 + gui_nose 0
  48 + gui_grid 0
  49 +
  50 + gui_outline 0
  51 + gripper_return 0
  52 + fiducial_return 0
  53 + laser_return 1
  54 +)
  55 +
  56 +# set the resolution of the underlying raytrace model in meters
  57 +resolution 0.02
  58 +interval_sim 100 # simulation timestep in milliseconds
  59 +
  60 +#Window Properties
  61 +window
  62 +(
  63 + size [ 612.000 484.000 ]
  64 +
  65 + rotate [ 0.000 0.0 ]
  66 + scale 2.358
  67 +)
  68 +
  69 +# load an environment bitmap
  70 +floorplan
  71 +(
  72 + name "tutorial"
  73 + bitmap "MAP.png"
  74 + size [259.0 194.0 0.5]
  75 + pose [ 129.5 97.0 0.0 0.0 ]
  76 +)
map/stage3.world View file @ 7e34b47
  1 +#Laser Ranger Properties
  2 +define topurg ranger
  3 +(
  4 + #Laser Sensor
  5 + #range => min & max ranger of a laser scan
  6 + #fov => angle of laser spread
  7 + #samples => number of laser scans
  8 +
  9 + sensor(
  10 + range [ 0.0 200.0 ]
  11 + fov 360
  12 + samples 100
  13 + )
  14 +
  15 + color "black"
  16 + size [ 0.05 0.05 0.1 ]
  17 +)
  18 +
  19 +#Robot Properties
  20 +define robot position
  21 +(
  22 + #DONT CHANGE SIZE, lot of random issues pop up if you try to increase it
  23 + size [0.35 0.35 0.25]
  24 + origin [0 0 0 0]
  25 + gui_nose 1
  26 + #Differential Turn
  27 + drive "diff"
  28 + #Min-Max velocities for x,y,z,theta
  29 + velocity_bounds [-40 40 -1 1 -1 1 -360 360 ]
  30 + acceleration_bounds [-1 1 -1 1 -1 1 -360 360]
  31 + #Laser sensor placed on the robot
  32 + topurg(pose [ 0.000 0.000 0 0.000 ])
  33 +)
  34 +
  35 +# throw in a robot in the map
  36 +robot( pose [ 225.0 15.0 0.0 0.0 ] name "robot" color "red")
  37 +
  38 +#FloorPlan Properties
  39 +define floorplan model
  40 +(
  41 + # sombre, sensible, artistic
  42 + color "gray30"
  43 +
  44 + # most maps will need a bounding box
  45 + boundary 1
  46 +
  47 + gui_nose 0
  48 + gui_grid 0
  49 +
  50 + gui_outline 0
  51 + gripper_return 0
  52 + fiducial_return 0
  53 + laser_return 1
  54 +)
  55 +
  56 +# set the resolution of the underlying raytrace model in meters
  57 +resolution 0.02
  58 +interval_sim 100 # simulation timestep in milliseconds
  59 +
  60 +#Window Properties
  61 +window
  62 +(
  63 + size [ 612.000 484.000 ]
  64 +
  65 + rotate [ 0.000 0.0 ]
  66 + scale 2.358
  67 +)
  68 +
  69 +# load an environment bitmap
  70 +floorplan
  71 +(
  72 + name "tutorial"
  73 + bitmap "MAP.png"
  74 + size [259.0 194.0 0.5]
  75 + pose [ 129.5 97.0 0.0 0.0 ]
  76 +)
scripts/configuration.json View file @ 7e34b47
... ... @@ -18,12 +18,13 @@
18 18 [-90.0,10.0,5]
19 19 ],
20 20  
  21 +
21 22 "first_move_sigma_x": 2,
22 23 "first_move_sigma_y": 2,
23 24 "first_move_sigma_angle": 0.08,
24   - "resample_sigma_x": 0.1,
25   - "resample_sigma_y": 0.1,
26   - "resample_sigma_angle": 0.02,
  25 + "resample_sigma_x": 1,
  26 + "resample_sigma_y": 1,
  27 + "resample_sigma_angle": 0.01,
27 28  
28 29 "laser_z_hit": 0.80,
29 30 "laser_z_rand": 0.05,
scripts/configuration1.json View file @ 7e34b47
  1 +{
  2 +
  3 + "seed": 0,
  4 +
  5 + "move_list": [
  6 + [0.0, 5.0,24],
  7 + [90.0,10.0,1],
  8 + [90.0,10.0,10],
  9 + [-90.0,10.0,7],
  10 + [0.0,5.0,1],
  11 + [180.0,20.0,7],
  12 + [0.0,5.0,3],
  13 + [180.0,20.0,4],
  14 + [90.0,20.0,4],
  15 + [-90.0,10.0,3],
  16 + [90.0,10.0,4],
  17 + [0.0,5.0,1],
  18 + [-90.0,10.0,5]
  19 + ],
  20 +
  21 +
  22 + "first_move_sigma_x": 2,
  23 + "first_move_sigma_y": 2,
  24 + "first_move_sigma_angle": 0.08,
  25 + "resample_sigma_x": 1,
  26 + "resample_sigma_y": 1,
  27 + "resample_sigma_angle": 0.01,
  28 +
  29 + "laser_z_hit": 0.80,
  30 + "laser_z_rand": 0.05,
  31 + "laser_sigma_hit": 2,
  32 +
  33 +
  34 + "num_particles": 800
  35 +
  36 +
  37 +}
scripts/configuration2.json View file @ 7e34b47
  1 +{
  2 +
  3 + "seed": 0,
  4 +
  5 + "move_list": [
  6 + [90.0, 5.0, 18],
  7 + [90.0, 10.0, 16],
  8 + [90.0, 10.0, 4],
  9 + [-90.0,10.0,5],
  10 + [90.0,10.0,5]
  11 + ],
  12 +
  13 +
  14 + "first_move_sigma_x": 2,
  15 + "first_move_sigma_y": 2,
  16 + "first_move_sigma_angle": 0.08,
  17 + "resample_sigma_x": 1,
  18 + "resample_sigma_y": 1,
  19 + "resample_sigma_angle": 0.01,
  20 +
  21 + "laser_z_hit": 0.80,
  22 + "laser_z_rand": 0.05,
  23 + "laser_sigma_hit": 2,
  24 +
  25 +
  26 + "num_particles": 800
  27 +
  28 +
  29 +}
scripts/configuration3.json View file @ 7e34b47
  1 +{
  2 +
  3 + "seed": 0,
  4 +
  5 + "move_list": [
  6 + [90.0, 5.0, 18],
  7 + [90.0, 10.0, 16],
  8 + [-90.0, 10.0, 3],
  9 + [90.0,10.0,4],
  10 + [0.0,5.0,1],
  11 + [-90.0,10.0,5]
  12 + ],
  13 +
  14 +
  15 + "first_move_sigma_x": 2,
  16 + "first_move_sigma_y": 2,
  17 + "first_move_sigma_angle": 0.08,
  18 + "resample_sigma_x": 1,
  19 + "resample_sigma_y": 1,
  20 + "resample_sigma_angle": 0.01,
  21 +
  22 + "laser_z_hit": 0.80,
  23 + "laser_z_rand": 0.05,
  24 + "laser_sigma_hit": 2,
  25 +
  26 +
  27 + "num_particles": 800
  28 +
  29 +
  30 +}
scripts/laser_model.py View file @ 7e34b47
  1 +#!/usr/bin/env python
  2 +import rospy
  3 +import numpy as np
  4 +import random as r
  5 +from math import *
  6 +from read_config import read_config
  7 +
  8 +from sensor_msgs.msg import LaserScan
  9 +
  10 +class laserModel():
  11 + def __init__(self):
  12 + rospy.init_node("laser_model")
  13 + self.config = read_config()
  14 + r.seed(self.config['seed'])
  15 + self.laser_sigma_hit = self.config['laser_sigma_hit']
  16 + rospy.Subscriber('original_scan', LaserScan, self.scan_callback)
  17 + self.base_scan_pub = rospy.Publisher('base_scan', LaserScan, queue_size = 10)
  18 + rospy.spin()
  19 +
  20 + def scan_callback(self, laser_scan):
  21 + self.base_scan_msg = LaserScan()
  22 + self.base_scan_msg.header = laser_scan.header
  23 + self.base_scan_msg.angle_min = laser_scan.angle_min
  24 + self.base_scan_msg.angle_max = laser_scan.angle_max
  25 + self.base_scan_msg.angle_increment = laser_scan.angle_increment
  26 + self.base_scan_msg.time_increment = laser_scan.time_increment
  27 + self.base_scan_msg.scan_time = laser_scan.scan_time
  28 + self.base_scan_msg.range_min = laser_scan.range_min
  29 + self.base_scan_msg.range_max = laser_scan.range_max
  30 + self.base_scan_msg.intensities = laser_scan.intensities
  31 + for i in range(len(laser_scan.ranges)):
  32 + if laser_scan.ranges[i] == laser_scan.range_max:
  33 + self.base_scan_msg.ranges.append(laser_scan.ranges[i])
  34 + else:
  35 + self.base_scan_msg.ranges.append(laser_scan.ranges[i] + ceil(r.gauss(0, self.laser_sigma_hit)*100.)/100.)
  36 + self.base_scan_pub.publish(self.base_scan_msg)
  37 +
  38 +
  39 +if __name__ == '__main__':
  40 + lm = laserModel()
scripts/test_metric.py View file @ 7e34b47
... ... @@ -7,6 +7,7 @@
7 7 import json
8 8 import tf
9 9 from math import *
  10 +from read_config import read_config
10 11  
11 12 class RobotLogger():
12 13 def __init__(self):
... ... @@ -32,6 +33,8 @@
32 33 self.handle_shutdown
33 34 )
34 35 self.init_files()
  36 + self.config = read_config()
  37 + self.num_particles = self.config['num_particles']
35 38 rospy.spin()
36 39  
37 40 def init_files(self):
... ... @@ -131,6 +134,18 @@
131 134  
132 135 self.update_metric()
133 136  
  137 + #particles_meeting_max_limit
  138 + count = 0.0
  139 + for particle_index in range(len(self.particlecloud_poses)):
  140 + pose = self.particlecloud_poses[particle_index]
  141 + diff_x_value = (pose.position.x - self.base_truth_x)
  142 + diff_y_value = (pose.position.y - self.base_truth_y)
  143 + diff_dist_val = sqrt(diff_x_value**2 + diff_y_value**2)
  144 + if (diff_dist_val <= 55):
  145 + count += 1
  146 + #percentage
  147 + self.particles_meeting_max_limit = float(count*100.0/self.num_particles)
  148 +
134 149 if message.data:
135 150 with open('time_results.json', 'w') as time:
136 151 time.write('{\n"time_elapsed" : ')
... ... @@ -150,6 +165,17 @@
150 165 metric_file.write('{\n"metric" : ')
151 166 json.dump(self.metric_data, metric_file)
152 167 metric_file.write('\n} \n')
  168 + with open('max_limit_results.json', 'w') as max_limit_file:
  169 + max_limit_file.write('{\n"max_limit_percentage" : ')
  170 + json.dump(self.particles_meeting_max_limit, max_limit_file)
  171 + max_limit_file.write('\n} \n')
  172 + with open('basetruth.json', 'w') as bt_file:
  173 + base_truth = []
  174 + base_truth.append(self.base_truth_x)
  175 + base_truth.append(self.base_truth_y)
  176 + bt_file.write('{\n"basetruth" : ')
  177 + json.dump(base_truth, bt_file)
  178 + bt_file.write('\n} \n')
153 179  
154 180  
155 181 if __name__ == '__main__':