robossembler/runtime
7
0
Fork 0
Code Issues 5 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",

98
rbss_movetopose/rbs_package.json
View file

@ -1,45 +1,59 @@
{ {
"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" },
"BTAction": [ "BTAction": [
{ {
"name": "move", "name": "move",
"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": [],
"typeAction": "ACTION" "output": "",
} "type": "formBuilder"
],
"topicsOut": [],
"Settings": {
"output": {
"params": [
{
"name": "server_name",
"value": "mtp_moveit",
"value2": "mtp_cart"
}, },
{ "isFilled": false
"name": "end_effector_velocity", }
"value": "1.0" ],
}, "result": [],
{ "typeAction": "ACTION"
"name": "end_effector_acceleration",
"value": "1.0"
}
]
},
"type": "formBuilder"
} }
} ],
] "topicsOut": [],
} "Settings": {
"result": "{\"params\": \\${ITEM:Array<ITEM>:[]}}",
"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
}

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