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

update move to pose

Browse source
This commit is contained in:
shalenikol 2024-10-18 18:23:53 +03:00
parent 16ec90cabe
commit d6cd0d5601
3 changed files with 102 additions and 82 deletions
Download patch file Download diff file Expand all files Collapse all files

38
rbss_movetopose/rbs_package.json
View file

@ -1,7 +1,7 @@
{ {
"SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" },
"Module": { "name": "MoveToPose", "description": "Move to Pose skill with cartesian controllers" }, "Module": { "name": "MoveToPose", "description": "Move to Pose skill with cartesian controllers", "node_name": "mtp_cartesian" },
"Launch": { "package": "rbss_movetopose", "executable": "mtp_cartesian.py", "name": "mtp_cartesian" }, "Launch": { "package": "rbss_movetopose", "executable": "mtp_cartesian.py" },
"BTAction": [ "BTAction": [
{ {
"name": "move", "name": "move",
@ -15,16 +15,28 @@
] ]
} }
], ],
"Interface": { "Settings": {
"Input": [ "result": "{\n  \"params\": \\${ITEM:Array<ITEM>:[]}\n}",
{ "name": "robotName", "type": "ROBOT" }, "context": "type ITEM = {\n\"name\": \\${NAME:string:default},\n\"value\": \\${VALUE:string:default}\n};",
{ "name": "pose", "type": "POSE" } "form": [
"{\"name\":\"ITEM\",\"type\":\"Array\",\"defaultValue\":\"[]\",\"values\":[{\"name\":\"NAME\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"7d70fba4-63b5-424b-8d6c-16d59a77e037\",\"totalValue\":\"mesh_scale\\n\"},{\"name\":\"VALUE\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"0d8d8ee4-ba61-4904-beeb-cda218506907\",\"totalValue\":\"0.001\\n\"}],\"totalValue\":[[{\"name\":\"NAME\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"7d70fba4-63b5-424b-8d6c-16d59a77e037\",\"totalValue\":\"publishDelay\\n\"},{\"name\":\"VALUE\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"0d8d8ee4-ba61-4904-beeb-cda218506907\",\"totalValue\":\"0.5\\n\"}],[{\"name\":\"NAME\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"7d70fba4-63b5-424b-8d6c-16d59a77e037\",\"totalValue\":\"publishDelay\\n\"},{\"name\":\"VALUE\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"0d8d8ee4-ba61-4904-beeb-cda218506907\",\"totalValue\":\"0.5\\n\"}]],\"isOpen\":true,\"subType\":\"ITEM\",\"id\":\"767a59ff-3d25-4c34-84a2-fa5baf074394\"}"
], ],
"Output": [] "output": {
}, "params": [
"Settings": [ {
{ "name": "server_name", "value": "cartesian_move_to_pose" }, "name": "server_name",
{ "name": "end_effector_velocity", "value": 1.0 }, "value": "cartesian_move_to_pose"
{ "name": "end_effector_acceleration", "value": 1.0 } },
] {
"name": "end_effector_velocity",
"value": "1.0"
},
{
"name": "end_effector_acceleration",
"value": "1.0"
}
]
},
"type": "formBuilder"
}
} }

18
rbss_movetopose/scripts/mtp_cartesian.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.1 @shalenikol release 0.2
""" """
import json import json
import math import math
@ -20,7 +20,7 @@ from geometry_msgs.msg import Pose, PoseStamped
from rbs_skill_interfaces.action import MoveitSendPose from rbs_skill_interfaces.action import MoveitSendPose
NODE_NAME_DEFAULT = "mtp_cartesian" # the name doesn't matter in this node (defined in Launch) NODE_NAME_DEFAULT = "mtp_cartesian" # this name must match the name in the description (["Module"]["node_name"])
PARAM_SKILL_CFG = "mtp_cartesian_cfg" PARAM_SKILL_CFG = "mtp_cartesian_cfg"
SERVER_NAME = "cartesian_movetopose" SERVER_NAME = "cartesian_movetopose"
TOPIC_CURRENT_POSE = "/cartesian_motion_controller/current_pose" TOPIC_CURRENT_POSE = "/cartesian_motion_controller/current_pose"
@ -32,15 +32,19 @@ class MoveToPoseCartesianSkill(Node):
""" <Move to pose> skill node """ """ <Move to pose> skill node """
def _Settings(self): def _Settings(self):
# Initialization service settings # Initialization service settings
for prop in self.skill_cfg["Settings"]: for prop in self.skill_cfg["Settings"]["output"]["params"]:
nam = prop["name"] nam = prop["name"]
val = prop["value"] val = prop["value"]
if nam == "server_name": if nam == "server_name":
self.server_name = val self.server_name = val
elif nam == "end_effector_velocity": elif nam == "end_effector_velocity":
self.end_effector_velocity = val self.end_effector_velocity = float(val)
elif nam == "end_effector_acceleration": elif nam == "end_effector_acceleration":
self.end_effector_acceleration = val self.end_effector_acceleration = float(val)
# for prop in self.skill_cfg["topicsOut"]:
# if prop["type"] == OUT_TOPIC_TYPE:
# self.topicSrv = prop["name"]
def __init__(self, **kwargs): def __init__(self, **kwargs):
self.end_effector_velocity = 1.0 self.end_effector_velocity = 1.0
@ -52,9 +56,9 @@ class MoveToPoseCartesianSkill(Node):
str_cfg = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value str_cfg = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value
self.skill_cfg = json.loads(str_cfg) self.skill_cfg = json.loads(str_cfg)
self.nodeName = self.skill_cfg["Module"]["node_name"] #["Launch"]["name"] self.nodeName = NODE_NAME_DEFAULT
self.server_name = SERVER_NAME #"cartesian_move_to_pose" self.server_name = SERVER_NAME
self._Settings() self._Settings()
self._action_client = ActionClient(self, MoveitSendPose, self.server_name) # "/"+ robot_name + self._action_client = ActionClient(self, MoveitSendPose, self.server_name) # "/"+ robot_name +

128
rbss_poseestimation/rbs_package.json
View file

@ -1,75 +1,79 @@
{ {
"_id": "670cedc848170be91d62984a",
"bgColor": "rgba(5, 26, 39, 1)",
"borderColor": "rgba(25, 130, 196, 1)",
"sid": "cca620d1-bb94-4430-ba2f-874ec49ad43f",
"SkillPackage": { "SkillPackage": {
"name": "Robossembler", "name": "Robossembler",
"version": "1.0", "version": "1.0",
"format": "1" "format": "1"
}, },
"Module": { "Module": {
"name": "PoseEstimation", "node_name": "lc_dope",
"description": "Pose Estimation skill with DOPE", "name": "PoseEstimation",
"node_name": "lc_dope" "description": "Pose Estimation skill with DOPE"
}, },
"Launch": { "Launch": {
"package": "rbss_poseestimation", "executable": "pe_dope_lc.py",
"executable": "pe_dope_lc.py" "package": "rbss_poseestimation"
}, },
"BTAction": [ "BTAction": [
{ {
"name": "peConfigure", "name": "peConfigure",
"type": "run", "type": "run",
"param": [ "param": [
{ {
"type": "weights", "type": "weights",
"dependency": { "dependency": {
"object_name": "knight", "object_name": "knight",
"weights_file": "/home/shalenikol/0_rbs/w_knight.pth", "weights_file": "/home/shalenikol/0_rbs/w_knight.pth",
"dimensions": [ "dimensions": [ 0.03, 0.026, 0.065 ]
0.03, }
0.026, },
0.065 {
] "type": "topic",
} "dependency": {
} "type": "topic",
] "topicType": "sensor_msgs/msg/Image",
}, "topicOut": "/rgbd_camera/image"
{ },
"name": "peStop", "isFilled": true
"type": "stop", },
"param": [], {
"result": [] "type": "topic",
} "dependency": {
"type": "topic",
"topicType": "sensor_msgs/msg/CameraInfo",
"topicOut": "/rgbd_camera/camera_info"
},
"isFilled": true
}
],
"result": [],
"typeAction": "ACTION"
}
], ],
"Interface": { "topicsOut": [
"Input": [
{ {
"name": "cameraLink", "name": "lc_dope/pose_estimation",
"type": "CAMERA" "type": "geometry_msgs/msg/Pose"
}
],
"Settings": {
"result": "{\n  \"params\": \\${ITEM:Array<ITEM>:[]}\n}",
"context": "type ITEM = {\n\"name\": \\${NAME:string:default},\n\"value\": \\${VALUE:string:default}\n};",
"form": [
"{\"name\":\"ITEM\",\"type\":\"Array\",\"defaultValue\":\"[]\",\"values\":[{\"name\":\"NAME\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"7d70fba4-63b5-424b-8d6c-16d59a77e037\",\"totalValue\":\"mesh_scale\\n\"},{\"name\":\"VALUE\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"0d8d8ee4-ba61-4904-beeb-cda218506907\",\"totalValue\":\"0.001\\n\"}],\"totalValue\":[[{\"name\":\"NAME\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"7d70fba4-63b5-424b-8d6c-16d59a77e037\",\"totalValue\":\"publishDelay\\n\"},{\"name\":\"VALUE\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"0d8d8ee4-ba61-4904-beeb-cda218506907\",\"totalValue\":\"0.5\\n\"}],[{\"name\":\"NAME\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"7d70fba4-63b5-424b-8d6c-16d59a77e037\",\"totalValue\":\"publishDelay\\n\"},{\"name\":\"VALUE\",\"type\":\"string\",\"defaultValue\":\"default\",\"isOpen\":false,\"id\":\"0d8d8ee4-ba61-4904-beeb-cda218506907\",\"totalValue\":\"0.5\\n\"}]],\"isOpen\":true,\"subType\":\"ITEM\",\"id\":\"767a59ff-3d25-4c34-84a2-fa5baf074394\"}"
],
"output": {
"params": [
{
"name": "tf2_send_pose",
"value": "1"
}
]
}, },
{ "type": "formBuilder"
"name": "object_name",
"type": "MODEL"
}
],
"Output": [
{
"name": "object_detection",
"type": "BoundBox",
"description": "Topic for publisher"
}
]
}, },
"Settings": [ "__v": 0
{
"name": "cameraLink",
"value": "rgbd_camera"
},
{
"name": "publishDelay",
"value": 0.5
},
{
"name": "is_image_mode",
"value": true
}
]
} }