def get_segment_plan(self, start_pose, waypoint_pose, time_stamp):
start_x, start_y, start_theta = start_pose[0], start_pose[
1], start_pose[2]
wp_x, wp_y, wp_theta = waypoint_pose[0], waypoint_pose[
1], waypoint_pose[2]
# set up and publish the start pose startMsg
startMsg = PoseWithCovarianceStamped()
quaternion = utils.angle_to_quaternion(start_theta)
startMsg.header.stamp = time_stamp
startMsg.header.frame_id = "/map"
startMsg.pose.pose.position.x = start_x
startMsg.pose.pose.position.y = start_y
startMsg.pose.pose.orientation = quaternion
# set up and publish the goal pose goalMsg
goalMsg = PoseStamped()
quaternion = utils.angle_to_quaternion(wp_theta)
goalMsg.header.stamp = time_stamp
goalMsg.header.frame_id = "/map"
goalMsg.pose.position.x = wp_x
goalMsg.pose.position.y = wp_y
goalMsg.pose.orientation = quaternion
self.initial_pub.publish(startMsg)
self.goal_pub.publish(goalMsg)
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.angle_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 visualize(self):
#print 'Visualizing...'
self.state_lock.acquire()
self.inferred_pose = self.expected_pose()
if isinstance(self.inferred_pose, np.ndarray):
self.publish_tf(self.inferred_pose)
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.angle_to_quaternion(self.inferred_pose[2])
if(self.pose_pub.get_num_connections() > 0):
self.pose_pub.publish(ps)
if(self.pub_odom.get_num_connections() > 0):
odom = Odometry()
odom.header = ps.header
odom.pose.pose = ps.pose
self.pub_odom.publish(odom)
if self.particle_pub.get_num_connections() > 0:
if self.particles.shape[0] > 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_laser.get_num_connections() > 0 and isinstance(self.sensor_model.last_laser, LaserScan):
self.sensor_model.last_laser.header.frame_id = "/laser"
self.sensor_model.last_laser.header.stamp = rospy.Time.now()
self.pub_laser.publish(self.sensor_model.last_laser)
self.state_lock.release()
def visualize(self, path, start, goal):
'''
Publish various visualization messages.
'''
#rospy.loginfo('visualizing start')
s = PointStamped()
s.header = Utils.make_header("/map")
s.point.x = start[0]
s.point.y = start[1]
s.point.z = 0
self.start_pub.publish(s)
#rospy.loginfo('visualizing goal')
g = PointStamped()
g.header = Utils.make_header("/map")
g.point.x = goal[0]
g.point.y = goal[1]
g.point.z = 0
self.goal_pub.publish(g)
#rospy.loginfo('visualizing path')
p = Path()
p.header = Utils.make_header("/map")
for point in path:
pose = PoseStamped()
pose.header = Utils.make_header("/map")
pose.pose.position.x = point[0]
pose.pose.position.y = point[1]
pose.pose.orientation = Utils.angle_to_quaternion(0)
p.poses.append(pose)
self.path_pub.publish(p)
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.angle_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.angle_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.265, 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
map_laser_pos[0] -= self.position[0]
map_laser_pos[1] -= self.position[1]
orientation_list = [
self.odom_orientation.x, self.odom_orientation.y,
self.odom_orientation.z, self.odom_orientation.w
]
(roll, pitch, odom_base_link_yaw
) = tf.transformations.euler_from_quaternion(orientation_list)
map_odom_yaw = pose[2] - odom_base_link_yaw
map_odom_rotation = np.array(
tf.transformations.quaternion_from_euler(0, 0, map_odom_yaw))
# Publish transform
self.pub_tf.sendTransform(map_laser_pos, map_odom_rotation, stamp,
"/odom", "/map")
def main():
rospy.init_node('line_follower', anonymous=True) # Initialize the node
plan_relative_path = "/saved_plans/plan2"
# load plan_array
# load raw_plan msg (PoseArray)
loaded_vars = pickle.load(open(CURRENT_PKG_PATH + plan_relative_path, "r"))
plan_array = loaded_vars[0]
# raw_plan = loaded_vars[1]
plan_array_new = []
total_distance = 0.0
for i in range(len(plan_array) - 1):
total_distance += np.sqrt(
np.square(plan_array[i][0] - plan_array[i + 1][0]) +
np.square(plan_array[i][1] - plan_array[i + 1][1]))
avg_distance = total_distance / len(plan_array)
array_len = len(plan_array)
i = 0
while i < array_len - 1:
current_distance = np.sqrt(
np.square(plan_array[i][0] - plan_array[i + 1][0]) +
np.square(plan_array[i][1] - plan_array[i + 1][1]))
if current_distance < avg_distance * 0.5:
plan_array_new.append(plan_array.pop(i + 1))
i -= 1
array_len -= 1
i += 1
PA = PoseArray() # create a PoseArray() msg
PA.header.stamp = rospy.Time.now() # set header timestamp value
PA.header.frame_id = "map" # set header frame id value
PA.poses = []
for pose in plan_array:
P = Pose()
P.position.x = float(pose[0])
P.position.y = float(pose[1])
P.position.z = 0
P.orientation = utils.angle_to_quaternion(float(pose[2]))
PA.poses.append(P)
# visualize edited loaded plan
PA_pub = rospy.Publisher("/LoadedPlan", PoseArray, queue_size=1)
for i in range(0, 5):
rospy.sleep(0.5)
PA_pub.publish(PA)
# save plan_array to file
# save plan PoseArray msg to file
file_temp = open(CURRENT_PKG_PATH + plan_relative_path + "_clean", 'w')
pickle.dump([plan_array_new, PA], file_temp)
file_temp.close()
def init_callback(self, init):
rospy.loginfo("Receiving init pose: " + init.data)
init_data = init.data.split(',')
init_msg = PoseWithCovarianceStamped()
init_msg.header.stamp = rospy.Time.now()
init_msg.header.frame_id = "/map"
init_msg.pose.pose.position.x = float(init_data[0])
init_msg.pose.pose.position.y = float(init_data[1])
init_msg.pose.pose.orientation = angle_to_quaternion(float(init_data[2]))
self.pub_init.publish(init_msg)
def _simulate_odom(self, evt):
# print "Simulate odometery..."
pose = self.car.state.copy()
self.odom_msg.header.stamp = rospy.Time.now()
self.odom_msg.pose.pose.position.x = pose[0]
self.odom_msg.pose.pose.position.y = pose[1]
self.odom_msg.pose.pose.orientation = utils.angle_to_quaternion(
pose[2])
self.odom_pub.publish(self.odom_msg)
self.odom_timer.tick()
def _cb_init(self, data):
rospy.loginfo('Receiving init pose: ' + data.data)
data = data.data.split(',')
init_pose = PoseWithCovarianceStamped()
init_pose.header.stamp = rospy.Time.now()
init_pose.header.frame_id = 'map'
init_pose.pose.pose.position.x = float(data[0])
init_pose.pose.pose.position.y = float(data[1])
init_pose.pose.pose.orientation = utils.angle_to_quaternion(
float(data[2]))
self.p_initpose.publish(init_pose)
def inferred_pose_map(self):
# Publishing inferred_pose in map/pixel coordinatess
if self.map_pose_pub.get_num_connections() > 0 and isinstance(self.inferred_pose, np.ndarray):
map_inferred_pose = Utils.world_to_map_slow(self.inferred_pose[0],self.inferred_pose[1],self.inferred_pose[2],self.map_info)
ps = PoseStamped()
ps.header = Utils.make_header("map")
ps.pose.position.x = map_inferred_pose[0]
ps.pose.position.y = self.map_info.height - map_inferred_pose[1]
ps.pose.orientation = Utils.angle_to_quaternion(map_inferred_pose[2])
self.map_pose_pub.publish(ps)
def publish_plan(self, plan):
pa = PoseArray()
pa.header.frame_id = "/map"
for i in xrange(len(plan)):
config = plan[i]
pose = Pose()
pose.position.x = config[0]
pose.position.y = config[1]
pose.position.z = 0.0
pose.orientation = utils.angle_to_quaternion(config[2])
pa.poses.append(pose)
self.plan_pub.publish(pa)
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.angle_to_quaternion(pose[2])
cov_mat = np.cov(self.particles,
rowvar=False,
ddof=0,
aweights=self.weights).flatten()
odom.pose.covariance[:cov_mat.shape[0]] = cov_mat
odom.twist.twist.linear.x = self.current_speed
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.265, 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")
def print_waypoints(self, waypoints):
WpMsg = PoseArray()
WpMsg.header.frame_id = "/map"
wp_plan = []
for waypoint in waypoints:
x = waypoints[0]
y = waypoints[1]
quaternion = utils.angle_to_quaternion(waypoints[2])
wp_plan.poses.orientation = quaternion
wp_plan.poses.pose.x = x
wp_plan.poses.pose.y = y
wp_plan.extend(wp_plan.poses)
WpMsg.poses = wp_plan
waypoint_sub.publish(WpMsg)
def update(self):
'''
Apply the MCL function to update particle filter state.
Ensures the state is correctly initialized, and acquires the state lock before proceeding.
'''
if self.lidar_initialized and self.odom_initialized and self.map_initialized:
self.timer.tick()
self.iters += 1
t1 = time.time()
with self.state_lock:
scans = np.copy(self.downsampled_ranges).astype(np.float32)
odom = np.copy(self.odometry_data)
self.odometry_data = np.zeros(3)
# run the MCL update algorithm
if not self.MCL(odom, scans):
rospy.logwarn("skipped update")
return
if np.isnan(self.weights).any():
rospy.logwarn(
"something weird happened to the particle distribution"
)
ps = Pose()
ps.position.x = self.inferred_pose[0]
ps.position.y = self.inferred_pose[1]
ps.orientation = Utils.angle_to_quaternion(
self.inferred_pose[2])
self.initialize_particles_pose(ps)
return
# compute the expected value of the robot pose
self.inferred_pose = self.expected_pose(self.particles)
#rospy.loginfo(self.inferred_pose)
t2 = time.time()
# publish transformation frame based on inferred pose
#rospy.loginfo("update")
self.publish_tf(self.inferred_pose, self.last_stamp)
# this is for tracking particle filter speed
ips = 1.0 / (t2 - t1)
self.smoothing.append(ips)
#if self.iters % 10 == 0:
# print "iters per sec:", int(self.timer.fps()), " possible:", int(self.smoothing.mean())
self.visualize()
def visualize(self):
"""
Visualize the current state of the filter
(1) Publishes a tf between the map and the laser. Necessary for visualizing the laser scan in the map
(2) Publishes the most recent laser measurement. Note that the frame_id of this message should be '/laser'
(3) Publishes a PoseStamped message indicating the expected pose of the car
(4) Publishes a subsample of the particles (use self.N_VIZ_PARTICLES).
Sample so that particles with higher weights are more likely to be sampled.
"""
self.state_lock.acquire()
self.inferred_pose = self.expected_pose()
if isinstance(self.inferred_pose, np.ndarray):
if self.PUBLISH_TF:
self.publish_tf(self.inferred_pose)
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.angle_to_quaternion(
self.inferred_pose[2])
if self.pose_pub.get_num_connections() > 0:
self.pose_pub.publish(ps)
if self.pub_odom.get_num_connections() > 0:
odom = Odometry()
odom.header = ps.header
odom.pose.pose = ps.pose
self.pub_odom.publish(odom)
if self.particle_pub.get_num_connections() > 0:
if self.particles.shape[0] > self.N_VIZ_PARTICLES:
# randomly downsample particles
proposal_indices = np.random.choice(self.particle_indices,
self.N_VIZ_PARTICLES,
p=self.weights)
self.publish_particles(self.particles[proposal_indices, :])
else:
self.publish_particles(self.particles)
if self.pub_laser.get_num_connections() > 0 and isinstance(
self.sensor_model.last_laser, LaserScan):
self.sensor_model.last_laser.header.frame_id = "/laser"
self.sensor_model.last_laser.header.stamp = rospy.Time.now()
self.pub_laser.publish(self.sensor_model.last_laser)
self.state_lock.release()
def goal_point_pub(self, pose):
# Publishing goal_point in map/pixel coordinatess
# if self.map_goal_pub.get_num_connections() > 0 and not self.goal_set:
if self.map_goal_pub.get_num_connections() > 0:
map_goal_point = Utils.world_to_map_slow(pose.position.x,pose.position.y,Utils.quaternion_to_angle(pose.orientation),self.map_info)
ps = PoseStamped()
ps.header = Utils.make_header("map")
ps.pose.position.x = map_goal_point[0]
ps.pose.position.y = self.map_info.height - map_goal_point[1]
ps.pose.orientation = Utils.angle_to_quaternion(map_goal_point[2])
# ps1 = PoseStamped()
# ps1.header = Utils.make_header("map")
# ps1.pose.position.x = pose.position.x
# ps1.pose.position.y = pose.position.y
# ps1.pose.orientation = pose.orientation
self.map_goal_pub.publish(ps)
# self.goal_set = True
print "goal point printed", map_goal_point
def viz_sub_cb(self, msg):
# Create the PoseArray to publish. Will contain N poses, where the n-th pose
# represents the last pose in the n-th trajectory
pa = PoseArray()
pa.header.frame_id = '/small_basement'
pa.header.stamp = rospy.Time.now()
# Transform the last pose of each trajectory to be w.r.t the world and insert into
# the pose array
for i in xrange(self.rollouts.shape[0]):
newPose = Pose()
newPose.position.x = self.rollouts[i, 299, 0]
newPose.position.y = self.rollouts[i, 299, 1]
newPose.position.z = 0.0
newPose.orientation = utils.angle_to_quaternion(self.rollouts[i,
299,
2])
pa.poses.append(newPose)
self.viz_pub.publish(pa)
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.angle_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.265, 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")
def viz_sub_cb(self, msg):
# Create the PoseArray to publish. Will contain N poses, where the n-th pose
# represents the last pose in the n-th trajectory
# PP - get current pose from mgs
cur_pose = np.array([
msg.pose.position.x, msg.pose.position.y,
utils.quaternion_to_angle(msg.pose.orientation)
])
pa = PoseArray()
pa.header.frame_id = '/map'
pa.header.stamp = rospy.Time.now()
# Transform the last pose of each trajectory to be w.r.t the world and insert into
for i in range(self.rollouts.shape[0]):
# PP - below code is similar to code in line_follower for transformation
trans_mat = [cur_pose[0], cur_pose[1]]
rot_mat = utils.rotation_matrix(cur_pose[2])
arrow_pose = (self.rollouts[i][-1][0], self.rollouts[i][-1][1])
arrow_pose = np.reshape(arrow_pose, (2, 1))
trans_mat = np.reshape(trans_mat, (2, 1))
arrow_wrt_cur_pose = rot_mat * arrow_pose + trans_mat # co ordinate transformation
# create Pose to add in PoseArray
pose = Pose()
pose.position.x = arrow_wrt_cur_pose[0]
pose.position.y = arrow_wrt_cur_pose[1]
pose.position.z = 0
pose.orientation = utils.angle_to_quaternion(
self.rollouts[i][-1][2] +
cur_pose[2]) # PP - is ths correct? verify with Patrick
#print last_pose_in_traj
pa.poses.append(pose)
self.viz_pub.publish(pa)
def viz_sub_cb(self, msg):
pa = PoseArray()
pa.header.frame_id = '/map'
pa.header.stamp = rospy.Time.now()
trans = np.array([msg.pose.position.x, msg.pose.position.y]).reshape(
(2, 1))
car_yaw = utils.quaternion_to_angle(msg.pose.orientation)
rot_mat = utils.rotation_matrix(car_yaw)
for i in xrange(self.rollouts.shape[0]):
robot_config = self.rollouts[i, -1, 0:2].reshape(2, 1)
map_config = rot_mat * robot_config + trans
map_config.flatten()
pose = Pose()
pose.position.x = map_config[0]
pose.position.y = map_config[1]
pose.position.z = 0.0
pose.orientation = utils.angle_to_quaternion(self.rollouts[i, -1,
2] +
car_yaw)
pa.poses.append(pose)
self.viz_pub.publish(pa)
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):
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.angle_to_quaternion(
self.inferred_pose[2])
self.pose_pub.publish(ps)
if self.iters % 10 == 0:
self.path.header = ps.header
self.path.poses.append(ps)
self.path_pub.publish(self.path)
if self.particle_pub.get_num_connections() > 0:
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.sensor_model.particle_scans, np.ndarray):
# generate the scan from the point of view of the inferred position for visualization
inferred_scans = self.sensor_model.get_scans(
self.inferred_pose[None, :])
self.publish_scan(self.downsampled_angles, inferred_scans[0, :])
def publish_tf(self, pose, stamp=None):
"""
Publish a tf from map to inferred_scan_frame.
http://wiki.ros.org/tf/Tutorials/Adding%20a%20frame%20(Python)
"""
# Publish the tf from map to inferred_scan_frame.
self.pub_tf.sendTransform((pose[0], pose[1], 0.0),
tf.transformations.quaternion_from_euler(0, 0, pose[2]),
stamp,
"inferred_scan_frame",
"map")
# Need to account for constant offset from laser frame to base_link.
self.pub_tf.sendTransform((-0.265, 0.0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
stamp,
"inferred_base_link",
"inferred_scan_frame")
self.pub_tf.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
stamp,
"base_link",
"inferred_base_link")
# Also publish the inferred pose as an odometry message in the map frame.
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.angle_to_quaternion(pose[2])
ground_truth_x = self.rel_odom_pose.pose.position.x
ground_truth_y = self.rel_odom_pose.pose.position.y
delta = ((pose[0]-ground_truth_x - math.cos(pose[2]) * 0.265)**2 + (pose[1]-ground_truth_y - math.sin(pose[2]) * 0.265)**2)**0.5
self.odom_pub.publish(odom)
self.ground_truth_delta.publish(delta)
def timer_cb(self, event):
now = rospy.Time.now()
# Publish a tf between map and odom if one does not already exist
# Otherwise, get the most recent tf between map and odom
self.cur_map_to_odom_lock.acquire()
try:
tmp_trans, tmp_rot = self.transformer.lookupTransform(
"/odom", "/map", rospy.Time(0))
self.cur_map_to_odom_trans[0] = tmp_trans[0]
self.cur_map_to_odom_trans[1] = tmp_trans[1]
self.cur_map_to_odom_rot = (
tf.transformations.euler_from_quaternion(tmp_rot))[2]
if tmp_trans[2] == -0.0001:
self.br.sendTransform(
(
self.cur_map_to_odom_trans[0],
self.cur_map_to_odom_trans[1],
0.0001,
),
tf.transformations.quaternion_from_euler(
0, 0, self.cur_map_to_odom_rot),
now,
"/odom",
"/map",
)
except Exception:
self.br.sendTransform(
(self.cur_map_to_odom_trans[0], self.cur_map_to_odom_trans[1],
0.0001),
tf.transformations.quaternion_from_euler(
0, 0, self.cur_map_to_odom_rot),
now,
"/odom",
"/map",
)
self.cur_map_to_odom_lock.release()
# Get the time since the last update
if self.last_stamp is None:
self.last_stamp = now
dt = (now - self.last_stamp).to_sec()
# Add noise to the speed
self.last_speed_lock.acquire()
v = self.last_speed + np.random.normal(
loc=self.SPEED_OFFSET * self.last_speed,
scale=self.SPEED_NOISE,
size=1)
self.last_speed_lock.release()
# Add noise to the steering angle
self.last_steering_angle_lock.acquire()
delta = self.last_steering_angle + np.random.normal(
loc=self.STEERING_ANGLE_OFFSET * self.last_steering_angle,
scale=self.STEERING_ANGLE_NOISE,
size=1,
)
self.last_steering_angle_lock.release()
self.cur_odom_to_base_lock.acquire()
# Apply kinematic car model to the previous pose
new_pose = np.array(
[
self.cur_odom_to_base_trans[0],
self.cur_odom_to_base_trans[1],
self.cur_odom_to_base_rot,
],
dtype=np.float,
)
if np.abs(delta) < 1e-2:
# Changes in x, y, and theta
dtheta = 0
dx = v * np.cos(self.cur_odom_to_base_rot) * dt
dy = v * np.sin(self.cur_odom_to_base_rot) * dt
# New joint values
joint_left_throttle = v * dt / self.CAR_WHEEL_RADIUS
joint_right_throttle = v * dt / self.CAR_WHEEL_RADIUS
joint_left_steer = 0.0
joint_right_steer = 0.0
else:
# Changes in x, y, and theta
tan_delta = np.tan(delta)
dtheta = ((v / self.CAR_LENGTH) * tan_delta) * dt
dx = (self.CAR_LENGTH /
tan_delta) * (np.sin(self.cur_odom_to_base_rot + dtheta) -
np.sin(self.cur_odom_to_base_rot))
dy = (self.CAR_LENGTH / tan_delta) * (
-1 * np.cos(self.cur_odom_to_base_rot + dtheta) +
np.cos(self.cur_odom_to_base_rot))
# New joint values
# Applt kinematic car model to compute wheel deltas
h_val = (self.CAR_LENGTH / tan_delta) - (self.CAR_WIDTH / 2.0)
joint_outer_throttle = (((self.CAR_WIDTH + h_val) /
(0.5 * self.CAR_WIDTH + h_val)) * v * dt /
self.CAR_WHEEL_RADIUS)
joint_inner_throttle = (((h_val) /
(0.5 * self.CAR_WIDTH + h_val)) * v * dt /
self.CAR_WHEEL_RADIUS)
joint_outer_steer = np.arctan2(self.CAR_LENGTH,
self.CAR_WIDTH + h_val)
joint_inner_steer = np.arctan2(self.CAR_LENGTH, h_val)
# Assign joint values according to whether we are turning left or right
if (delta) > 0.0:
joint_left_throttle = joint_inner_throttle
joint_right_throttle = joint_outer_throttle
joint_left_steer = joint_inner_steer
joint_right_steer = joint_outer_steer
else:
joint_left_throttle = joint_outer_throttle
joint_right_throttle = joint_inner_throttle
joint_left_steer = joint_outer_steer - np.pi
joint_right_steer = joint_inner_steer - np.pi
# Apply kinematic model updates and noise to the new pose
new_pose[0] += (dx + np.random.normal(
loc=self.FORWARD_OFFSET, scale=self.FORWARD_FIX_NOISE,
size=1) + np.random.normal(
loc=0.0, scale=np.abs(v) * self.FORWARD_SCALE_NOISE, size=1))
new_pose[1] += (
dy + np.random.normal(
loc=self.SIDE_OFFSET, scale=self.SIDE_FIX_NOISE, size=1) +
np.random.normal(
loc=0.0, scale=np.abs(v) * self.SIDE_SCALE_NOISE, size=1))
new_pose[2] += dtheta + np.random.normal(
loc=self.THETA_OFFSET, scale=self.THETA_FIX_NOISE, size=1)
new_pose[2] = self.clip_angle(new_pose[2])
# Compute the new pose w.r.t the map in meters
new_map_pose = np.zeros(3, dtype=np.float)
new_map_pose[0] = self.cur_map_to_odom_trans[0] + (
new_pose[0] * np.cos(self.cur_map_to_odom_rot) -
new_pose[1] * np.sin(self.cur_map_to_odom_rot))
new_map_pose[1] = self.cur_map_to_odom_trans[1] + (
new_pose[0] * np.sin(self.cur_map_to_odom_rot) +
new_pose[1] * np.cos(self.cur_map_to_odom_rot))
new_map_pose[2] = self.cur_map_to_odom_rot + new_pose[2]
# Get the new pose w.r.t the map in pixels
if self.map_info is not None:
new_map_pose = utils.world_to_map(new_map_pose, self.map_info)
# Update the pose of the car if either bounds checking is not enabled,
# or bounds checking is enabled but the car is in-bounds
new_map_pose_x = int(new_map_pose[0] + 0.5)
new_map_pose_y = int(new_map_pose[1] + 0.5)
if self.permissible_region is None or (
new_map_pose_x >= 0
and new_map_pose_x < self.permissible_region.shape[1]
and new_map_pose_y >= 0
and new_map_pose_y < self.permissible_region.shape[0] and
self.permissible_region[new_map_pose_y, new_map_pose_x] == 1):
# Update pose of base_footprint w.r.t odom
self.cur_odom_to_base_trans[0] = new_pose[0]
self.cur_odom_to_base_trans[1] = new_pose[1]
self.cur_odom_to_base_rot = new_pose[2]
# Update joint values
self.joint_msg.position[0] += joint_left_throttle
self.joint_msg.position[1] += joint_right_throttle
self.joint_msg.position[2] += joint_left_throttle
self.joint_msg.position[3] += joint_right_throttle
self.joint_msg.position[4] = joint_left_steer
self.joint_msg.position[5] = joint_right_steer
# Clip all joint angles
for i in range(len(self.joint_msg.position)):
self.joint_msg.position[i] = self.clip_angle(
self.joint_msg.position[i])
# Publish the tf from odom to base_footprint
self.br.sendTransform(
(self.cur_odom_to_base_trans[0], self.cur_odom_to_base_trans[1],
0.0),
tf.transformations.quaternion_from_euler(
0, 0, self.cur_odom_to_base_rot),
now,
"base_footprint",
"odom",
)
# Publish the joint states
self.joint_msg.header.stamp = now
self.cur_joints_pub.publish(self.joint_msg)
self.last_stamp = now
self.cur_odom_to_base_lock.release()
# Publish current state as a PoseStamped topic
cur_pose = PoseStamped()
cur_pose.header.frame_id = "map"
cur_pose.header.stamp = now
cur_pose.pose.position.x = (self.cur_odom_to_base_trans[0] +
self.cur_map_to_odom_trans[0])
cur_pose.pose.position.y = (self.cur_odom_to_base_trans[1] +
self.cur_map_to_odom_trans[1])
cur_pose.pose.position.z = 0.0
rot = self.cur_odom_to_base_rot + self.cur_map_to_odom_rot
cur_pose.pose.orientation = utils.angle_to_quaternion(rot)
self.state_pub.publish(cur_pose)
def publish_waypoints_viz(self):
green_color = ColorRGBA()
green_color.r = 0.0
green_color.g = 1.0 #or 1.0
green_color.b = 0.0
green_color.a = 1.0
blue_color = ColorRGBA()
blue_color.r = 0
blue_color.g = 0
blue_color.b = 1.0
blue_color.a = 1.0
red_color = ColorRGBA()
red_color.r = 1.0
red_color.g = 0
red_color.b = 0
red_color.a = 1.0
w_id = 0
p_start = Marker()
p_good = MarkerArray()
p_bad = MarkerArray()
p_start.header.frame_id = "map"
#start: is a single waypoint
config = self.start_waypoint_pose[:]
p_start.ns = 'start_waypoint'
p_start.id = w_id
w_id += 1
p_start.header.stamp = rospy.Time()
p_start.action = p_start.ADD
p_start.scale.x = 0.35
p_start.scale.y = 0.35
p_start.scale.z = 0.1
p_start.pose.position.x = config[0]
p_start.pose.position.y = config[1]
p_start.pose.position.z = 0.0
p_start.type = p_start.SPHERE
p_start.color = green_color
p_start.pose.orientation = utils.angle_to_quaternion(0.0)
rospy.sleep(0.5)
self.start_waypoint_pub.publish(p_start)
#good points: an array of waypoints
for i in xrange(len(self.good_waypoint_pose)):
config = self.good_waypoint_pose[i]
marker = Marker()
marker.ns = 'good_waypoint'
marker.id = w_id
w_id += 1
marker.header.stamp = rospy.Time()
marker.action = marker.ADD
marker.scale.x = 0.35
marker.scale.y = 0.35
marker.scale.z = 0.1
marker.header.frame_id = "map"
marker.pose.position.x = config[0]
marker.pose.position.y = config[1]
marker.pose.position.z = 0.0
marker.type = marker.SPHERE
marker.color = blue_color
marker.pose.orientation = utils.angle_to_quaternion(0.0)
p_good.markers.append(marker)
rospy.sleep(0.5)
self.good_waypoint_pub.publish(p_good)
#bad points
for i in xrange(len(self.bad_waypoint_pose)):
config = self.bad_waypoint_pose[i]
marker = Marker()
marker.ns = 'bad_waypoint'
marker.id = w_id
w_id += 1
marker.header.stamp = rospy.Time()
marker.action = marker.ADD
marker.scale.x = 0.35
marker.scale.y = 0.35
marker.scale.z = 0.1
marker.header.frame_id = "map"
marker.pose.position.x = config[0]
marker.pose.position.y = config[1]
marker.pose.position.z = 0.0
marker.type = marker.SPHERE
marker.color = red_color
marker.pose.orientation = utils.angle_to_quaternion(0.0)
p_bad.markers.append(marker)
rospy.sleep(0.5)
self.bad_waypoint_pub.publish(p_bad)
queue_size=1)
pub_init = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped,
queue_size=1)
# set initial pose
init_x = 0.0
init_y = 0.0
init_heading = 0.0
rospy.loginfo("Setting init pose: " + str((init_x, init_y, init_heading)))
init_msg = PoseWithCovarianceStamped()
init_msg.header.stamp = rospy.Time.now()
init_msg.header.frame_id = "/map"
init_msg.pose.pose.position.x = init_x
init_msg.pose.pose.position.y = init_y
init_msg.pose.pose.orientation = angle_to_quaternion(init_heading)
rospy.sleep(1.0)
pub_init.publish(init_msg)
cmdcnt = 0
rate = rospy.Rate(20)
phase = 1
period = 2 * np.pi / (np.tan(steering_angle) * velocity / tires_dist)
print "period =", period, "secs"
period = rospy.Duration(period)
start_time = rospy.Time.now()
while not rospy.is_shutdown():
t = rospy.Time.now()
def rawPathCB(self, msg):
''' 1. Save raw path to raw_path_.
2. Smooth path by gradient descending.
3. Remove point from smoothed path which is too close to its neighborhood points ,or its neighborhood points is too close(which means there is probably a peak in path).
4. Publish the result path
'''
self.raw_path_ = msg
if not isinstance(self.map_data_, OccupancyGrid):
print 'Received raw path, but cannot smooth when map data not received.'
return
diff = self.param_tolerance_ + 1
step = 0
np_path = self.makeNpArray(self.raw_path_)
if not isinstance(np_path, object):
return
new_path = deepcopy(np_path)
while step < self.param_iterations_:
if diff < self.param_tolerance_:
break
step += 1
diff = 0.
pre_path = deepcopy(new_path)
i = 1
while i != new_path.shape[0] - 2:
new_path[i] += self.param_alpha_ * (
pre_path[i] - new_path[i]) + self.param_beta_ * (
new_path[i - 1] + new_path[i + 1] - 2 * new_path[i])
if self.isCollision(new_path[i], self.map_data_,
self.param_margin_):
new_path[i] = deepcopy(pre_path[i])
i += 1
continue
# if np.sum((new_path[i] - new_path[i - 1])**
# 2) < self.param_min_point_dist_ or np.sum(
# (new_path[i] - new_path[i + 1])**
# 2) < self.param_min_point_dist_ or np.sum(
# (new_path[i - 1] - new_path[i + 1])**
# 2) < self.param_min_point_dist_:
if np.sum((new_path[i - 1] - new_path[i + 1])**
2) < self.param_min_point_dist_:
new_path = np.delete(new_path, i, axis=0)
pre_path = np.delete(pre_path, i, axis=0)
i -= 1
i += 1
diff += np.sum((new_path - pre_path)**2)
print 'round: ', step, '; diff: ', diff, '; origin # of points: ', len(
self.raw_path_.poses
), '; result # of points: ', new_path.shape[
0], '; # of deleted points: ', np_path.shape[0] - new_path.shape[0]
smoothed_path = Path()
smoothed_path.header = make_header(self.param_map_frame_)
for i in xrange(new_path.shape[0]):
pose = PoseStamped()
pose.pose.position.x = new_path[i][0]
pose.pose.position.y = new_path[i][1]
pose.pose.position.z = 0
pose.pose.orientation = angle_to_quaternion(0)
smoothed_path.poses.append(pose)
self.pub_smoothed_path_.publish(smoothed_path)
def get_plan(initial_pose, goal_pose, counter):
# Create a publisher to publish the initial pose
init_pose_pub = rospy.Publisher(
INIT_POSE_TOPIC, PoseWithCovarianceStamped,
queue_size=1) # to publish init position x=2500, y=640
# Create a publisher to publish the goal pose
goal_pose_pub = rospy.Publisher(
GOAL_POSE_TOPIC, PoseStamped,
queue_size=1) # create a publisher for goal pose
map_img, map_info = utils.get_map(MAP_TOPIC) # Get and store the map
PWCS = PoseWithCovarianceStamped(
) # create a PoseWithCovarianceStamped() msg
PWCS.header.stamp = rospy.Time.now() # set header timestamp value
PWCS.header.frame_id = "map" # set header frame id value
temp_pose = utils.map_to_world(initial_pose, map_info) # init pose
PWCS.pose.pose.position.x = temp_pose[0]
PWCS.pose.pose.position.y = temp_pose[1]
PWCS.pose.pose.position.z = 0
PWCS.pose.pose.orientation = utils.angle_to_quaternion(
temp_pose[2]
) # set msg orientation to [converted to queternion] value of the yaw angle in the look ahead pose from the path
print "Pubplishing Initial Pose to topic", INIT_POSE_TOPIC
print "Initial ", PWCS.pose
init_pose_pub.publish(
PWCS
) # publish initial pose, now you can add a PoseWithCovariance with topic of "/initialpose" in rviz
if counter == 0:
for i in range(0, 5):
init_pose_pub.publish(PWCS)
rospy.sleep(0.5)
print "Initial Pose Set."
# raw_input("Press Enter")
PS = PoseStamped() # create a PoseStamped() msg
PS.header.stamp = rospy.Time.now() # set header timestamp value
PS.header.frame_id = "map" # set header frame id value
temp_pose = utils.map_to_world(goal_pose, map_info) # init pose
PS.pose.position.x = temp_pose[
0] # set msg x position to value of the x position in the look ahead pose from the path
PS.pose.position.y = temp_pose[
1] # set msg y position to value of the y position in the look ahead pose from the path
PS.pose.position.z = 0 # set msg z position to 0 since robot is on the ground
PS.pose.orientation = utils.angle_to_quaternion(temp_pose[2])
print "Pubplishing Goal Pose to topic", GOAL_POSE_TOPIC
print "\nGoal ", PS.pose
goal_pose_pub.publish(PS)
print "Goal Pose Set"
# raw_input("Press Enter")
print "\nwaiting for plan ", str(counter), "\n"
raw_plan = rospy.wait_for_message(PLAN_POSE_ARRAY_TOPIC, PoseArray)
print "\nPLAN COMPUTED! of type:", type(raw_plan)
path_part_pub = rospy.Publisher(
"/CoolPlan/plan" + str(counter), PoseArray,
queue_size=1) # create a publisher for plan lookahead follower
for i in range(0, 4):
path_part_pub.publish(raw_plan)
rospy.sleep(0.4)
return raw_plan
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.
if not self.PUBLISH_MAP_TO_ODOM:
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.angle_to_quaternion(pose[2])
self.odom_pub.publish(odom)
"""
Our particle filter provides estimates for the "laser" frame
since that is where our laser range estimates are measure from. Thus,
it provides the "map" -> "laser" transform.
However, to make this compatible with libraries such as move_base
in the ROS navigation stack, we need to publish a "map" -> "odom" transform.
"""
if self.PUBLISH_MAP_TO_ODOM:
# 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]))
# Get laser -> odom transform.
t = self.tf_listener.getLatestCommonTime("/laser", "/odom")
laser_odom_pos, laser_odom_quaternion = self.tf_listener.lookupTransform("/laser", "/odom", t)
# Apply laser -> odom transform to map -> laser transform
# This gives a map -> odom transform
map_laser_matrix = self.tf_listener.fromTranslationRotation(map_laser_pos, map_laser_rotation)
laser_odom_matrix = self.tf_listener.fromTranslationRotation(laser_odom_pos, laser_odom_quaternion)
map_odom_matrix = np.dot(map_laser_matrix, laser_odom_matrix)
map_odom_pos = map_odom_matrix[:3, 3]
map_odom_rotation = tf.transformations.quaternion_from_matrix(map_odom_matrix)
# Transform tolerance is the time with which to post-date the transform that is published
# to indicate that this transform is valid into the future
stamp = stamp + rospy.Duration.from_sec(self.TRANSFORM_TOLERANCE)
# Publish transform
self.pub_tf.sendTransform(map_odom_pos, map_odom_rotation, stamp, "/odom", "/map")
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.265, 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")
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()
# also publish odometry to facilitate getting the localization pose
if self.PUBLISH_ODOM:
odom = Odometry()
odom.header = Utils.make_header("map", stamp)
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
self.odom_pub.publish(odom)
#-------------------------- SAMUEL - added - Publish PoseWithCovariance Msg ---------------------------------
if self.PUBLISH_POSE_COVARIANCE:
pose_covariance = PoseWithCovarianceStamped()
pose_covariance.header = Utils.make_header("map", stamp)
pose_covariance.pose.pose.position.x = pose[0]
pose_covariance.pose.pose.position.y = pose[1]
pose_covariance.pose.pose.orientation = Utils.angle_to_quaternion(
pose[2])
# Copy in the Covariance Matrix, Converting from 3-D to 6-D
temp_covariance = self.covariance_generator(
self.expected_square_pose(), self.square_expected_pose())
for i in range(0, 2):
for j in range(0, 2):
pose_covariance.pose.covariance[6 * i +
j] = temp_covariance[i][j]
pose_covariance.pose.covariance[6 * 5 + 5] = temp_covariance[2][2]
self.pose_covariance_pub.publish(pose_covariance)
# Publishing "map -> laser" TF Transform
if self.TF_MODE == 1:
self.pub_tf.sendTransform(
(pose[0], pose[1], 0),
tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp,
"laser", "map")
# Publishing "map -> base_link" TF Transform
elif self.TF_MODE == 2:
self.pub_tf.sendTransform(
(pose[0], pose[1], 0),
tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp,
"base_link", "map")
# Publishing "map -> odom" TF Transform
elif self.TF_MODE == 3:
try:
# "odom -> base" transform
(odom_base_trans, odom_base_rot) = self.tfTL.lookupTransform(
"odom", "base_link", rospy.Time(0))
odom_base_trans_mat = tf.transformations.translation_matrix(
odom_base_trans)
odom_base_rot_mat = tf.transformations.quaternion_matrix(
odom_base_rot)
odom_base_mat = np.dot(odom_base_trans_mat, odom_base_rot_mat)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
# "map -> base" transform
map_base_trans = [pose[0], pose[1], 0]
map_base_rot = tf.transformations.quaternion_from_euler(
0, 0, pose[2])
map_base_trans_mat = tf.transformations.translation_matrix(
map_base_trans)
map_base_rot_mat = tf.transformations.quaternion_matrix(
map_base_rot)
map_base_mat = np.dot(map_base_trans_mat, map_base_rot_mat)
# "base -> map" transform
base_map_mat = tf.transformations.inverse_matrix(map_base_mat)
base_map_trans = tf.transformations.translation_from_matrix(
base_map_mat)
base_map_rot = tf.transformations.quaternion_from_matrix(
base_map_mat)
# "odom -> map" transform
odom_map_mat = np.dot(odom_base_mat, base_map_mat)
# "map -> odom" transform
map_odom_mat = tf.transformations.inverse_matrix(odom_map_mat)
map_odom_trans = tf.transformations.translation_from_matrix(
map_odom_mat)
map_odom_rot = tf.transformations.quaternion_from_matrix(
map_odom_mat)
self.pub_tf.sendTransform(map_odom_trans, map_odom_rot, stamp,
"odom", "map")
else:
print "--------------- WARNING : TF_MODE was wrongly selected ---------------"
#------------------------------------------------------------------------------------------------------------
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, 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")
def measurement(self, pid, pose, pub, best_err=100000.):
''' Calculate the error in path pursuit, given the PID params.
In other words, check the smoothness of the pursuit process.
'''
if self.trajectory.empty():
print 'no trajectory found'
return self.stop()
# print "Start measurement:", pid
# this instructs the trajectory to convert the list of waypoints into a numpy matrix
if self.trajectory.dirty():
self.trajectory.make_np_array()
lookahead_point = np.ndarray(1)
current_pose = deepcopy(pose)
first_angle = True
prev_angle = 0.
int_cte = 0.
diff_cte = 0.
err = 0.
pa = PoseArray()
pa.header = utils.make_header("/map")
step = 0
while isinstance(lookahead_point, np.ndarray) and err < best_err:
pre_pose = current_pose
# step 1
nearest_point, nearest_dist, t, i = utils.nearest_point_on_trajectory(
current_pose[:2], self.trajectory.np_points)
if nearest_dist < self.lookahead:
# step 2
lookahead_point, i2, t2 = \
utils.first_point_on_trajectory_intersecting_circle(current_pose[:2], self.lookahead, self.trajectory.np_points, i+t, wrap=self.wrap)
if i2 == None:
lookahead_point = None
elif nearest_dist < self.max_reacquire:
lookahead_point = nearest_point
else:
lookahead_point = None
if not isinstance(lookahead_point, np.ndarray):
dist = (current_pose[0] - self.trajectory.points[-1][0])**2 + (
current_pose[1] - self.trajectory.points[-1][1])**2
if dist > self.max_reacquire**2:
err += best_err + 1
# print "Cannot get the end, stopping Car"
else:
# print "move_point: ", current_pose
steering_angle = self.determine_steering_angle(
current_pose, lookahead_point)
if first_angle:
prev_angle = steering_angle
int_cte = 0.
first_angle = False
diff_cte = steering_angle - prev_angle
pid_angle = pid[0] * steering_angle + pid[2] * diff_cte + pid[
1] * int_cte
if np.abs(pid_angle) > self.max_angle:
pid_angle = self.max_angle * np.sign(pid_angle)
# TODO: Wrong!
# err += np.abs(pid_angle - prev_angle)
err += nearest_dist + np.abs(prev_angle - pid_angle)
# print "err", err, "; angle", pid_angle, "; current_pose", current_pose
int_cte += pid_angle
prev_angle = pid_angle
current_pose = self.move(current_pose,
self.speed,
pid_angle,
duration=1.0)
dist = (current_pose[0] - pre_pose[0])**2 + (current_pose[1] -
pre_pose[1])**2
if dist < 0.001:
err += (self.lookahead + 1) / (dist + 0.1)
if self.isCollision(current_pose, self.map_data, self.margin):
# print "collision!"
err += best_err + 1
if self.viz:
tmp = Pose()
tmp.position.x = current_pose[0]
tmp.position.y = current_pose[1]
tmp.position.z = 0.
tmp.orientation = utils.angle_to_quaternion(
current_pose[2])
pa.poses.append(tmp)
# if step % 30 == 0:
# pub.publish(pa)
if self.viz and err <= best_err:
print 'Better pid params found.'
pub.publish(pa)
time.sleep(0.1)
# print "End measurement: err", err
return err
AckermannDriveStamped,
queue_size=1)
bag_file = rospy.get_param('~bag_file')
rospy.loginfo('Bag file: ' + str(bag_file))
reverse = rospy.get_param('~reverse')
rospy.loginfo('Reverse: ' + str(reverse))
# Publish initial position
init_pose = PoseWithCovarianceStamped()
init_pose.header.stamp = rospy.Time.now()
init_pose.header.frame_id = 'map'
init_pose.pose.pose.position.x = 0.0
init_pose.pose.pose.position.y = 0.0
init_pose.pose.pose.orientation = utils.angle_to_quaternion(0.0)
rospy.sleep(1) # publisher needs some time before it can publish
rospy.loginfo('Publish initialpose')
p_initpose.publish(init_pose)
# Publish drive info
r = rospy.Rate(20)
bag = rosbag.Bag(bag_file)
for topic, msg, t in bag.read_messages(
topics=['/vesc/low_level/ackermann_cmd_mux/input/teleop']):
if reverse:
msg.drive.speed = -msg.drive.speed
p_nav0.publish(msg)
r.sleep()
bag.close()