def update_pose(self, msg):
# YOUR CODE HERE
# Compute the pose of the clone
# Note: To convert from a message quaternion to corresponding rotation matrix,
# look at the functions in Utils.py
x = msg.Point.x
y = msg.Point.y
quant = msg.Quaternion
theta = Utils.quaternion_to_angle(quant)
rot = Utils.rotation_matrix(theta)
x_, y_ = self.compute_follow_pose(trans=[self.follow_offset, 0],
rot=rot)
# Check bounds if required
if self.force_in_bounds:
# Functions in Utils.py will again be useful here
if ((x >= self.map_img[1]) | (x <= 0) | (y >= self.map_img[0]) |
(y <= 0)):
x_, y_ = self.compute_follow_pose(
trans=[-1 * self.follow_offset, 0], rot=rot)
# Setup the out going PoseStamped message
###### FIX THIS #######
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = '/map'
world_pose = [x_, y_, rot]
map_pose = Utils.world_to_map(world_pose, self.map_info)
pose.Point.x = map_pose[0]
pose.Point.y = map_pose[1]
pose.Quaternion = map_pose[2]
# Publish the clone's pose
pub.publish(pose)
