31 lines
1.2 KiB
Python
Executable File
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() |