def visualize(self):
'''
Publish various visualization messages.
'''
if not self.DO_VIZ:
return
if self.pose_pub.get_num_connections() > 0 and isinstance(self.inferred_pose, np.ndarray):
# Publish the inferred pose for visualization
ps = PoseStamped()
ps.header = Utils.make_header('map')
ps.pose.position.x = self.inferred_pose[0]
ps.pose.position.y = self.inferred_pose[1]
ps.pose.orientation = Utils.yaw_to_quaternion(self.inferred_pose[2])
self.pose_pub.publish(ps)
if self.particle_pub.get_num_connections() > 0:
# publish a downsampled version of the particle distribution to avoid a lot of latency
if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES:
# randomly downsample particles
proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights)
# proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES)
self.publish_particles(self.particles[proposal_indices,:])
else:
self.publish_particles(self.particles)
if self.pub_fake_scan.get_num_connections() > 0 and isinstance(self.ranges, np.ndarray):
# generate the scan from the point of view of the inferred position for visualization
self.viz_queries[:,0] = self.inferred_pose[0]
self.viz_queries[:,1] = self.inferred_pose[1]
self.viz_queries[:,2] = self.downsampled_angles + self.inferred_pose[2]
self.range_method.calc_range_many(self.viz_queries, self.viz_ranges)
self.publish_scan(self.downsampled_angles, self.viz_ranges)
def publish_tf(self, pose, stamp=None):
""" Publish a tf for the car. This tells ROS where the car is with respect to the map. """
if stamp == None:
stamp = rospy.Time.now()
# this may cause issues with the TF tree. If so, see the below code.
# self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]),
# stamp , "/laser", "/map")
# also publish odometry to facilitate getting the localization pose
if self.PUBLISH_ODOM:
odom = Odometry()
odom.header = Utils.make_header("/map", stamp)
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = Utils.yaw_to_quaternion(pose[2])
self.odom_pub.publish(odom)
# return # below this line is disabled
"""
Our particle filter provides estimates for the "laser" frame
since that is where our laser range estimates are measure from. Thus,
We want to publish a "map" -> "laser" transform.
However, the car's position is measured with respect to the "base_link"
frame (it is the root of the TF tree). And "odom" to "base_link" transform is published
by vesc_to_odom node. Thus, we should actually define
a "map" -> "odom" transform as to not break the TF tree.
"""
# Get map -> laser transform.
# map_laser_pos = np.array( (pose[0],pose[1],0) )
# map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
# Apply laser -> base_link transform to map -> laser transform
# This gives a map -> base_link transform
# laser_base_link_offset = (0.06, 0, 0)
# map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T
# Publish transform
# self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , "/base_link", "/map")
# Assuming only translational offset, no rotation
base_link_laser_delta = 0.06
map_base_link_pos_x = pose[0] - base_link_laser_delta * math.cos(
pose[2])
map_base_link_pos_y = pose[1] - base_link_laser_delta * math.sin(
pose[2])
# This assumes the map and odom frames are initially aligned
map_odom_delta = np.array(
(map_base_link_pos_x, map_base_link_pos_y, 0))
map_odom_orientation = np.array(
tf.transformations.quaternion_from_euler(0, 0, pose[2]))
self.pub_tf.sendTransform(map_odom_delta, map_odom_orientation, stamp,
"/base_link", "/map")
def publish_tf(self,pose, stamp=None):
''' Publish a tf for the car. This tells ROS where the car is with respect to the map. '''
if stamp == None:
stamp = rospy.Time.now()
# this may cause issues with the TF tree. If so, see the below code.
# self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]),
# stamp , '/laser', '/map')
# also publish odometry to facilitate getting the localization pose
if self.PUBLISH_ODOM:
odom = Odometry()
odom.header = Utils.make_header('/map', stamp)
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = Utils.yaw_to_quaternion(pose[2])
self.odom_pub.publish(odom)
# return # below this line is disabled
'''
Our particle filter provides estimates for the 'laser' frame
since that is where our laser range estimates are measure from. Thus,
We want to publish a 'map' -> 'laser' transform.
However, the car's position is measured with respect to the 'base_link'
frame (it is the root of the TF tree). Thus, we should actually define
a 'map' -> 'base_link' transform as to not break the TF tree.
'''
# Get map -> laser transform.
map_laser_pos = np.array( (pose[0],pose[1],0) )
map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
# Apply laser -> base_link transform to map -> laser transform
# This gives a map -> base_link transform
laser_base_link_offset = (0.15, 0.0, 0.0)
map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T
# Publish transform
self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , '/{}_base_link'.format(car_name), '/map')
def __init__(self):
# parameters
self.ANGLE_STEP = int(rospy.get_param("~angle_step"))
self.MAX_PARTICLES = int(rospy.get_param("~max_particles"))
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles"))
self.INV_SQUASH_FACTOR = 1.0 / float(rospy.get_param("~squash_factor"))
self.MAX_RANGE_METERS = float(rospy.get_param("~max_range"))
self.THETA_DISCRETIZATION = int(
rospy.get_param("~theta_discretization"))
self.WHICH_RM = rospy.get_param("~range_method", "cddt").lower()
self.RANGELIB_VAR = int(rospy.get_param("~rangelib_variant", "3"))
self.SHOW_FINE_TIMING = bool(rospy.get_param("~fine_timing", "0"))
self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1"))
self.DO_VIZ = bool(rospy.get_param("~viz"))
# sensor model constants
self.Z_SHORT = float(rospy.get_param("~z_short", 0.01))
self.Z_MAX = float(rospy.get_param("~z_max", 0.07))
self.Z_RAND = float(rospy.get_param("~z_rand", 0.12))
self.Z_HIT = float(rospy.get_param("~z_hit", 0.75))
self.SIGMA_HIT = float(rospy.get_param("~sigma_hit", 8.0))
# motion model constants
self.MOTION_DISPERSION_X = float(
rospy.get_param("~motion_dispersion_x", 0.05))
self.MOTION_DISPERSION_Y = float(
rospy.get_param("~motion_dispersion_y", 0.025))
self.MOTION_DISPERSION_THETA = float(
rospy.get_param("~motion_dispersion_theta", 0.25))
# various data containers used in the MCL algorithm
self.MAX_RANGE_PX = None
self.odometry_data = np.array([0.0, 0.0, 0.0])
self.laser = None
self.iters = 0
self.map_info = None
self.map_initialized = False
self.lidar_initialized = False
self.odom_initialized = False
self.last_pose = None
self.laser_angles = None
self.downsampled_angles = None
self.range_method = None
self.last_time = None
self.last_stamp = None
self.first_sensor_update = True
self.state_lock = Lock()
# cache this to avoid memory allocation in motion model
self.local_deltas = np.zeros((self.MAX_PARTICLES, 3))
# cache this for the sensor model computation
self.queries = None
self.ranges = None
self.tiled_angles = None
self.sensor_model_table = None
# particle poses and weights
self.inferred_pose = None
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.particles = np.zeros((self.MAX_PARTICLES, 3))
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES)
# initialize the state
self.smoothing = Utils.CircularArray(10)
self.timer = Utils.Timer(10)
self.get_omap()
self.precompute_sensor_model()
# self.initialize_global()
# these topics are for visualization
self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose",
PoseStamped,
queue_size=1)
self.particle_pub = rospy.Publisher("/pf/viz/particles",
PoseArray,
queue_size=1)
self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan",
LaserScan,
queue_size=1)
self.rect_pub = rospy.Publisher("/pf/viz/poly1",
PolygonStamped,
queue_size=1)
if self.PUBLISH_ODOM:
self.odom_pub = rospy.Publisher("/pf/pose/odom",
Odometry,
queue_size=1)
# these topics are for coordinate space things
self.pub_tf = tf.TransformBroadcaster()
# these topics are to receive data from the racecar
self.laser_sub = rospy.Subscriber(rospy.get_param(
"~scan_topic", "/scan"),
LaserScan,
self.lidarCB,
queue_size=1)
self.odom_sub = rospy.Subscriber(rospy.get_param(
"~odometry_topic", "/odom"),
Odometry,
self.odomCB,
queue_size=1)
# self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1)
# self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1)
initial_pose_x = float(rospy.get_param("~initial_pose_x", "0.0"))
initial_pose_y = float(rospy.get_param("~initial_pose_y", "0.0"))
initial_pose_a = float(rospy.get_param("~initial_pose_a", "0.0"))
initial_pose = Pose()
initial_pose.position.x = initial_pose_x
initial_pose.position.y = initial_pose_y
initial_pose.orientation = Utils.yaw_to_quaternion(initial_pose_a)
self.clicked_pose(initial_pose)
print "Finished initializing, waiting on messages..."