def prediction(odom):
"""
Predict the particle's new location based on Odometry information and particle's location.
:param odom: Odometry information retrieved by odometer sensors.
:return: None
"""
print("entered prediction", len(particles))
odom_delta = odom_to_pose(odom)
delta = ekf.get_odom_delta(odom_delta)
if delta is not None:
tuple_odom = delta.T.tolist()[0]
else:
tuple_odom = 0, 0, 0
delta_x = tuple_odom[0]
delta_y = tuple_odom[1]
delta_theta = tuple_odom[2]
trv_dist = math.sqrt((delta_x ** 2) + (delta_y ** 2))
particle_count = 0
mean = 0
trv_sigma = 0.05
orient_sigma = (0.5 / 360) * math.pi * 2
# Add noises to odometry info and use the result to update every particle.
for each_particle in particles:
each_particle.predict_update(trv_dist, delta_theta, mean, trv_sigma, orient_sigma)
# After updating, if particle's new location is "in the wall", it's definitely a "bad" particle.
if each_particle.loc_check() is False:
each_particle.probability = 0
particle_count += 1
def odom_callback(odom):
# by definition, if there is laser and/or QR data, it was there
# before this odometry came in, so process that first.
if ekf.lastlaser is not None and ekf.uselaser:
ekf.update_pos(ekf.lastlaser)
ekf.lastlaser = None
ekf.uselaser = False
pose = ekf.get_pose()
rospy.loginfo("After laser pose is (%6.3f, %6.3f, %6.3f) cov %s",
pose.x, pose.y, pose.theta, pose.cov)
if ekf.lastqr is not None and ekf.useqr:
ekf.update_pos(ekf.lastqr, True)
ekf.lastqr = None
ekf.useqr = False
pose = ekf.get_pose()
rospy.loginfo("After QR pose is (%6.3f, %6.3f, %6.3f) cov %s", pose.x,
pose.y, pose.theta, pose.cov)
opose = odom_to_pose(odom)
rospy.loginfo("Odom is (%6.3f, %6.3f, %6.3f)", opose.x, opose.y,
opose.theta)
ekf.predict(opose)
#print ekf.get_pose()
pose = ekf.get_pose()
rospy.loginfo("After odom pose is (%6.3f, %6.3f, %6.3f) cov %s", pose.x,
pose.y, pose.theta, pose.cov)
pose_pub.publish(ekf.get_pose())
def odom_callback(odom):
#print "ODOMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMM"
# print odom.pose
# odom = linear twist, angular twist and convariance
# between xl, yl, zl, xa, ya, za = 36 values
# converting odom topic to pose value
ekf.predict(odom_to_pose(odom))
def odom_callback(odom):
# print "ODOMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMMM"
# print odom.pose
# odom = linear twist, angular twist and convariance
# between xl, yl, zl, xa, ya, za = 36 values
# converting odom topic to pose value
ekf.predict(odom_to_pose(odom))
def odom_callback(odometry):
"""
Handles the odometry messages. Once receiving an odometry, calculates the current pose of robot.
:param odometry: The odometry message.
:return: None
"""
global ekf, current_pose, smart_slam, traditional_slam
odom_pose = odom_to_pose(odometry)
ekf.predict(odom_pose)
# current_pose is set as the latest known pose of robot.
current_pose = ekf.get_pose()
# robot_poses records the trajectory of the robot.
last_pose = current_pose -
robot_poses.append(current_pose)
last_pose =
smart_slam.predict()
def odom_callback(odom):
# by definition, if there is laser and/or QR data, it was there
# before this odometry came in, so process that first.
if ekf.lastlaser is not None and ekf.uselaser:
ekf.update_pos(ekf.lastlaser)
ekf.lastlaser = None
ekf.uselaser = False
pose = ekf.get_pose()
rospy.loginfo("After laser pose is (%6.3f, %6.3f, %6.3f) cov %s",pose.x, pose.y,pose.theta,pose.cov)
if ekf.lastqr is not None and ekf.useqr:
ekf.update_pos(ekf.lastqr,True)
ekf.lastqr = None
ekf.useqr = False
pose = ekf.get_pose()
rospy.loginfo("After QR pose is (%6.3f, %6.3f, %6.3f) cov %s",pose.x, pose.y,pose.theta,pose.cov)
opose = odom_to_pose(odom)
rospy.loginfo("Odom is (%6.3f, %6.3f, %6.3f)",opose.x, opose.y,opose.theta)
ekf.predict(opose)
#print ekf.get_pose()
pose = ekf.get_pose()
rospy.loginfo("After odom pose is (%6.3f, %6.3f, %6.3f) cov %s",pose.x, pose.y,pose.theta,pose.cov)
pose_pub.publish(ekf.get_pose())
def odom_comb_callback(odom):
ocpose = odom_to_pose(odom)
rospy.loginfo("Odom combined is (%6.3f, %6.3f, %6.3f)", ocpose.x, ocpose.y,
ocpose.theta)
def odom_comb_callback(odom):
ocpose = odom_to_pose(odom)
rospy.loginfo("Odom combined is (%6.3f, %6.3f, %6.3f)",ocpose.x, ocpose.y,ocpose.theta)
def odom_callback(odom):
ekf.predict(odom_to_pose(odom))
pose_pub.publish(ekf.get_pose())
def odom_callback(odom):
ekf.predict(odom_to_pose(odom))
pose_pub.publish(ekf.get_pose())