first commit

script/transport.py

@@ -0,0 +1,31 @@
+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()