def move_cb(self, msg):
to_send = PoseStamped()
to_send.header.frame_id = "/enu"
R = trns.quaternion_matrix([
self.odom.pose.pose.orientation.x,
self.odom.pose.pose.orientation.y,
self.odom.pose.pose.orientation.z,
self.odom.pose.pose.orientation.w
])[:2, :2]
theta = gh.quat_to_euler(self.odom.pose.pose.orientation)
current = numpy.array([
self.odom.pose.pose.position.x, self.odom.pose.pose.position.y,
numpy.rad2deg(theta[2])
])
shift = numpy.concatenate((R.dot([msg.x, msg.y]), [msg.z]))
desired = current + shift
desired_quaternion = trns.quaternion_from_euler(
0, 0, numpy.deg2rad(desired[2]))
to_send.pose.position.x = desired[0]
to_send.pose.position.y = desired[1]
to_send.pose.orientation = numpy_to_quaternion(desired_quaternion)
self.pose_pub.publish(to_send)
def pack_odom(self, pose, twist):
"""
Converts pose and twist into an Odometry message
"""
msg = Odometry()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.world_frame
msg.child_frame_id = self.body_frame
msg.pose.pose.position.x, msg.pose.pose.position.y = pose[0], pose[1]
quat = trns.quaternion_from_euler(0, 0, pose[2])
msg.pose.pose.orientation = numpy_to_quaternion(quat)
msg.twist.twist.linear.x, msg.twist.twist.linear.y = twist[0:2]
msg.twist.twist.angular.z = twist[2]
return msg
def move_cb(self, msg):
to_send = PoseStamped()
to_send.header.frame_id = "/enu"
R = trns.quaternion_matrix([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y,
self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w])[:2, :2]
theta = gh.quat_to_euler(self.odom.pose.pose.orientation)
current = numpy.array([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, numpy.rad2deg(theta[2])])
shift = numpy.concatenate((R.dot([msg.x, msg.y]), [msg.z]))
desired = current + shift
desired_quaternion = trns.quaternion_from_euler(0, 0, numpy.deg2rad(desired[2]))
to_send.pose.position.x = desired[0]
to_send.pose.position.y = desired[1]
to_send.pose.orientation = numpy_to_quaternion(desired_quaternion)
self.pose_pub.publish(to_send)