def pose_callback(self, pose_msg):
'''
Uses a pose message to generate an odometry and state message.
:param pose_msg: (geometry_msgs/PoseStamped) pose message
'''
new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map",
pose_msg.header)
if not self.use_amcl:
pose_msg = new_pose_msg
self.new_pose_pub.publish(
pose_msg
) # if using AMCL, this will be the same as the original pose message
if trans and rot: # if not getting tf, trans and rot will be None
footprint = PolygonStamped()
footprint.header = pose_msg.header # has same frame_id (map) and time stamp
loomo_points = np.array([[0.16, -0.31], [0.16, 0.31],
[-0.16, 0.31], [-0.16, -0.31]])
roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot)
rot = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array
rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]])
polygon = Polygon(
points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T])
footprint.polygon = polygon
self.footprint_pub.publish(footprint)
odom_msg = Odometry()
odom_msg.pose.pose = pose_msg.pose
odom_msg.header = pose_msg.header
self.odom_pub.publish(odom_msg)
state_msg = State()
state_msg.header = pose_msg.header
state_msg.state_stamp = pose_msg.header.stamp
state_msg.pos.x = pose_msg.pose.position.x
state_msg.pos.y = pose_msg.pose.position.y
state_msg.pos.z = pose_msg.pose.position.z
state_msg.quat.x = pose_msg.pose.orientation.x
state_msg.quat.y = pose_msg.pose.orientation.y
state_msg.quat.z = pose_msg.pose.orientation.z
state_msg.quat.w = pose_msg.pose.orientation.w
if self.last_state_time and self.last_state:
dt = pose_msg.header.stamp.nsecs - self.last_state_time
state_msg.vel.x = (state_msg.pos.x -
self.last_state.pos.x) / (float(dt) / 10**9)
state_msg.vel.y = (state_msg.pos.y -
self.last_state.pos.y) / (float(dt) / 10**9)
state_msg.vel.z = 0 # ground robot not traveling in z direction
self.last_state_time = pose_msg.header.stamp.nsecs
self.last_state = state_msg
self.state_pub.publish(state_msg)
def publishMavstate(self):
msg = State()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'map'
msg.state_stamp = rospy.Time.now()
msg.pos.x = self.mavpos_[0]
msg.pos.y = self.mavpos_[1]
msg.pos.z = self.mavpos_[2]
msg.quat.x = self.mavrot_[0]
msg.quat.y = self.mavrot_[1]
msg.quat.z = self.mavrot_[2]
msg.quat.w = self.mavrot_[3]
msg.vel.x = self.mav_linear_vel_[0]
msg.vel.y = self.mav_linear_vel_[1]
msg.vel.z = self.mav_linear_vel_[2]
msg.w.x = self.mav_angular_vel_[0]
msg.w.y = self.mav_angular_vel_[1]
msg.w.z = self.mav_angular_vel_[2]
self.mavstatePub_.publish(msg)
