From d7e51b71b4b8af0f61af1c40dac67cfa48beb904 Mon Sep 17 00:00:00 2001 From: shalenikol Date: Thu, 23 Jan 2025 18:15:46 +0300 Subject: [PATCH] run skill_movetopose via webservice --- rbs_bt_executor/launch/rbs_bt_web.launch.py | 5 ++ rbss_movetopose/rbs_package.json | 98 ++++++++++++--------- rbss_movetopose/scripts/skill_movetopose.py | 21 +++-- 3 files changed, 73 insertions(+), 51 deletions(-) diff --git a/rbs_bt_executor/launch/rbs_bt_web.launch.py b/rbs_bt_executor/launch/rbs_bt_web.launch.py index cec9d92..707d773 100644 --- a/rbs_bt_executor/launch/rbs_bt_web.launch.py +++ b/rbs_bt_executor/launch/rbs_bt_web.launch.py @@ -4,6 +4,7 @@ @shalenikol release 0.1 @shalenikol release 0.2 BT v.4 + @shalenikol release 0.3 bt_path = os.path.dirname(bt_path) """ from launch import LaunchDescription from launch_ros.actions import Node @@ -16,6 +17,10 @@ def launch_setup(context, *args, **kwargs): bt_path = LaunchConfiguration("bt_path") bt_path = bt_path.perform(context) + if bt_path[-4:] == ".xml": + import os + bt_path = os.path.dirname(bt_path) + # rbs_bt = Node( # package = "rbs_bt_executor", # executable = "rbs_bt_executor", diff --git a/rbss_movetopose/rbs_package.json b/rbss_movetopose/rbs_package.json index 5db26ad..ce47654 100644 --- a/rbss_movetopose/rbs_package.json +++ b/rbss_movetopose/rbs_package.json @@ -1,45 +1,59 @@ { - "skills": [ - { - "sid": "a", - "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, - "Module": { "node_name": "skill_mtp", "name": "MoveToPose", "description": "Move to Pose skill" }, - "Launch": { "executable": "skill_movetopose.py", "package": "rbss_movetopose" }, - "BTAction": [ - { - "name": "move", - "type": "action", - "param": [ - { - "type": "move_to_pose", - "dependency": { "robot_name": "rbs_arm", - "pose": { "position": {"x":0.1, "y":0.1, "z":0.7}, "orientation": {"x":0.55, "y":0.0, "z":0.45, "w": 1.0} } } - } - ], - "typeAction": "ACTION" - } - ], - "topicsOut": [], - "Settings": { - "output": { - "params": [ - { - "name": "server_name", - "value": "mtp_moveit", - "value2": "mtp_cart" + "_id": "670fb72fbe59e3bad409bef4", + "bgColor": "rgba(5, 26, 39, 1)", + "borderColor": "rgba(25, 130, 196, 1)", + "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, + "Module": { "node_name": "skill_mtp", "name": "MoveToPose", "description": "Move to Pose skill" }, + "Launch": { "executable": "skill_movetopose.py", "package": "rbss_movetopose" }, + "BTAction": [ + { + "name": "move", + "type": "action", + "param": [ + { + "type": "formBuilder", + "dependency": { + "result": "{\"robot_name\": \\${ROBOT_NAME:string:rbs_arm}, \"pose\": {\"position\": { \"x\": \\${X:number:0.0}, \"y\": \\${Y:number:0.2}, \"z\": \\${Z:number:0.5} }, \"orientation\": { \"x\": \\${X:number:0.5}, \"y\": \\${Y:number:0.4}, \"z\": \\${Z:number:0.0}, \"w\": \\${W:number:1.0} }}}", + "context": "type ITEM = {\"name\": \\${NAME:string:default}};", + "form": [], + "output": "", + "type": "formBuilder" }, - { - "name": "end_effector_velocity", - "value": "1.0" - }, - { - "name": "end_effector_acceleration", - "value": "1.0" - } - ] - }, - "type": "formBuilder" + "isFilled": false + } + ], + "result": [], + "typeAction": "ACTION" } - } - ] -} + ], + "topicsOut": [], + "Settings": { + "result": "{\"params\": \\${ITEM:Array:[]}}", + "context": "type ITEM = {\"name\": \\${NAME:string:default},\"value\": \\${VALUE:string:default}};", + "form": [], + "output": { + "params": [ + { + "name": "server_name", + "value": "mtp_jtc", + "value2": "mtp_moveit", + "value3": "mtp_cart" + }, + { + "name": "duration", + "value": "5.0" + }, + { + "name": "end_effector_velocity", + "value": "1.0" + }, + { + "name": "end_effector_acceleration", + "value": "1.0" + } + ] + }, + "type": "formBuilder" + }, + "__v": 0 +} \ No newline at end of file diff --git a/rbss_movetopose/scripts/skill_movetopose.py b/rbss_movetopose/scripts/skill_movetopose.py index af1656d..a2846dc 100755 --- a/rbss_movetopose/scripts/skill_movetopose.py +++ b/rbss_movetopose/scripts/skill_movetopose.py @@ -3,7 +3,7 @@ Move_to_pose_cartesian_node_via_interface_node ROS 2 program for Move to Pose skill - @shalenikol release 0.3 + @shalenikol release 0.4 """ import json @@ -74,8 +74,11 @@ class MoveToPoseCartesianSkill(Node): self.dependency = {} for comm in self.skill_cfg["BTAction"]: for par in comm["param"]: - if par["type"] == "move_to_pose": + p_type = par["type"] + if p_type == "move_to_pose": # obsolete version self.dependency = par["dependency"] + elif p_type == "formBuilder": + self.dependency = par["dependency"]["output"] assert self.dependency, "no dependency" self._completion = False # run new action @@ -119,14 +122,14 @@ class MoveToPoseCartesianSkill(Node): def set_goal_msg(self, dep): goal_msg = MoveitSendPose.Goal() pose = dep["pose"] - goal_msg.target_pose.position.x = pose["position"]["x"] - goal_msg.target_pose.position.y = pose["position"]["y"] - goal_msg.target_pose.position.z = pose["position"]["z"] + goal_msg.target_pose.position.x = float(pose["position"]["x"]) + goal_msg.target_pose.position.y = float(pose["position"]["y"]) + goal_msg.target_pose.position.z = float(pose["position"]["z"]) - goal_msg.target_pose.orientation.x = pose["orientation"]["x"] - goal_msg.target_pose.orientation.y = pose["orientation"]["y"] - goal_msg.target_pose.orientation.z = pose["orientation"]["z"] - goal_msg.target_pose.orientation.w = pose["orientation"]["w"] + goal_msg.target_pose.orientation.x = float(pose["orientation"]["x"]) + goal_msg.target_pose.orientation.y = float(pose["orientation"]["y"]) + goal_msg.target_pose.orientation.z = float(pose["orientation"]["z"]) + goal_msg.target_pose.orientation.w = float(pose["orientation"]["w"]) goal_msg.robot_name = dep["robot_name"] goal_msg.duration = self.duration