robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

run skill_movetopose via webservice

Browse source
This commit is contained in:
shalenikol 2025-01-23 18:15:46 +03:00
parent 07607958c9
commit d7e51b71b4
3 changed files with 73 additions and 51 deletions
Download patch file Download diff file Expand all files Collapse all files

5
rbs_bt_executor/launch/rbs_bt_web.launch.py
View file

@ -4,6 +4,7 @@
@shalenikol release 0.1 @shalenikol release 0.1
@shalenikol release 0.2 BT v.4 @shalenikol release 0.2 BT v.4
@shalenikol release 0.3 bt_path = os.path.dirname(bt_path)
""" """
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch_ros.actions import Node
@ -16,6 +17,10 @@ def launch_setup(context, *args, **kwargs):
bt_path = LaunchConfiguration("bt_path") bt_path = LaunchConfiguration("bt_path")
bt_path = bt_path.perform(context) bt_path = bt_path.perform(context)
if bt_path[-4:] == ".xml":
import os
bt_path = os.path.dirname(bt_path)
# rbs_bt = Node( # rbs_bt = Node(
# package = "rbs_bt_executor", # package = "rbs_bt_executor",
# executable = "rbs_bt_executor", # executable = "rbs_bt_executor",

36
rbss_movetopose/rbs_package.json
View file

@ -1,7 +1,7 @@
{ {
"skills": [ "_id": "670fb72fbe59e3bad409bef4",
{ "bgColor": "rgba(5, 26, 39, 1)",
"sid": "a", "borderColor": "rgba(25, 130, 196, 1)",
"SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" },
"Module": { "node_name": "skill_mtp", "name": "MoveToPose", "description": "Move to Pose skill" }, "Module": { "node_name": "skill_mtp", "name": "MoveToPose", "description": "Move to Pose skill" },
"Launch": { "executable": "skill_movetopose.py", "package": "rbss_movetopose" }, "Launch": { "executable": "skill_movetopose.py", "package": "rbss_movetopose" },
@ -11,22 +11,37 @@
"type": "action", "type": "action",
"param": [ "param": [
{ {
"type": "move_to_pose", "type": "formBuilder",
"dependency": { "robot_name": "rbs_arm", "dependency": {
"pose": { "position": {"x":0.1, "y":0.1, "z":0.7}, "orientation": {"x":0.55, "y":0.0, "z":0.45, "w": 1.0} } } "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"
},
"isFilled": false
} }
], ],
"result": [],
"typeAction": "ACTION" "typeAction": "ACTION"
} }
], ],
"topicsOut": [], "topicsOut": [],
"Settings": { "Settings": {
"result": "{\"params\": \\${ITEM:Array<ITEM>:[]}}",
"context": "type ITEM = {\"name\": \\${NAME:string:default},\"value\": \\${VALUE:string:default}};",
"form": [],
"output": { "output": {
"params": [ "params": [
{ {
"name": "server_name", "name": "server_name",
"value": "mtp_moveit", "value": "mtp_jtc",
"value2": "mtp_cart" "value2": "mtp_moveit",
"value3": "mtp_cart"
},
{
"name": "duration",
"value": "5.0"
}, },
{ {
"name": "end_effector_velocity", "name": "end_effector_velocity",
@ -39,7 +54,6 @@
] ]
}, },
"type": "formBuilder" "type": "formBuilder"
} },
} "__v": 0
]
} }

21
rbss_movetopose/scripts/skill_movetopose.py
View file

@ -3,7 +3,7 @@
Move_to_pose_cartesian_node_via_interface_node Move_to_pose_cartesian_node_via_interface_node
ROS 2 program for Move to Pose skill ROS 2 program for Move to Pose skill
@shalenikol release 0.3 @shalenikol release 0.4
""" """
import json import json
@ -74,8 +74,11 @@ class MoveToPoseCartesianSkill(Node):
self.dependency = {} self.dependency = {}
for comm in self.skill_cfg["BTAction"]: for comm in self.skill_cfg["BTAction"]:
for par in comm["param"]: 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"] self.dependency = par["dependency"]
elif p_type == "formBuilder":
self.dependency = par["dependency"]["output"]
assert self.dependency, "no dependency" assert self.dependency, "no dependency"
self._completion = False # run new action self._completion = False # run new action
@ -119,14 +122,14 @@ class MoveToPoseCartesianSkill(Node):
def set_goal_msg(self, dep): def set_goal_msg(self, dep):
goal_msg = MoveitSendPose.Goal() goal_msg = MoveitSendPose.Goal()
pose = dep["pose"] pose = dep["pose"]
goal_msg.target_pose.position.x = pose["position"]["x"] goal_msg.target_pose.position.x = float(pose["position"]["x"])
goal_msg.target_pose.position.y = pose["position"]["y"] goal_msg.target_pose.position.y = float(pose["position"]["y"])
goal_msg.target_pose.position.z = pose["position"]["z"] goal_msg.target_pose.position.z = float(pose["position"]["z"])
goal_msg.target_pose.orientation.x = pose["orientation"]["x"] goal_msg.target_pose.orientation.x = float(pose["orientation"]["x"])
goal_msg.target_pose.orientation.y = pose["orientation"]["y"] goal_msg.target_pose.orientation.y = float(pose["orientation"]["y"])
goal_msg.target_pose.orientation.z = pose["orientation"]["z"] goal_msg.target_pose.orientation.z = float(pose["orientation"]["z"])
goal_msg.target_pose.orientation.w = pose["orientation"]["w"] goal_msg.target_pose.orientation.w = float(pose["orientation"]["w"])
goal_msg.robot_name = dep["robot_name"] goal_msg.robot_name = dep["robot_name"]
goal_msg.duration = self.duration goal_msg.duration = self.duration