From d6cd0d5601493ad4f4b7c77b7cbc2d62455c8176 Mon Sep 17 00:00:00 2001 From: shalenikol Date: Fri, 18 Oct 2024 18:23:53 +0300 Subject: [PATCH] update move to pose --- rbss_movetopose/rbs_package.json | 38 ++++--- rbss_movetopose/scripts/mtp_cartesian.py | 18 ++-- rbss_poseestimation/rbs_package.json | 128 ++++++++++++----------- 3 files changed, 102 insertions(+), 82 deletions(-) diff --git a/rbss_movetopose/rbs_package.json b/rbss_movetopose/rbs_package.json index 8d72a60..a9235c0 100644 --- a/rbss_movetopose/rbs_package.json +++ b/rbss_movetopose/rbs_package.json @@ -1,7 +1,7 @@ { "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, - "Module": { "name": "MoveToPose", "description": "Move to Pose skill with cartesian controllers" }, - "Launch": { "package": "rbss_movetopose", "executable": "mtp_cartesian.py", "name": "mtp_cartesian" }, + "Module": { "name": "MoveToPose", "description": "Move to Pose skill with cartesian controllers", "node_name": "mtp_cartesian" }, + "Launch": { "package": "rbss_movetopose", "executable": "mtp_cartesian.py" }, "BTAction": [ { "name": "move", @@ -15,16 +15,28 @@ ] } ], - "Interface": { - "Input": [ - { "name": "robotName", "type": "ROBOT" }, - { "name": "pose", "type": "POSE" } + "Settings": { + "result": "{\n  \"params\": \\${ITEM:Array:[]}\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": [] - }, - "Settings": [ - { "name": "server_name", "value": "cartesian_move_to_pose" }, - { "name": "end_effector_velocity", "value": 1.0 }, - { "name": "end_effector_acceleration", "value": 1.0 } - ] + "output": { + "params": [ + { + "name": "server_name", + "value": "cartesian_move_to_pose" + }, + { + "name": "end_effector_velocity", + "value": "1.0" + }, + { + "name": "end_effector_acceleration", + "value": "1.0" + } + ] + }, + "type": "formBuilder" + } } diff --git a/rbss_movetopose/scripts/mtp_cartesian.py b/rbss_movetopose/scripts/mtp_cartesian.py index 5fd5405..52143f8 100755 --- a/rbss_movetopose/scripts/mtp_cartesian.py +++ b/rbss_movetopose/scripts/mtp_cartesian.py @@ -3,7 +3,7 @@ Move_to_pose_cartesian_node_via_interface_node ROS 2 program for Move to Pose skill - @shalenikol release 0.1 + @shalenikol release 0.2 """ import json import math @@ -20,7 +20,7 @@ from geometry_msgs.msg import Pose, PoseStamped 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" SERVER_NAME = "cartesian_movetopose" TOPIC_CURRENT_POSE = "/cartesian_motion_controller/current_pose" @@ -32,15 +32,19 @@ class MoveToPoseCartesianSkill(Node): """ skill node """ def _Settings(self): # Initialization service settings - for prop in self.skill_cfg["Settings"]: + for prop in self.skill_cfg["Settings"]["output"]["params"]: nam = prop["name"] val = prop["value"] if nam == "server_name": self.server_name = val elif nam == "end_effector_velocity": - self.end_effector_velocity = val + self.end_effector_velocity = float(val) 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): 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 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._action_client = ActionClient(self, MoveitSendPose, self.server_name) # "/"+ robot_name + diff --git a/rbss_poseestimation/rbs_package.json b/rbss_poseestimation/rbs_package.json index b6740a7..56a3763 100644 --- a/rbss_poseestimation/rbs_package.json +++ b/rbss_poseestimation/rbs_package.json @@ -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": { - "name": "Robossembler", - "version": "1.0", - "format": "1" + "name": "Robossembler", + "version": "1.0", + "format": "1" }, "Module": { - "name": "PoseEstimation", - "description": "Pose Estimation skill with DOPE", - "node_name": "lc_dope" + "node_name": "lc_dope", + "name": "PoseEstimation", + "description": "Pose Estimation skill with DOPE" }, "Launch": { - "package": "rbss_poseestimation", - "executable": "pe_dope_lc.py" + "executable": "pe_dope_lc.py", + "package": "rbss_poseestimation" }, "BTAction": [ - { - "name": "peConfigure", - "type": "run", - "param": [ - { - "type": "weights", - "dependency": { - "object_name": "knight", - "weights_file": "/home/shalenikol/0_rbs/w_knight.pth", - "dimensions": [ - 0.03, - 0.026, - 0.065 - ] - } - } - ] - }, - { - "name": "peStop", - "type": "stop", - "param": [], - "result": [] - } + { + "name": "peConfigure", + "type": "run", + "param": [ + { + "type": "weights", + "dependency": { + "object_name": "knight", + "weights_file": "/home/shalenikol/0_rbs/w_knight.pth", + "dimensions": [ 0.03, 0.026, 0.065 ] + } + }, + { + "type": "topic", + "dependency": { + "type": "topic", + "topicType": "sensor_msgs/msg/Image", + "topicOut": "/rgbd_camera/image" + }, + "isFilled": true + }, + { + "type": "topic", + "dependency": { + "type": "topic", + "topicType": "sensor_msgs/msg/CameraInfo", + "topicOut": "/rgbd_camera/camera_info" + }, + "isFilled": true + } + ], + "result": [], + "typeAction": "ACTION" + } ], - "Interface": { - "Input": [ + "topicsOut": [ { - "name": "cameraLink", - "type": "CAMERA" + "name": "lc_dope/pose_estimation", + "type": "geometry_msgs/msg/Pose" + } + ], + "Settings": { + "result": "{\n  \"params\": \\${ITEM:Array:[]}\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" + } + ] }, - { - "name": "object_name", - "type": "MODEL" - } - ], - "Output": [ - { - "name": "object_detection", - "type": "BoundBox", - "description": "Topic for publisher" - } - ] + "type": "formBuilder" }, - "Settings": [ - { - "name": "cameraLink", - "value": "rgbd_camera" - }, - { - "name": "publishDelay", - "value": 0.5 - }, - { - "name": "is_image_mode", - "value": true - } - ] + "__v": 0 } \ No newline at end of file