def cmd_cb(self, msg):
data = PoseStamped()
data.header = msg.header
data.pose.position.x = msg.position.x
data.pose.position.y = msg.position.y
data.pose.position.z = msg.position.z
q = to_quaternion(0, 0, msg.yaw)
data.pose.orientation.x = q.orientation.x
data.pose.orientation.y = q.orientation.y
data.pose.orientation.z = q.orientation.z
data.pose.orientation.w = q.orientation.w
self.pub_pose.publish(data)
data = command_acc()
data.header = msg.header
data.command = 0
data.sub_mode = 0
data.comid = 0
data.pos_sp[0] = msg.position.x
data.pos_sp[1] = msg.position.y
data.pos_sp[2] = msg.position.z
data.vel_sp[0] = msg.velocity.x
data.vel_sp[1] = msg.velocity.y
data.vel_sp[2] = msg.velocity.z
data.acc_sp[0] = msg.acceleration.x
data.acc_sp[1] = msg.acceleration.y
data.acc_sp[2] = msg.acceleration.z
data.yaw_sp = msg.yaw * 180 / math.pi
data.yaw_rate_sp = msg.yaw_dot * 180 / math.pi
self.pub_cmd.publish(data)
