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)