update move to pose
This commit is contained in:
parent
16ec90cabe
commit
d6cd0d5601
3 changed files with 102 additions and 82 deletions
|
|
@ -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<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": []
|
||||
},
|
||||
"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"
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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):
|
|||
""" <Move to pose> 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 +
|
||||
|
|
|
|||
|
|
@ -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<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"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"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
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue