def odom_to_pose(odom):
pose = Pose()
pose.x = odom.pose.pose.position.x
pose.y = odom.pose.pose.position.y,
pose.theta = quaternion_to_angle(odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w)
return pose
def odom_to_pose(odom):
pose = Pose()
pose.x = odom.pose.pose.position.x
pose.y = odom.pose.pose.position.y,
pose.theta = quaternion_to_angle(odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w)
return pose
def odom_to_pose(odom):
pose = Pose()
pose.header = odom.header
pose.x = odom.pose.pose.position.x
pose.y = odom.pose.pose.position.y
qz = odom.pose.pose.orientation.z
qw = odom.pose.pose.orientation.w
pose.theta = atan2(2*qw*qz, 1-2*qz*qz)
return pose
def odom_to_pose(odom):
"""Utility function to convert an Odometry message into a Pose message."""
pose = Pose()
pose.header = odom.header
pose.x = odom.pose.pose.position.x
pose.y = odom.pose.pose.position.y
qz = odom.pose.pose.orientation.z
qw = odom.pose.pose.orientation.w
pose.theta = atan2(2 * qw * qz, 1 - 2 * qz * qz)
pose.cov = reduce_covariance(odom.pose.covariance)
return pose
def odom_to_pose(odom):
"""Utility function to convert an Odometry message into a Pose message."""
pose = Pose()
pose.header = odom.header
pose.x = odom.pose.pose.position.x
pose.y = odom.pose.pose.position.y
qz = odom.pose.pose.orientation.z
qw = odom.pose.pose.orientation.w
pose.theta = atan2(2 * qw * qz, 1 - 2 * qz * qz)
pose.cov = reduce_covariance(odom.pose.covariance)
return pose
def calc_lmpose(robot_pose, dist, angle, camera_angle):
"""
Calculate the pose of a landmark, given robot's pose and its camera readings.
:param robot_pose: Robot pose.
:param dist: Range reading given by robot camera.
:param angle: Angle reading given by robot camera.
:param camera_angle: The angle between camera's orientation and robot's orientation. -pi / 2 or pi / 2.
:return: Pose of a landmark.
"""
lm_pose = Pose()
lm_pose.header.frame_id = "landmark"
robot_lm_angle = robot_pose.theta + camera_angle + angle
lm_pose.x = math.cos(robot_lm_angle) * dist
lm_pose.y = math.sin(robot_lm_angle) * dist
lm_pose.theta = 0
lm_pose.theta = 0
return lm_pose
def laser_callback(scan):
rospy.loginfo("laser callback, pose (%.4g, %.4g, %.4g)", pose.x, pose.y,
pose.theta)
global posepub
global lockedOn
# upon startup we won't have a place to bootstrap from, so ignore
if pose.x < 0.1 and pose.y < 0.1:
return
currentPose = pose
samplePoints = get_sample_points(pose)
#samplePoints = [(pose.x, pose.y, pose.theta)]
sampleProbability = []
#rospy.loginfo("%s", samplePoints)
sample = pose
maxprob = 0
for sample in samplePoints:
newLaser = get_expected_scan(sample)
(currentProbability,
goodscans) = get_sample_probability(scan.ranges, newLaser)
if currentProbability > maxprob:
maxprob = currentProbability
sampleProbability.append(currentProbability)
#rospy.loginfo("Prob: %g",currentProbability)
sum_x = 0.0
sum_y = 0.0
sum_theta = 0.0
count = 0.0
for i in range(0, len(samplePoints)):
sample = samplePoints[i]
sum_x += sampleProbability[i] * sample[0]
sum_y += sampleProbability[i] * sample[1]
sum_theta += sampleProbability[i] * sample[2]
count = count + sampleProbability[i]
rospy.loginfo("Prob: %6.3g from %d scan points", count, goodscans)
mean_pose = [sum_x / count, sum_y / count, sum_theta / count]
rospy.loginfo("Mean pose: (%f, %f, %f)", mean_pose[0], mean_pose[1],
mean_pose[2])
# if we don't like any of the samples, don't say anything.
# this is probably wrong, but may help when lost or when
# there are too many obstacles about.
if maxprob < (probThresh**goodscans) or goodscans <= 10:
rospy.loginfo("Skipping laser estimate, thresh = %6.3g",
probThresh**goodscans)
lockedOn = False
# we'll spit out a bogus pose to let the GUI know we're still alive
posemsg = Pose()
posemsg.x = -1000
posemsg.y = -1000
posemsg.theta = 0
posemsg.cov = [0, 0, 0, 0, 0, 0, 0, 0, 0]
posepub.publish(posemsg)
return
lockedOn = True
sum_cx = 0.0
sum_cy = 0.0
sum_ctheta = 0.0
sum_xy = 0.0
sum_xt = 0.0
sum_yt = 0.0
#cCount = len(samplePoints) - 1
#cCount = 0
sum_wt = 0.0
sum_wt2 = 0.0
for i in range(0, len(samplePoints)):
sample = samplePoints[i]
xx = (sample[0] - mean_pose[0])
yy = (sample[1] - mean_pose[1])
tt = (sample[2] - mean_pose[2])
wt = sampleProbability[i] / count
#rospy.loginfo("%s at P = %5.3g",sample,wt)
sum_cx += xx * xx * wt
sum_cy += yy * yy * wt
sum_ctheta += tt * tt * wt
sum_xy += xx * yy * wt
sum_xt += xx * tt * wt
sum_yt += yy * tt * wt
sum_wt += wt
sum_wt2 += wt * wt
#cCount = sum_wt/(sum_wt*sum_wt - sum_wt2)
cCount = 1 #sum_wt
# if we are near a discontinuity in the underlying distribution,
# the covariance will be bogus (not really Gaussian). So for now
# we just don't report, but could consider sending mean pose
# with a default covariance matrix.
if sum_cx / cCount < 1e-6 or sum_cy / cCount < 1e-6 or sum_ctheta / cCount < 1e-6:
rospy.loginfo("Modifying laser estimate, diag(cov) = %.3g, %.3g, %.3g",\
sum_cx/cCount, sum_cy/cCount, sum_ctheta/cCount)
# Use a sloppy independent covariance here, I guess?
covariance = [max(sum_cx/cCount, 1e-6), 0, 0, \
0, max(sum_cy/cCount, 1e-6), 0, \
0, 0, max(sum_ctheta/cCount, 1e-6)]
return
else:
covariance = [sum_cx/cCount, sum_xy/cCount, sum_xt/cCount, \
sum_xy/cCount, sum_cy/cCount, sum_yt/cCount, \
sum_xt/cCount, sum_yt/cCount, sum_ctheta/cCount]
#rospy.loginfo("Covariance: %s", coveriance_pose)
posemsg = Pose()
# convert from Kinect pose back to robot center pose
posemsg.x = (mean_pose[0] - kinectOffset * math.cos(mean_pose[2])) - pose.x
posemsg.y = (mean_pose[1] - kinectOffset * math.sin(mean_pose[2])) - pose.y
posemsg.theta = mean_pose[2] - pose.theta
posemsg.cov = covariance
posepub.publish(posemsg)
def laser_callback(scan):
rospy.loginfo("laser callback, pose (%.4g, %.4g, %.4g)",pose.x,pose.y,pose.theta)
global posepub
global lockedOn
# upon startup we won't have a place to bootstrap from, so ignore
if pose.x < 0.1 and pose.y < 0.1:
return
currentPose = pose
samplePoints = get_sample_points(pose)
#samplePoints = [(pose.x, pose.y, pose.theta)]
sampleProbability = []
#rospy.loginfo("%s", samplePoints)
sample = pose
maxprob = 0
for sample in samplePoints:
newLaser = get_expected_scan(sample)
(currentProbability, goodscans) = get_sample_probability(scan.ranges, newLaser)
if currentProbability > maxprob:
maxprob = currentProbability
sampleProbability.append(currentProbability)
#rospy.loginfo("Prob: %g",currentProbability)
sum_x = 0.0
sum_y = 0.0
sum_theta = 0.0
count = 0.0
for i in range(0, len(samplePoints)):
sample = samplePoints[i]
sum_x += sampleProbability[i] * sample[0]
sum_y += sampleProbability[i] * sample[1]
sum_theta += sampleProbability[i] * sample[2]
count = count + sampleProbability[i]
rospy.loginfo("Prob: %6.3g from %d scan points", count, goodscans)
mean_pose = [sum_x/count, sum_y/count, sum_theta/count]
rospy.loginfo("Mean pose: (%f, %f, %f)",mean_pose[0],mean_pose[1],mean_pose[2])
# if we don't like any of the samples, don't say anything.
# this is probably wrong, but may help when lost or when
# there are too many obstacles about.
if maxprob < (probThresh**goodscans) or goodscans <= 10:
rospy.loginfo("Skipping laser estimate, thresh = %6.3g",probThresh**goodscans)
lockedOn = False
# we'll spit out a bogus pose to let the GUI know we're still alive
posemsg = Pose()
posemsg.x = -1000
posemsg.y = -1000
posemsg.theta = 0
posemsg.cov = [0, 0, 0, 0, 0, 0, 0, 0, 0]
posepub.publish(posemsg)
return
lockedOn = True
sum_cx = 0.0
sum_cy = 0.0
sum_ctheta = 0.0
sum_xy = 0.0
sum_xt = 0.0
sum_yt = 0.0
#cCount = len(samplePoints) - 1
#cCount = 0
sum_wt = 0.0
sum_wt2 = 0.0
for i in range(0, len(samplePoints)):
sample = samplePoints[i]
xx = (sample[0] - mean_pose[0])
yy = (sample[1] - mean_pose[1])
tt = (sample[2] - mean_pose[2])
wt = sampleProbability[i]/count
#rospy.loginfo("%s at P = %5.3g",sample,wt)
sum_cx += xx * xx * wt
sum_cy += yy * yy * wt
sum_ctheta += tt * tt * wt
sum_xy += xx * yy * wt
sum_xt += xx * tt * wt
sum_yt += yy * tt * wt
sum_wt += wt
sum_wt2 += wt*wt
#cCount = sum_wt/(sum_wt*sum_wt - sum_wt2)
cCount = 1#sum_wt
# if we are near a discontinuity in the underlying distribution,
# the covariance will be bogus (not really Gaussian). So for now
# we just don't report, but could consider sending mean pose
# with a default covariance matrix.
if sum_cx/cCount < 1e-6 or sum_cy/cCount < 1e-6 or sum_ctheta/cCount < 1e-6:
rospy.loginfo("Modifying laser estimate, diag(cov) = %.3g, %.3g, %.3g",\
sum_cx/cCount, sum_cy/cCount, sum_ctheta/cCount)
# Use a sloppy independent covariance here, I guess?
covariance = [max(sum_cx/cCount, 1e-6), 0, 0, \
0, max(sum_cy/cCount, 1e-6), 0, \
0, 0, max(sum_ctheta/cCount, 1e-6)]
else:
covariance = [sum_cx/cCount, sum_xy/cCount, sum_xt/cCount, \
sum_xy/cCount, sum_cy/cCount, sum_yt/cCount, \
sum_xt/cCount, sum_yt/cCount, sum_ctheta/cCount]
#rospy.loginfo("Covariance: %s", coveriance_pose)
posemsg = Pose()
# convert from Kinect pose back to robot center pose
posemsg.x = (mean_pose[0] - kinectOffset*math.cos(mean_pose[2])) - pose.x
posemsg.y = (mean_pose[1] - kinectOffset*math.sin(mean_pose[2])) - pose.y
posemsg.theta = mean_pose[2] - pose.theta
posemsg.cov = covariance
posepub.publish(posemsg)
def convertRobotCoorToGlobalCoor(self, polarPoint):
sp = Pose()
sp.x = self.pose.x + polarPoint.d * cos(self.pose.theta + polarPoint.a)
sp.y = self.pose.y + polarPoint.d * sin(self.pose.theta + polarPoint.a)
sp.theta = 0
return sp
