69 lines
2 KiB
Python
69 lines
2 KiB
Python
import rclpy
|
|
from rclpy.action import ActionClient
|
|
from rclpy.node import Node
|
|
|
|
from rbs_skill_interfaces.action import MoveitSendPose
|
|
|
|
|
|
class TestMoveToPose(Node):
|
|
|
|
def __init__(self, robot_name: str):
|
|
super().__init__('test_move_to_pose')
|
|
self._action_client = ActionClient(self, MoveitSendPose, "/"+ robot_name + "/move_topose")
|
|
|
|
def send_goal(self, msg):
|
|
goal_msg = MoveitSendPose.Goal()
|
|
# goal_msg.target_pose
|
|
goal_msg.target_pose.position.x = msg["target_pose"]["position"]["x"]
|
|
goal_msg.target_pose.position.y = msg["target_pose"]["position"]["y"]
|
|
goal_msg.target_pose.position.z = msg["target_pose"]["position"]["z"]
|
|
|
|
goal_msg.target_pose.orientation.x = msg["target_pose"]["orientation"]["x"]
|
|
goal_msg.target_pose.orientation.y = msg["target_pose"]["orientation"]["y"]
|
|
goal_msg.target_pose.orientation.z = msg["target_pose"]["orientation"]["z"]
|
|
goal_msg.target_pose.orientation.w = msg["target_pose"]["orientation"]["w"]
|
|
|
|
goal_msg.robot_name = msg["robot_name"]
|
|
goal_msg.end_effector_velocity = msg["end_effector_velocity"]
|
|
goal_msg.end_effector_acceleration = msg["end_effector_acceleration"]
|
|
|
|
self._action_client.wait_for_server()
|
|
|
|
return self._action_client.send_goal_async(goal_msg)
|
|
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
|
|
robot_name = "arm0"
|
|
|
|
action_client = TestMoveToPose(robot_name)
|
|
|
|
msg = {
|
|
"constraint_mode": 0,
|
|
"target_pose": {
|
|
"position": {
|
|
"x": 0.2,
|
|
"y": 0.2,
|
|
"z": 0.35,
|
|
},
|
|
"orientation": {
|
|
"x": 0.0,
|
|
"y": 0.0,
|
|
"z": 0.0,
|
|
"w": 1.0,
|
|
},
|
|
},
|
|
"robot_name": robot_name,
|
|
"end_effector_velocity": 1.0,
|
|
"end_effector_acceleration": 1.0,
|
|
"timeout_seconds": 0.0
|
|
}
|
|
|
|
future = action_client.send_goal(msg)
|
|
|
|
rclpy.spin_until_future_complete(action_client, future)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|