import rospy from geometry_msgs.msg import PoseStamped from ocs2_msgs.msg import mpc_target_trajectories, mpc_state, mpc_input class TransNode: def __init__(self): self.pose_sub = rospy.Subscriber("right_arm_tf", PoseStamped, self.sub_callback, queue_size=1, buff_size=2048) self.target_pub = rospy.Publisher("/mobile_manipulator_mpc_target", mpc_target_trajectories, queue_size=1) def sub_callback(self, msg): # print(msg) position = msg.pose.position orientation = msg.pose.orientation # print(position, orientation) combine_data = [position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w] # print(combine_data) target = mpc_target_trajectories() state = mpc_state() state.value = combine_data input = mpc_input() input.value = [0, 0, 0, 0, 0, 0] target.timeTrajectory = [0] target.stateTrajectory = [state] target.inputTrajectory = [input] print(target) self.target_pub.publish(target) if __name__ == "__main__": rospy.init_node("transNode") rospy.loginfo("start trans node...") node = TransNode() rospy.spin()