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.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",

98
rbss_movetopose/rbs_package.json
View file

@ -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<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
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