ocs2_code_2/script/transport.py
2024-11-06 09:43:33 +08:00

31 lines
1.2 KiB
Python
Executable File

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()