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

18
rbss_movetopose/scripts/mtp_cartesian.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.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 +