def __init__(self):
rospy.init_node('error_validation_node')
# Initialize subscribers to sensors and motors
rospy.Subscriber('/scan', LaserScan, self.read_sensor)
# Initialize publishers for visualization
self.error_markers_pub = rospy.Publisher('/error_markers',
MarkerArray,
queue_size=10)
self.odom_pose_pub = rospy.Publisher('odom_particle_pose',
Pose,
queue_size=10)
self.latest_scan_ranges = []
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# Class initializations
self.map_model = MapModel()
self.tf_helper = TFHelper()
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
"""for static error validation"""
self.static_particle = Particle(x=0, y=0, theta=0, weight=1)
self.sample_ranges = np.ones(361)
self.predicted_obstacle_x = 0.0
self.predicted_obstacle_y = 0.0
def __init__(self):
rospy.init_node('pf')
# create instances of two helper objects that are provided to you
# as part of the project
self.occupancy_field = OccupancyField()
self.tfh = TFHelper()
self.ros_boss = RosBoss()
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("/odom", Odometry, self.update_initial_pose)
self.current_particles = {}
self.max_particle_number = 100
self.map_width = self.occupancy_field.map.info.width
self.map_height = self.occupancy_field.map.info.height
self.map_origin = (self.occupancy_field.map.info.origin.position.x,
self.occupancy_field.map.info.origin.position.y)
self.map_resolution = self.occupancy_field.map.info.resolution
self.particle_viz = ParticlesMarker()
self.last_time = time.time()
def __init__(self):
rospy.init_node('pf_node')
# Initialize subscribers to sensors and motors
rospy.Subscriber('/scan', LaserScan, self.read_sensor)
# Initialize publishers for visualization
self.particle_pose_pub = rospy.Publisher('/particle_pose_array',
PoseArray,
queue_size=10)
self.odom_pose_pub = rospy.Publisher('odom_pose',
PoseArray,
queue_size=10)
self.map_marker_pub = rospy.Publisher('/map_marker',
Marker,
queue_size=10)
# self.particle_obstacles_pub = rospy.Publisher('/particle_obstacles', Marker, queue_size=10)
self.latest_scan_ranges = []
# Class initializations
self.p_distrib = ParticleDistribution()
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
self.map_model = MapModel()
self.tf_helper = TFHelper()
# When to run the particle filter
self.distance_moved_threshold = 0.2 # m
self.angle_turned_threshold = 10 # deg
# After map model has been initialized, create the initial particle distribution
self.p_distrib.init_particles(self.map_model)
self.particle_pose_pub.publish(
self.p_distrib.get_particle_pose_array())
def __init__(self):
rospy.init_node("propagation_test_node")
self.tf_helper = TFHelper()
self.base_link_pose = PoseStamped()
self.base_link_pose.header.frame_id = "base_link"
self.base_link_pose.header.stamp = rospy.Time(0)
self.last_odom_pose = PoseStamped()
self.last_odom_pose.header.frame_id = "odom"
self.last_odom_pose.header.stamp = rospy.Time(0)
self.particle_pose_pub = rospy.Publisher('/particle_pose_array',
PoseArray,
queue_size=10)
self.odom_pose_pub = rospy.Publisher('/odom_pose',
PoseArray,
queue_size=10)
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time(0)
self.pose_array.header.frame_id = "odom"
self.p = Particle(x=0, y=0, theta=180, weight=0)
self.p_array = PoseArray()
self.p_array.header.stamp = rospy.Time(0)
self.p_array.header.frame_id = "map"
self.is_first = True
def __init__(self):
rospy.init_node('calc_pos_node')
self.pos_change_pub = rospy.Publisher('/pos_change',
Vector3,
queue_size=10)
self.prev_pos = Vector3(0, 0,
0) # previous odometry of position and angle
self.prev_time = time.time()
self.transform_helper = TFHelper()
def __init__(self):
self.transform_helper = TFHelper()
self.odom_sub = rospy.Subscriber('odom', Odometry, self.odom_callback)
self.odom_pos = None
self.odom_ori = None
self.odom_header = None
self.robot_xyyaw_pose = None
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 300 # the number of particles to use
self.initial_uncertainty_xy = 1 # Amplitute factor of initial x and y uncertainty
self.initial_uncertainty_theta = 0.5 # Amplitude factor of initial theta uncertainty
self.variance_scale = 0.15 # Scaling term for variance effect on resampling
self.n_particles_average = 20 # Number of particles to average for pose update
self.linear_var_thresh = 0.05 # Maximum confidence along x/y (meters)
self.angular_var_thresh = 0.2 # Maximum confidence along theta (radians)
# self.resample_noise_xy = 0.1 # Amplitude factor of resample x and y noise
# self.resample_noise_theta = 0.1 # Amplitude factor of resample theta noise
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi/6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# TODO: define additional constants if needed
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
# publish custom particle array messge type
self.particle_viz_pub = rospy.Publisher("weighted_particlecloud", ParticleArray, queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 300 # the number of particles to use
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.weight_pub = rospy.Publisher('visualization_marker',
MarkerArray,
queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
# Holds all particles
self.particle_cloud = []
# Holds pre-normalized probabilities for each particle
self.scan_probabilities = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def __init__(self, waypoints):
rospy.init_node('contourBot')
# parameters related to waypoints
self.waypoints = waypoints
self.reach_first_point = False
self.angle_offset = 0.005
self.distance_offset = 0.1
# current position of robot
self.odom_pose = None
self.cur_pose_x = 0
self.cur_pose_y = 0
self.cur_pose_yaw = 0
# next waypoint robot tries to go to
self.last_waypoint = [0, 0]
self.waypoint_pose_x = 0
self.waypoint_pose_y = 0
self.waypoint_pose_theta = 0
self.finish_trace = False
# robot motion parameters
self.angle_diff = math.inf
self.distance_to_waypoint = math.inf
self.velocity = Twist()
self.k_linear_vel = 1.5
self.k_angular_vel = 1.5
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.transform_helper = TFHelper()
# coordinate frames
self.base_frame = "base_link"
self.odom_frame = "odom"
# set up pubs and subs
# rospy.Subscriber("/odom", Pose, self.update_robot_pose)
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.waypoints_marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
# visualization parameters
self.waypoints_marker = Marker()
self.waypoint_marker_line_color = ColorRGBA()
self.waypoint_marker_line_color.r = 0
self.waypoint_marker_line_color.g = 1.0
self.waypoint_marker_line_color.b = 0
self.waypoint_marker_line_color.a = 1.0
self.initialize_waypoints_as_markers()
def __init__(self):
rospy.init_node('pf')
self.particle_publisher = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.particle_manager = ParticleManager()
self.sensor_manager = SensorManager()
self.particle_manager.init_particles(self.occupancy_field)
self.scanDistance = 0.2
self.scanAngle = 0.5
self.moved = (0, 0)
def __init__(self):
# Initialize node and attributes
rospy.init_node("ParticleFilter")
# Subscribers
self.lidar_sub = rospy.Subscriber("/scan", LaserScan,
self.lidar_callback)
self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
self.initial_estimate_sub = rospy.Subscriber(
"/initialpose", PoseWithCovarianceStamped,
self.pose_estimate_callback)
# publishers
self.all_particles_pub = rospy.Publisher("/visualization_particles",
MarkerArray,
queue_size=10)
self.init_particles_pub = rospy.Publisher("/visualization_init",
MarkerArray,
queue_size=10)
self.new_particles_pub = rospy.Publisher("/particlecloud",
PoseArray,
queue_size=10)
# constants
self.number_of_particles = 30
pos_std_dev = 0.25
ori_std_dev = 25 * math.pi / 180
self.initial_std_dev = np.array(
[[pos_std_dev, pos_std_dev, ori_std_dev]]).T
self.lidar_std_dev = 0.02
self.resample_threshold = 0.1
# changing attributes
self.particles = np.ones([3, self.number_of_particles], dtype=np.float)
self.weights = np.ones(self.number_of_particles, dtype=np.float)
self.odom_tf_time = 0
self.base_tf_time = 0
self.scan = None
self.prev_pose = None
self.delta_pose = None
self.initial_pose_estimate = None
self.pose = None
# helper classes
self.occupancy_field = OccupancyField()
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.transform_helper = TFHelper()
rospy.loginfo("Initialized")
def __init__(self):
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# create instances of two helper objects that are provided to you
# as part of the project
self.num_particles = 500 # # of particles to use
self.odom_pose = PoseStamped()
self.transform_helper = TFHelper()
self.particle_cloud = []
class CalcPosNode(object):
def __init__(self):
rospy.init_node('calc_pos_node')
self.pos_change_pub = rospy.Publisher('/pos_change',
Vector3,
queue_size=10)
self.prev_pos = Vector3(0, 0,
0) # previous odometry of position and angle
self.prev_time = time.time()
self.transform_helper = TFHelper()
def odometry_callback(self, msg):
# operate on the msg
# set self.prev_pos to the last used position
if time.time() - self.prev_time < TIME_THROTTLE:
return
self.prev_time = time.time()
# storing and working with absolute odometry, publishing relative changes
transformed_pos = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose) # transform to x,y,yaw
new_pos = Vector3(transformed_pos[0], transformed_pos[1],
math.degrees(transformed_pos[2]))
self.pos_change_pub.publish(new_pos.x - self.prev_pos.x,
new_pos.y - self.prev_pos.y,
(new_pos.z - self.prev_pos.z) % 360)
self.prev_pos = new_pos
def run(self):
# read in the data (odometry)
# calculate the change in position (difference between current and last position)
rospy.Subscriber('/odom', Odometry, self.odometry_callback)
while not rospy.is_shutdown():
rospy.spin()
class ParticleFilter(object):
""" The class that represents a Particle Filter ROS Node
"""
def __init__(self):
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# create instances of two helper objects that are provided to you
# as part of the project
self.num_particles = 500 # # of particles to use
self.odom_pose = PoseStamped()
self.transform_helper = TFHelper()
self.particle_cloud = []
def particle_cloud_init(self, xy_theta=None):
'''
Initialize the particle cloud
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If None, odometry will be used instead
'''
#add some noise to our particle cloud
linear_noise = 1.0
angular_noise = math.pi / 2.0
# Use odom if there's no xy_theta
if xy_theta == None:
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
self.particle_cloud = []
for x in range(self.num_particles):
x = xy_theta[0] + (random_sample() * linear_noise -
(linear_noise / 2.0))
y = xy_theta[1] + (random_sample() * linear_noise -
(linear_noise / 2.0))
theta = xy_theta[2] + (random_sample() * angular_noise -
(angular_noise / 2.0))
particles = Particle(x, y, theta)
self.particle_cloud.append(particles)
self.particle_normalizer()
def particle_normalizer(self):
'''Make sure the particle weights sum to 1'''
weights_sum = sum(particle.w for particle in self.particle_cloud)
for particle in self.particle_cloud:
particle.w /= weights_sum
def particle_publisher(self, msg):
particles = []
for p in self.particle_cloud:
particles.append(p.particle_to_pose())
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(), frame_id="map"),
poses=particles))
def __init__(self, top_particle_pub, particle_cloud_pub, particle_cloud_marker_pub):
# # pose_listener responds to selection of a new approximate robot
# # location (for instance using rviz)
# rospy.Subscriber("initialpose",
# PoseWithCovarianceStamped,
# self.update_initial_pose)
self.top_particle_pub = top_particle_pub
self.particle_cloud_pub = particle_cloud_pub
self.particle_cloud_marker_pub = particle_cloud_marker_pub
# create instances of two helper objects that are provided to you
# as part of the project
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.particles = []
self.markerArray = MarkerArray()
def __init__(self):
rospy.init_node('pf')
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose",
PoseWithCovarianceStamped,
self.update_initial_pose)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# create instances of two helper objects that are provided to you
# as part of the project
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
def __init__(self):
rospy.init_node('motor_model')
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 10)
self.pub2 = rospy.Publisher('Robot_marker', Marker, queue_size = 10)
self.pub3 = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size = 10)
rospy.Subscriber('/odom', Odometry, self.read_pos)
self.rate = rospy.Rate(10)
self.key = None
self.pose = None #as a Twist
self.orientation = None #as a quaternion
self.tf = TFHelper()
self.real_pose = None
self.particle_array = []
self.num_particles = 15
self.create_particle_array()
self.sd_filter = self.num_particles/3.0 #standard deviation for random sampling
self.sd_position = .1 #standard deviation of position noise
self.sd_theta = 5/3.0 #standard deviation (bounded by 5 degrees) of theta noise
def __init__(self):
"""
__init__ function to create main attributes, setup threshold values, setup rosnode subs and pubs
"""
rospy.init_node('pf')
self.initialized = False
self.num_particles = 150
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.particle_cloud = []
self.lidar_points = []
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.best_particle_pub = rospy.Publisher("particlebest",
PoseStamped,
queue_size=10)
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.best_guess = (
None, None) # (index of particle with highest weight, its weight)
self.particles_to_replace = .075
self.n_effective = 0 # this is a measure of the particle diversity
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# create instances of two helper objects that are provided to you
# as part of the project
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def __init__(self):
print("init RobotLocalizer")
rospy.init_node('localizer')
self.tfHelper = TFHelper()
self.xs = None
self.ys = None
self.ranges = [] # Lidar scan
self.last_odom_msg = None
print(self.last_odom_msg)
self.diff_transform = {
'translation': None,
'rotation': None,
}
self.odom_changed = False # Toggles to True when the odom frame has changed enough
# subscribers and publisher
self.laser_sub = rospy.Subscriber('/scan', LaserScan,
self.process_scan)
self.odom_sub = rospy.Subscriber("/odom", Odometry, self.update_odom)
### Used for the particle filter
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# publisher for the top weighted particle
self.topparticle_pub = rospy.Publisher("topparticle",
PoseArray,
queue_size=10)
# publisher for rviz markers
self.pub_markers = rospy.Publisher('/visualization_markerarray',
MarkerArray,
queue_size=10)
self.particle_filter = ParticleFilter(self.topparticle_pub,
self.particle_pub,
self.pub_markers)
print("ParticleFilter initialized")
print("RobotLocalizer initialized")
def __init__(self, num_positions=500, num_orientations=10):
# TODO: Interface with SLAM algorithm's published map
# Initialize map.
rospy.init_node(
"neato_mdp") # May break if markov_model is also subscribed...?
rospy.wait_for_service("static_map")
static_map = rospy.ServiceProxy("static_map", GetMap)
# Initialize MDP
self.mdp = MDP(num_positions=num_positions,
num_orientations=num_orientations,
map=static_map().map)
self.state_idx = None # Current state idx is unknown.
self.curr_odom_pose = Pose()
self.tf_helper = TFHelper()
# Velocity publisher
self.cmd_vel_publisher = rospy.Publisher("/cmd_vel",
Twist,
queue_size=10,
latch=True)
self.odom_subscriber = rospy.Subscriber('/odom', Odometry,
self.set_odom)
self.goal_state = None
# Visualize robot
self.robot_state_pub = rospy.Publisher('/robot_state_marker',
Marker,
queue_size=10)
self.robot_state_pose_pub = rospy.Publisher('/robot_state_pose',
PoseArray,
queue_size=10)
self.goal_state_pub = rospy.Publisher('/goal_state_marker',
Marker,
queue_size=10)
# # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
#
self.odom_pose = PoseStamped()
self.odom_pose.header.stamp = rospy.Time(0)
self.odom_pose.header.frame_id = 'odom'
#
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
rospy.Subscriber("move_base_simple/goal", PoseStamped,
self.update_goal_state)
class ParticleFilter(object):
""" The class that represents a Particle Filter ROS Node
"""
def __init__(self):
rospy.init_node('pf')
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose",
PoseWithCovarianceStamped,
self.update_initial_pose)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# create instances of two helper objects that are provided to you
# as part of the project
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter
based on a pose estimate. These pose estimates could be generated
by another ROS Node or could come from the rviz GUI """
xy_theta = \
self.transform_helper.convert_pose_to_xy_and_theta(msg.pose.pose)
# TODO this should be deleted before posting
self.transform_helper.fix_map_to_odom_transform(msg.pose.pose,
msg.header.stamp)
# initialize your particle filter based on the xy_theta tuple
def run(self):
r = rospy.Rate(5)
while not(rospy.is_shutdown()):
# in the main loop all we do is continuously broadcast the latest
# map to odom transform
self.transform_helper.send_last_map_to_odom_transform()
r.sleep()
def __init__(self):
rospy.init_node("sensor_likelihood_test")
self.occupancy_field = OccupancyField()
self.tf_helper = TFHelper()
self.latest_scan_ranges = []
rospy.Subscriber('/scan', LaserScan, self.read_sensor)
self.odom_poses = PoseArray()
self.odom_poses.header.frame_id = "odom"
self.particle_pose_pub = rospy.Publisher('/particle_pose_array',
PoseArray,
queue_size=10)
self.odom_pose_pub = rospy.Publisher('odom_pose',
PoseArray,
queue_size=10)
self.marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10)
self.p_distrib = ParticleDistribution()
self.init_particles()
# self.p = Particle(x=0, y=0, theta=0, weight=1)
self.particle_poses = PoseArray()
self.particle_poses.header.frame_id = "map"
self.last_odom_pose = PoseStamped()
self.last_odom_pose.header.frame_id = "odom"
self.last_odom_pose.header.stamp = rospy.Time(0)
self.base_link_pose = PoseStamped()
self.base_link_pose.header.frame_id = "base_link"
self.base_link_pose.header.stamp = rospy.Time(0)
self.counter = 0
self.is_first = True
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
def __init__(self):
"""initialize node, ROS things, etc."""
rospy.init_node("ParticleFilter")
rospy.Subscriber('/scan', LaserScan, self.process_scan)
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.pub2 = rospy.Publisher('Robot_marker', Marker, queue_size=10)
self.pub3 = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=10)
self.particle_pub = rospy.Publisher("particle_cloud",
PointCloud,
queue_size=10)
rospy.Subscriber('/odom', Odometry, self.read_pos)
self.rate = rospy.Rate(20)
self.xs_bl = [] # list of xs from lidar in base link frame
self.ys_bl = [] # list of ys from lidar in base link frame
self.scanx_now = []
self.scany_now = []
self.last_pose = None
self.pose = None #as a Twist
self.orientation = None #as a quaternion
self.tf = TFHelper()
self.real_pose = None
self.particle_array = []
self.num_particles = 500 #adjust to increase number of particles
self.update_rate = .5 #in seconds
self.sd_filter = self.num_particles * 3.0 / (
4) #standard deviation for random sampling
self.current_pose = (0, 0, 0)
self.previous_pose = (0, 0, 0)
self.previous_time = rospy.get_time()
self.current_time = rospy.get_time()
self.cloud_array = PointCloud(
) #point cloud for laser scan visualization
self.x_array = []
self.y_array = []
self.initialize_pf()
print('Init complete')
def __init__(self):
print('Initializing')
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('localizer')
self.pf = ParticleFilter()
self.base_frame = "base_link" # Robot base frame
self.map_frame = "map" # Map coord frame
self.odom_frame = "odom" # Odom coord frame
self.scan_topic = "scan" # Laser scan topic
self.linear_threshold = 0.1 # the amount of linear movement before performing an update
self.angular_threshold = math.pi / 10 # the amount of angular movement before performing an update
self.max_dist = 2.0 # maximum penalty to assess in the likelihood field model
self.odom_pose = PoseStamped()
self.robot_pose = Pose()
self.robot_pose = Pose()
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan)
# init pf
# subscribers and publisher
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.odom_particle_updater)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.pose_updater)
# enable listening for and broadcasting coord transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.current_odom_xy_theta = []
print("initialization complete")
self.initialized = True
def __init__(self):
rospy.init_node('pf')
real_robot = False
# create instances of two helper objects that are provided to you
# as part of the project
self.particle_filter = ParticleFilter()
self.occupancy_field = OccupancyField()
self.TFHelper = TFHelper()
self.sensor_model = sensor_model = SensorModel(
model_noise_rate=0.5,
odometry_noise_rate=0.15,
world_model=self.occupancy_field,
TFHelper=self.TFHelper)
self.position_delta = None # Pose, delta from current to previous odometry reading
self.last_scan = None # list of ranges
self.odom = None # Pose, current odometry reading
self.x_y_spread = 0.3 # Spread constant for x-y initialization of particles
self.z_spread = 0.2 # Spread constant for z initialization of particles
self.n_particles = 150 # number of particles
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
rospy.Subscriber("odom", Odometry, self.update_position)
rospy.Subscriber("stable_scan", LaserScan, self.update_scan)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
class RunRobot(object):
''' Represents all of the sensor and robot model related operations'''
def __init__(self):
self.transform_helper = TFHelper()
self.odom_sub = rospy.Subscriber('odom', Odometry, self.odom_callback)
self.odom_pos = None
self.odom_ori = None
self.odom_header = None
self.robot_xyyaw_pose = None
def odom_callback(self, msg):
self.odom_header = msg.header
self.odom_pos = msg.pose.pose.position
self.odom_ori = msg.pose.pose.orientation
def laserCallback(self, msg):
''' Represents all of the logic for handling laser messages'''
# Set ranges for front, back, left, and right
front_left = msg.ranges[0:90]
back_left = msg.ranges[90:180]
back_right = msg.ranges[180:270]
front_right = msg.ranges[270:360]
laser_diff = 0 #set difference between quadrants = 0
# Calculate difference between left front and back to determine where wall is on left side
for front_left, back_left in zip(front_left, reversed(back_left)):
if front_left == 0.0 or back_left == 0.0:
continue
laser_diff += front_left - back_left
# Calculate diff between right front and back to determine where wall is on right side
for front_right, back_right in zip(front_right, reversed(back_right)):
if front_right == 0.0 or back_right == 0.0:
continue
laser_diff += back_right - front_right
def robot_position(self):
'''Represents the position of the robot as a x, y, yaw tuple'''
pose = self.transform_helper.convert_translation_rotation_to_pose(self.odom_pos, self.odom_ori)
self.robot_xyyaw_pose = self.transform.helper.convert_pose_to_xy_and_theta(pose)
return self.robot_xyyaw_pose
def __init__(self):
rospy.init_node('finite_state')
self.distance_threshold = 0.8
self.state = "teleop"
self.x = 0
self.y = 0
self.rotation = 0
self.linear_error = 0
self.angular_error = 0
self.linear_k = 0.1
self.angular_k = .005
self.vel_msg = Twist()
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# rospy.Subscriber('stable_scan', LaserScan, self.process_scan)
rospy.Subscriber('projected_stable_scan', PointCloud2,
self.projected_scan_received)
rospy.Subscriber('odom', Odometry, self.process_odom)
# Initializing user input
self.settings = termios.tcgetattr(sys.stdin)
self.key = None
self.map_y = []
self.map_x = []
self.map_neatox = []
self.map_neatoy = []
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.transform_helper = TFHelper()
self.initialized = True
self.moved_flag = False
self.init_x = None
self.init_y = None
self.init_z = None
self.starting_threshold = 0.1
self.transform = None
self.index_saved = 0
self.old_scan = []
self.new_scan = []
self.data = {"odom": [], "scans": [], "closures": []}
class ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
start_particles: the number of particles first initalized
end_particles: the number of particles which end in the filter
middle_step: the step at which the number of particles has decayed about 50%
d_thresh: the amount of linear movement before triggering a filter update
a_thresh: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
tf_broadcaster: broadcaster for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
""" Define a new particle filter
"""
print("RUNNING")
self.initialized = False # make sure we don't perform updates before everything is setup
self.kidnap = False
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.start_particles = 1000 # the number of particles to use
self.end_particles = 200
self.resample_count = 10
self.middle_step = 10
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# publish weights for live graph node
self.weight_pub = rospy.Publisher("/graph_data",
Float64MultiArray,
queue_size=10)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
# publish the marker array
# self.viz = rospy.Publisher('/particle_marker', Marker, queue_size=10)
# self.marker = Marker()
self.viz = rospy.Publisher('/particle_marker',
MarkerArray,
queue_size=10)
self.markerArray = MarkerArray()
self.initialized = True
# assigns robot pose. used only a visual debugger, the real data comes from the bag file.
def update_robot_pose(self, timestamp):
#print("Guessing Robot Position")
self.normalize_particles(self.particle_cloud)
weights = [p.w for p in self.particle_cloud]
index_best = weights.index(max(weights))
best_particle = self.particle_cloud[index_best]
self.robot_pose = self.transform_helper.covert_xy_and_theta_to_pose(
best_particle.x, best_particle.y, best_particle.theta)
self.transform_helper.fix_map_to_odom_transform(
self.robot_pose, timestamp)
def projected_scan_received(self, msg):
self.last_projected_stable_scan = msg
# deadreckons particles with respect to robot motion.
def update_particles_with_odom(self, msg):
""" To apply the robot transformations to a particle, it can be broken down into a rotations, a linear movement, and another rotation (which could equal 0)
"""
#print("Deadreckoning")
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
delta_x = delta[0]
delta_y = delta[1]
delta_theta = delta[2]
direction = math.atan2(delta_y, delta_x)
theta_1 = self.transform_helper.angle_diff(
direction, self.current_odom_xy_theta[2])
for p in self.particle_cloud:
distance = math.sqrt((delta_x**2) +
(delta_y**2)) + np.random.normal(
0, 0.001)
dx = distance * np.cos(p.theta + theta_1)
dy = distance * np.sin(p.theta + theta_1)
p.x += dx
p.y += dy
p.theta += delta_theta + np.random.normal(0, 0.005)
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
def resample_particles(self):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
# print("Resampling Particles")
# make sure the distribution is normalized
self.normalize_particles(self.particle_cloud)
particle_cloud_length = len(self.particle_cloud)
n_particles = ParticleFilter.sigmoid_function(self.resample_count,
self.start_particles,
self.end_particles,
self.middle_step, 1)
print("Number of Particles Reassigned: " + str(n_particles))
norm_weights = [p.w for p in self.particle_cloud]
# print("Weights: "+ str(norm_weights))
top_percent = 0.20
ordered_indexes = np.argsort(norm_weights)
ordered_particles = [
self.particle_cloud[index] for index in ordered_indexes
]
best_particles = ordered_particles[int(particle_cloud_length *
(1 - top_percent)):]
new_particles = ParticleFilter.draw_random_sample(
self.particle_cloud, norm_weights,
n_particles - int(particle_cloud_length * top_percent))
dist = 0.001 # adding a square meter of noise around each ideal particle
self.particle_cloud = []
self.particle_cloud += best_particles
for p in new_particles:
x_pos, y_pos, angle = p.x, p.y, p.theta
x_particle = np.random.normal(x_pos, dist)
y_particle = np.random.normal(y_pos, dist)
theta_particle = np.random.normal(angle, 0.05)
self.particle_cloud.append(
Particle(x_particle, y_particle, theta_particle))
self.normalize_particles(self.particle_cloud)
self.resample_count += 1
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
#transform laser data from particle's perspective to map coords
#print("Assigning Weights")
scan = msg.ranges
num_particles = len(self.particle_cloud)
num_scans = 361
step = 2
angles = np.arange(num_scans) # will be scan indices (0-361)
distances = np.array(scan) # will be scan values (scan)
angles_rad = np.deg2rad(angles)
for p in self.particle_cloud:
sin_values = np.sin(angles_rad + p.theta)
cos_values = np.cos(angles_rad + p.theta)
d_angles_sin = np.multiply(distances, sin_values)
d_angles_cos = np.multiply(distances, cos_values)
d_angles_sin = d_angles_sin[0:361:step]
d_angles_cos = d_angles_cos[0:361:step]
total_beam_x = np.add(p.x, d_angles_cos)
total_beam_y = np.add(p.y, d_angles_sin)
particle_distances = self.occupancy_field.get_closest_obstacle_distance(
total_beam_x, total_beam_y)
cleaned_particle_distances = [
2 * np.exp(-(dist**2)) for dist in particle_distances
if (math.isnan(dist) != True)
]
p_d_cubed = np.power(cleaned_particle_distances, 3)
p.w = np.sum(p_d_cubed)
self.normalize_particles(self.particle_cloud)
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
#print("Initial Pose Set")
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
self.initialize_particle_cloud(msg.header.stamp, xy_theta)
def initialize_particle_cloud(self, timestamp, xy_theta=None):
"""
Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is omitted, the odometry will be used
Also check to see if we are attempting the robot kidnapping problem or are given an initial 2D pose
"""
if self.kidnap:
print("Kidnap Problem")
x_bound, y_bound = self.occupancy_field.get_obstacle_bounding_box()
x_particle = np.random.uniform(x_bound[0],
x_bound[1],
size=self.start_particles)
y_particle = np.random.uniform(y_bound[0],
y_bound[1],
size=self.start_particles)
theta_particle = np.deg2rad(
np.random.randint(0, 360, size=self.start_particles))
else:
print("Starting with Inital Position")
if xy_theta is None:
print("No Position Definied")
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
x, y, theta = xy_theta
x_particle = np.random.normal(x, 0.25, size=self.start_particles)
y_particle = np.random.normal(y, 0.25, size=self.start_particles)
theta_particle = np.random.normal(theta,
0.001,
size=self.start_particles)
self.particle_cloud = [Particle(x_particle[i],\
y_particle[i],\
theta_particle[i]) \
for i in range(self.start_particles)]
def normalize_particles(self, particle_list):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
#print("Normalize Particles")
old_weights = [p.w for p in particle_list]
new_weights = []
for p in particle_list:
p.w = float(p.w) / sum(old_weights)
new_weights.append(p.w)
float_array = Float64MultiArray()
float_array.data = new_weights
self.weight_pub.publish(float_array)
def publish_particles(self, msg):
"""
Publishes particle poses on the map.
Uses Paul's default code at the moment, maybe later attempt to publish a visualization/MarkerArray
"""
particles_conv = []
for num, p in enumerate(self.particle_cloud):
particles_conv.append(p.as_pose())
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
# self.marker_update("map", self.particle_cloud, False)
# self.viz.publish()
def scan_received(self, msg):
"""
All control flow happens here!
Special init case then goes into loop
"""
if not (self.initialized):
# wait for initialization to complete
return
# wait a little while to see if the transform becomes available. This fixes a race
# condition where the scan would arrive a little bit before the odom to base_link transform
# was updated.
# self.tf_listener.waitForTransform(self.base_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(0.5))
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not self.current_odom_xy_theta:
self.current_odom_xy_theta = new_odom_xy_theta
return
if not (self.particle_cloud):
print("Particle Cloud Empty")
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud(msg.header.stamp)
self.update_particles_with_laser(msg)
self.normalize_particles(self.particle_cloud)
self.update_robot_pose(msg.header.stamp)
self.resample_particles()
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
print("UPDATING PARTICLES")
self.update_particles_with_odom(msg) # update based on odometry
if self.last_projected_stable_scan:
last_projected_scan_timeshift = deepcopy(
self.last_projected_stable_scan)
last_projected_scan_timeshift.header.stamp = msg.header.stamp
self.scan_in_base_link = self.tf_listener.transformPointCloud(
"base_link", last_projected_scan_timeshift)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose(msg.header.stamp) # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def marker_update(self, frame_id, p_cloud, delete):
num = 0
if (delete):
self.markerArray.markers = []
else:
for p in p_cloud:
marker = Marker()
marker.header.frame_id = frame_id
marker.header.stamp = rospy.Time.now()
marker.ns = "my_namespace"
marker.id = num
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = p.as_pose()
marker.pose.position.z = 0.5
marker.scale.x = 1.0
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0 # Don't forget to set the alpha!
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
num += 1
self.markerArray.markers.append(marker)
@staticmethod
def sigmoid_function(value, max_output, min_output, middle, inc=1):
particle_difference = max_output - min_output
exponent = inc * (value - (middle / 2))
return int(particle_difference / (1 + np.exp(exponent)) + min_output)
class ParticleFilter(object):
"""
Class to represent Particle Filter ROS Node
Subscribes to /initialpose for initial pose estimate
Publishes top particle estimate to /particlebest and all particles in cloud to /particlecloud
"""
def __init__(self):
"""
__init__ function to create main attributes, setup threshold values, setup rosnode subs and pubs
"""
rospy.init_node('pf')
self.initialized = False
self.num_particles = 150
self.d_thresh = 0.2 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update
self.particle_cloud = []
self.lidar_points = []
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.best_particle_pub = rospy.Publisher("particlebest",
PoseStamped,
queue_size=10)
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.best_guess = (
None, None) # (index of particle with highest weight, its weight)
self.particles_to_replace = .075
self.n_effective = 0 # this is a measure of the particle diversity
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)
# create instances of two helper objects that are provided to you
# as part of the project
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
print("xy_theta", xy_theta)
self.initialize_particle_cloud(
msg.header.stamp,
xy_theta) # creates particle cloud at position passed in
# by message
print("INITIALIZING POSE")
# Use the helper functions to fix the transform
def initialize_particle_cloud(self, timestamp, xy_theta):
"""
Creates initial particle cloud based on robot pose estimate position
"""
self.particle_cloud = []
angle_variance = math.pi / 10 # POint the points in the general direction of the robot
x_cur = xy_theta[0]
y_cur = xy_theta[1]
theta_cur = self.transform_helper.angle_normalize(xy_theta[2])
# print("theta_cur: ", theta_cur)
for i in range(self.num_particles):
# Generate values for and add a new particle!!
x_rel = random.uniform(-.3, .3)
y_rel = random.uniform(-.3, .3)
new_theta = (random.uniform(theta_cur - angle_variance,
theta_cur + angle_variance))
# TODO: Could use a tf transform to add x and y in the robot's coordinate system
new_particle = Particle(x_cur + x_rel, y_cur + y_rel, new_theta)
self.particle_cloud.append(new_particle)
print("Done initializing particles")
self.normalize_particles()
# publish particles (so things like rviz can see them)
self.publish_particles()
print("normalized correctly")
self.update_robot_pose(timestamp)
print("updated robot pose")
def normalize_particles(self):
"""
Normalizes particle weights to total but retains weightage
"""
total_weights = sum([particle.w for particle in self.particle_cloud])
# if your weights aren't normalized then normalize them
if total_weights != 1.0:
for i in self.particle_cloud:
i.w = i.w / total_weights
def update_robot_pose(self, timestamp):
""" Update the estimate of the robot's pose in the map frame given the updated particles.
There are two logical methods for this:
(1): compute the mean pose based on all the high weight particles
(2): compute the most likely pose (i.e. the mode of the distribution)
"""
# first make sure that the particle weights are normalized
self.normalize_particles()
print("Normalized particles in update robot pose")
# create average pose for robot pose based on entire particle cloud
average_x = 0
average_y = 0
average_theta = 0
# walk through all particles, calculate weighted average for x, y, z, in particle map.
for p in self.particle_cloud:
average_x += p.x * p.w
average_y += p.y * p.w
average_theta += p.theta * p.w
# # create new particle representing weighted average values, pass in Pose to new robot pose
self.robot_pose = Particle(average_x, average_y,
average_theta).as_pose()
print(timestamp)
self.transform_helper.fix_map_to_odom_transform(
self.robot_pose, timestamp)
print("Done fixing map to odom")
def publish_particles(self):
"""
Publish entire particle cloud as pose array for visualization in RVIZ
Also publish the top / best particle based on its weight
"""
# Convert the particles from xy_theta to pose!!
pose_particle_cloud = []
for p in self.particle_cloud:
pose_particle_cloud.append(p.as_pose())
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=pose_particle_cloud))
# doing shit based off best pose
best_pose_quat = max(self.particle_cloud,
key=attrgetter('w')).as_pose()
#self.best_particle_pub.publish(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), pose=best_pose_quat)
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
# TODO: FIX noise incorporation into movement.
min_travel = 0.2
xy_spread = 0.02 / min_travel # More variance with driving forward
theta_spread = .005 / min_travel
random_vals_x = np.random.normal(0, abs(delta[0] * xy_spread),
self.num_particles)
random_vals_y = np.random.normal(0, abs(delta[1] * xy_spread),
self.num_particles)
random_vals_theta = np.random.normal(0, abs(delta[2] * theta_spread),
self.num_particles)
for p_num, p in enumerate(self.particle_cloud):
# compute phi, or basically the angle from 0 that the particle
# needs to be moving - phi equals OG diff angle - robot angle + OG partilce angle
# ADD THE NOISE!!
noisy_x = (delta[0] + random_vals_x[p_num])
noisy_y = (delta[1] + random_vals_y[p_num])
ang_of_dest = math.atan2(noisy_y, noisy_x)
# calculate angle needed to turn in angle_to_dest
ang_to_dest = self.transform_helper.angle_diff(
self.current_odom_xy_theta[2], ang_of_dest)
d = math.sqrt(noisy_x**2 + noisy_y**2)
phi = p.theta + ang_to_dest
p.x += math.cos(phi) * d
p.y += math.sin(phi) * d
p.theta += self.transform_helper.angle_normalize(
delta[2] + random_vals_theta[p_num])
self.current_odom_xy_theta = new_odom_xy_theta
def update_particles_with_laser(self, msg):
"""
calculate particle weights based off laser scan data passed into param
"""
# print("Updating particles with Laser")
lidar_points = msg.ranges
for p_deg, p in enumerate(self.particle_cloud):
# do we need to compute particle pos in diff frame?
p.occ_scan_mapped = [] # reset list
for scan_distance in lidar_points:
# handle edge case
if scan_distance == 0.0:
continue
# calc a delta theta and use that to overlay scan data onto the particle headings
pt_rad = deg2rad(p_deg)
particle_pt_theta = self.transform_helper.angle_normalize(
p.theta + pt_rad)
particle_pt_x = p.x + math.cos(
particle_pt_theta) * scan_distance
particle_pt_y = p.y + math.sin(
particle_pt_theta) * scan_distance
# calculate distance from every single scan point in particle frame
occ_value = self.occupancy_field.get_closest_obstacle_distance(
particle_pt_x, particle_pt_y)
# Think about cutting off max penalty if occ_value is too big
p.occ_scan_mapped.append(occ_value)
# assign weights based off newly assigned occ_scan_mapped
# apply gaussian e**-d**2 to every weight, then cube to emphasize
p.occ_scan_mapped = [(math.e / (d)**2) if (d)**2 != 0 else
(math.e / (d + .01)**2)
for d in p.occ_scan_mapped]
p.occ_scan_mapped = [d**3 for d in p.occ_scan_mapped]
p.w = sum(p.occ_scan_mapped)
#print("Set weight to: ", p.w)
p.occ_scan_mapped = []
self.normalize_particles()
@staticmethod
def draw_random_sample(choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(random_sample(n), bins)]
samples = []
for i in inds:
samples.append(deepcopy(choices[int(i)]))
return samples
def resample_particles(self):
"""
Re initialize particles in self.particle_cloud
based on preivous weightages.
"""
weights = [p.w for p in self.particle_cloud]
# after calculating all particle weights, we want to calc the n_effective
# self.n_effective = 0
self.n_effective = 1 / sum(
[w**2 for w in weights]) # higher is more diversity, so less noise
print("n_effective: ", self.n_effective)
temp_particle_cloud = self.draw_random_sample(
self.particle_cloud, weights,
int((1 - self.particles_to_replace) * self.num_particles))
# temp_particle_cloud = self.draw_random_sample(self.particle_cloud, weights, self.num_particles)
particle_cloud_to_transform = self.draw_random_sample(
self.particle_cloud, weights, self.num_particles - int(
(1 - self.particles_to_replace) * self.num_particles))
# NOISE POLLUTION - larger noise, smaller # particles
# normal_std_xy = .25
normal_std_xy = 10 / self.n_effective # feedback loop? 8,3
normal_std_theta = 3 / self.n_effective
# normal_std_theta = math.pi/21
random_vals_x = np.random.normal(0, normal_std_xy,
len(particle_cloud_to_transform))
random_vals_y = np.random.normal(0, normal_std_xy,
len(particle_cloud_to_transform))
random_vals_theta = np.random.normal(0, normal_std_theta,
len(particle_cloud_to_transform))
for p_num, p in enumerate(
particle_cloud_to_transform): # add in noise in x,y, theta
p.x += random_vals_x[p_num]
p.y += random_vals_y[p_num]
p.theta += random_vals_theta[p_num]
# reset the partilce cloud based on the newly transformed particles
self.particle_cloud = temp_particle_cloud + particle_cloud_to_transform
def scan_received(self, msg):
"""
Callback function for recieving laser scan - should pass data into global scan object
"""
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, we hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative to the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
# grab from listener & store the the odometry pose in a more convenient format (x,y,theta)
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not self.current_odom_xy_theta:
self.current_odom_xy_theta = new_odom_xy_theta
return
# Now we've done all calcs, we exit the scan_recieved() method by either initializing a cloud
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
# TODO: Where do we get the xy_theta needed for initialize_particle_cloud?
self.initialize_particle_cloud(msg.header.stamp,
self.current_odom_xy_theta)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg)
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose(msg.header.stamp) # update robot's pose
self.resample_particles(
) # resample particles to focus on areas of high density
# # publish particles (so things like rviz can see them)
self.publish_particles()
def run(self):
"""
main run loop for rosnode
"""
r = rospy.Rate(5)
print("Nathan and Adi ROS Loop code is starting!!!")
while not (rospy.is_shutdown()):
# in the main loop all we do is continuously broadcast the latest
# map to odom transform
self.transform_helper.send_last_map_to_odom_transform()
r.sleep()
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node(
'pf') # tell roscore that we are creating a new node named "pf"
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 600 # the number of particles to use
self.particle_init_options = ParticleInitOptions.UNIFORM_DISTRIBUTION
self.d_thresh = 0.1 # the amount of linear movement before performing an update
self.a_thresh = math.pi / 12.0 # the amount of angular movement before performing an update
self.num_lidar_points = 180 # int from 1 to 360
# Note: self.laser_max_distance is NOT implemented
# TODO: Experiment with setting a max acceptable distance for lidar scans
self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model
# Setup pubs and subs
# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
# laser_subscriber listens for data from the lidar
rospy.Subscriber(self.scan_topic,
LaserScan,
self.scan_received,
queue_size=1)
# publish the current particle cloud. This enables viewing particles in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
# publish our hypotheses points
self.hypothesis_pub = rospy.Publisher("hypotheses",
MarkerArray,
queue_size=10)
# Publish our hypothesis points right before they get udpated through odom
self.before_odom_hypothesis_pub = rospy.Publisher(
"before_odom_hypotheses", MarkerArray, queue_size=10)
# Publish where the hypothesis points will be once they're updated with the odometry
self.future_hypothesis_pub = rospy.Publisher("future_hypotheses",
MarkerArray,
queue_size=10)
# Publish the lidar scan that pf.py sees
self.lidar_pub = rospy.Publisher("lidar_visualization",
MarkerArray,
queue_size=1)
# Publish the lidar scan projected from the first hypothesis
self.projected_lidar_pub = rospy.Publisher(
"projected_lidar_visualization", MarkerArray, queue_size=1)
# enable listening for and broadcasting coordinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_cloud = []
# change use_projected_stable_scan to True to use point clouds instead of laser scans
self.use_projected_stable_scan = False
self.last_projected_stable_scan = None
if self.use_projected_stable_scan:
# subscriber to the odom point cloud
rospy.Subscriber("projected_stable_scan", PointCloud,
self.projected_scan_received)
self.current_odom_xy_theta = []
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.initialized = True
