diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index eec1afd..1f52448 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -36,7 +36,7 @@ repos:
rev: 6.1.0
hooks:
- id: flake8
- args: ["--ignore=E501,W503,E203"] # ignore too long line and line break before binary operator
+ args: ["--ignore=E501,W503,E203", "--exclude=grasp_service_parameters.py"]
- repo: https://github.com/PyCQA/doc8
rev: v1.1.1
diff --git a/ros2_grasp_service/package.xml b/ros2_grasp_service/package.xml
index e1abd29..c4dcec6 100644
--- a/ros2_grasp_service/package.xml
+++ b/ros2_grasp_service/package.xml
@@ -7,6 +7,8 @@
Wiktor Bajor
Apache-2.0
+ generate_parameter_library
+
ament_copyright
ament_flake8
ament_pep257
diff --git a/ros2_grasp_service/ros2_grasp_service/grasp_service.py b/ros2_grasp_service/ros2_grasp_service/grasp_service.py
index c3c4ffb..1b51325 100644
--- a/ros2_grasp_service/ros2_grasp_service/grasp_service.py
+++ b/ros2_grasp_service/ros2_grasp_service/grasp_service.py
@@ -1,13 +1,13 @@
from rclpy import init, spin_once, shutdown, spin
from control_msgs.action import FollowJointTrajectory, GripperCommand
from rclpy.node import Node
-from rclpy.parameter import Parameter
from std_srvs.srv import Empty
from rclpy.action import ActionClient
from action_msgs.msg import GoalStatus
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
from builtin_interfaces.msg import Duration
from time import sleep
+from ros2_grasp_service.grasp_service_parameters import grasp_service_parameters
class FollowJointTrajectoryActionClient(Node):
@@ -69,109 +69,57 @@ def get_result_callback(self, future):
class GraspService(Node):
def __init__(self):
super().__init__("grasp_service")
- self.declare_initial_parameters()
- self.get_initial_parameters()
- self.declare_nested_parameters()
- self.get_nested_parameters()
+ param_listener = grasp_service_parameters.ParamListener(self)
+ self.params = param_listener.get_params()
self.log_parameters()
self.service = self.create_service(srv_type=Empty, srv_name="grasp_service", callback=self.grasp)
- self.joint_trajectory_action_client = FollowJointTrajectoryActionClient(self.joints_controller_name)
- self.gripper_action = GripperActionClient(self.gripper_controller_name)
+ self.joint_trajectory_action_client = FollowJointTrajectoryActionClient(self.params.joints_controller_name)
+ self.gripper_action = GripperActionClient(self.params.gripper_controller_name)
- def declare_nested_parameters(self):
- self.declar_times_and_positions_parameters_from_list(
- "times_for_positions_to_target", "positions_to_target", self.positions_to_target_list
- )
- self.declar_times_and_positions_parameters_from_list(
- "times_for_positions_to_home", "positions_to_home", self.positions_to_home_list
- )
-
- def get_nested_parameters(self):
- self.positions_to_target = self.get_nested_double_array_parameters(
- "positions_to_target", self.positions_to_target_list
- )
-
- self.times_for_positions_to_target = self.get_nested_double_parameters(
- "times_for_positions_to_target", self.positions_to_target_list
- )
+ def log_parameters(self):
+ self.get_logger().info(f"Got joints {self.params.joints_names}")
+ self.get_logger().info(f"Got positions to target list {self.params.positions_to_target_list}")
+ for pos_to_target in self.params.positions_to_target_list:
+ self.get_logger().info(
+ f"Got {pos_to_target} to target"
+ f" {self.params.positions_to_target.get_entry(pos_to_target).positions} with time"
+ f" {self.params.positions_to_target.get_entry(pos_to_target).time_to_target}"
+ )
- self.positions_to_home = self.get_nested_double_array_parameters(
- "positions_to_home", self.positions_to_home_list
- )
+ self.get_logger().info(f"Got positions to home list {self.params.positions_to_home_list}")
- self.times_for_positions_to_home = self.get_nested_double_parameters(
- "times_for_positions_to_home", self.positions_to_home_list
- )
+ for pos_to_home in self.params.positions_to_home_list:
+ self.get_logger().info(
+ f"Got {pos_to_home} to home {self.params.positions_to_home.get_entry(pos_to_home).positions} with time"
+ f" {self.params.positions_to_home.get_entry(pos_to_home).time_to_target}"
+ )
- def get_nested_double_array_parameters(self, primary_key_name, nested_keys_names_list):
- return [
- self.get_parameter(f"{primary_key_name}.{nested_key_name}").get_parameter_value().double_array_value
- for nested_key_name in nested_keys_names_list
- ]
+ self.get_logger().info(f"Got time to wait on target {self.params.time_to_wait_on_target}")
- def get_nested_double_parameters(self, primary_key_name, nested_keys_names_list):
- return [
- self.get_parameter(f"{primary_key_name}.{nested_key_name}").get_parameter_value().integer_value
- for nested_key_name in nested_keys_names_list
- ]
+ self.get_logger().info(f"Got joints_controller_name {self.params.joints_controller_name}")
+ self.get_logger().info(f"Got gripper_controller_name {self.params.gripper_controller_name}")
- def get_initial_parameters(self):
- self.joints_controller_name = self.get_parameter("joints_controller_name").get_parameter_value().string_value
- self.joints_names = self.get_parameter("joints_names").get_parameter_value().string_array_value
- self.time_to_wait_on_target = self.get_parameter("time_to_wait_on_target").get_parameter_value().integer_value
- self.positions_to_target_list = (
- self.get_parameter("positions_to_target_list").get_parameter_value().string_array_value
- )
- self.positions_to_home_list = (
- self.get_parameter("positions_to_home_list").get_parameter_value().string_array_value
- )
- self.gripper_close_position = self.get_parameter("gripper_close").get_parameter_value().double_value
- self.gripper_open_position = self.get_parameter("gripper_open").get_parameter_value().double_value
- self.gripper_controller_name = self.get_parameter("gripper_controller_name").get_parameter_value().string_value
-
- def declare_initial_parameters(self):
- self.declare_parameters(
- namespace="",
- parameters=[
- ("joints_controller_name", Parameter.Type.STRING),
- ("joints_names", Parameter.Type.STRING_ARRAY),
- ("positions_to_target_list", Parameter.Type.STRING_ARRAY),
- ("positions_to_target_list", Parameter.Type.STRING_ARRAY),
- ("positions_to_home_list", Parameter.Type.STRING_ARRAY),
- ("time_to_wait_on_target", Parameter.Type.INTEGER),
- ("gripper_close", Parameter.Type.DOUBLE),
- ("gripper_open", Parameter.Type.DOUBLE),
- ("gripper_controller_name", Parameter.Type.STRING),
- ],
- )
+ self.get_logger().info(f"Got gripper_close {self.params.gripper_close}")
+ self.get_logger().info(f"Got gripper_open {self.params.gripper_open}")
- def log_parameters(self):
- self.get_logger().info(f"Got joint {self.joints_names}")
- self.get_logger().info(f"Got positions {self.positions_to_target_list}")
- self.get_logger().info(f"Got positions to target {self.positions_to_target}")
- self.get_logger().info(f"Got times for positions to target {self.times_for_positions_to_target}")
- self.get_logger().info(f"Got positions to home {self.positions_to_home}")
- self.get_logger().info(f"Got times for positions to home {self.times_for_positions_to_home}")
- self.get_logger().info(f"Got time to wait on target {self.time_to_wait_on_target}")
-
- def declar_times_and_positions_parameters_from_list(self, times_prefix, position_prefix, list_of_parameters):
- for position in list_of_parameters:
- self.get_logger().info(f"{position_prefix}.{position}")
- self.declare_parameter(f"{times_prefix}.{position}", Parameter.Type.INTEGER)
- self.declare_parameter(f"{position_prefix}.{position}", Parameter.Type.DOUBLE_ARRAY)
-
- def create_trajectory_goal(self, array_of_points, times_in_sec):
+ def create_trajectory_goal(self, list_of_position, map_of_positions):
trajectory = JointTrajectory()
- trajectory.joint_names = self.joints_names
+ trajectory.joint_names = self.params.joints_names
trajectory.points = [
- JointTrajectoryPoint(positions=points, time_from_start=Duration(sec=time_from_sec))
- for points, time_from_sec in zip(array_of_points, times_in_sec)
+ JointTrajectoryPoint(
+ positions=map_of_positions.get_entry(positions).positions,
+ time_from_start=Duration(sec=map_of_positions.get_entry(positions).time_to_target),
+ )
+ for positions in list_of_position
]
trajectory.points.append(
JointTrajectoryPoint(
- positions=array_of_points[-1], time_from_start=Duration(sec=times_in_sec[-1], nanosec=1)
+ positions=map_of_positions.get_entry(list_of_position[-1]).positions,
+ time_from_start=Duration(
+ sec=map_of_positions.get_entry(list_of_position[-1]).time_to_target, nanosec=1
+ ),
)
)
goal_msg = FollowJointTrajectory.Goal()
@@ -197,13 +145,13 @@ def set_gripper_postion(self, gripper_postion):
def grasp(self, request, response):
self.get_logger().error("Trigger received. Grasp sequence will be started.")
- self.go_to_pose(self.positions_to_target, self.times_for_positions_to_target)
- self.set_gripper_postion(self.gripper_close_position)
+ self.go_to_pose(self.params.positions_to_target_list, self.params.positions_to_target)
+ self.set_gripper_postion(self.params.gripper_close)
- sleep(self.time_to_wait_on_target)
+ sleep(self.params.time_to_wait_on_target)
- self.go_to_pose(self.positions_to_home, self.times_for_positions_to_home)
- self.set_gripper_postion(self.gripper_open_position)
+ self.go_to_pose(self.params.positions_to_home_list, self.params.positions_to_home)
+ self.set_gripper_postion(self.params.gripper_open)
return response
diff --git a/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py b/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py
new file mode 100644
index 0000000..1b08479
--- /dev/null
+++ b/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py
@@ -0,0 +1,320 @@
+# auto-generated DO NOT EDIT
+
+from rcl_interfaces.msg import ParameterDescriptor
+from rcl_interfaces.msg import SetParametersResult
+from rcl_interfaces.msg import FloatingPointRange, IntegerRange
+from rclpy.clock import Clock
+from rclpy.exceptions import InvalidParameterValueException
+from rclpy.time import Time
+import copy
+import rclpy
+from generate_parameter_library_py.python_validators import ParameterValidators
+
+
+class grasp_service_parameters:
+ class Params:
+ # for detecting if the parameter struct has been updated
+ stamp_ = Time()
+
+ joints_names = None
+ positions_to_target_list = None
+ positions_to_home_list = None
+ time_to_wait_on_target = None
+ joints_controller_name = None
+ gripper_controller_name = None
+ gripper_close = None
+ gripper_open = None
+
+ class __PositionsToTarget:
+ class __MapPositionsToTargetList:
+ positions = None
+ time_to_target = None
+
+ __map_type = __MapPositionsToTargetList
+
+ def add_entry(self, name):
+ if not hasattr(self, name):
+ setattr(self, name, self.__map_type())
+
+ def get_entry(self, name):
+ return getattr(self, name)
+
+ positions_to_target = __PositionsToTarget()
+
+ class __PositionsToHome:
+ class __MapPositionsToHomeList:
+ positions = None
+ time_to_target = None
+
+ __map_type = __MapPositionsToHomeList
+
+ def add_entry(self, name):
+ if not hasattr(self, name):
+ setattr(self, name, self.__map_type())
+
+ def get_entry(self, name):
+ return getattr(self, name)
+
+ positions_to_home = __PositionsToHome()
+
+ class ParamListener:
+ def __init__(self, node, prefix=""):
+ self.prefix_ = prefix
+ self.params_ = grasp_service_parameters.Params()
+ self.node_ = node
+ self.logger_ = rclpy.logging.get_logger("grasp_service_parameters." + prefix)
+
+ self.declare_params()
+
+ self.node_.add_on_set_parameters_callback(self.update)
+ self.clock_ = Clock()
+
+ def get_params(self):
+ tmp = self.params_.stamp_
+ self.params_.stamp_ = None
+ paramCopy = copy.deepcopy(self.params_)
+ paramCopy.stamp_ = tmp
+ self.params_.stamp_ = tmp
+ return paramCopy
+
+ def is_old(self, other_param):
+ return self.params_.stamp_ != other_param.stamp_
+
+ def refresh_dynamic_parameters(self):
+ updated_params = self.get_params()
+ # TODO remove any destroyed dynamic parameters
+
+ # declare any new dynamic parameters
+ for value in updated_params.positions_to_target_list:
+ updated_params.positions_to_target.add_entry(value)
+ entry = updated_params.positions_to_target.get_entry(value)
+ param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+ if not self.node_.has_parameter(self.prefix_ + param_name):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.DOUBLE_ARRAY
+ self.node_.declare_parameter(param_name, parameter, descriptor)
+ param = self.node_.get_parameter(param_name)
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ entry.positions = param.value
+ for value in updated_params.positions_to_target_list:
+ updated_params.positions_to_target.add_entry(value)
+ entry = updated_params.positions_to_target.get_entry(value)
+ param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+ if not self.node_.has_parameter(self.prefix_ + param_name):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.INTEGER
+ self.node_.declare_parameter(param_name, parameter, descriptor)
+ param = self.node_.get_parameter(param_name)
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ entry.time_to_target = param.value
+ for value in updated_params.positions_to_home_list:
+ updated_params.positions_to_home.add_entry(value)
+ entry = updated_params.positions_to_home.get_entry(value)
+ param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+ if not self.node_.has_parameter(self.prefix_ + param_name):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.DOUBLE_ARRAY
+ self.node_.declare_parameter(param_name, parameter, descriptor)
+ param = self.node_.get_parameter(param_name)
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ entry.positions = param.value
+ for value in updated_params.positions_to_home_list:
+ updated_params.positions_to_home.add_entry(value)
+ entry = updated_params.positions_to_home.get_entry(value)
+ param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+ if not self.node_.has_parameter(self.prefix_ + param_name):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.INTEGER
+ self.node_.declare_parameter(param_name, parameter, descriptor)
+ param = self.node_.get_parameter(param_name)
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ entry.time_to_target = param.value
+
+ def update(self, parameters):
+ updated_params = self.get_params()
+
+ for param in parameters:
+ if param.name == self.prefix_ + "joints_names":
+ updated_params.joints_names = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "positions_to_target_list":
+ updated_params.positions_to_target_list = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "positions_to_home_list":
+ updated_params.positions_to_home_list = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "time_to_wait_on_target":
+ updated_params.time_to_wait_on_target = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "joints_controller_name":
+ updated_params.joints_controller_name = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "gripper_controller_name":
+ updated_params.gripper_controller_name = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "gripper_close":
+ updated_params.gripper_close = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "gripper_open":
+ updated_params.gripper_open = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ # update dynamic parameters
+ for param in parameters:
+ for value in updated_params.positions_to_target_list:
+ param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+ if param.name == param_name:
+ updated_params.positions_to_target.positions_to_target_list_map[value].positions = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ for value in updated_params.positions_to_target_list:
+ param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+ if param.name == param_name:
+ updated_params.positions_to_target.positions_to_target_list_map[value].time_to_target = (
+ param.value
+ )
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ for value in updated_params.positions_to_home_list:
+ param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+ if param.name == param_name:
+ updated_params.positions_to_home.positions_to_home_list_map[value].positions = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ for value in updated_params.positions_to_home_list:
+ param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+ if param.name == param_name:
+ updated_params.positions_to_home.positions_to_home_list_map[value].time_to_target = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ updated_params.stamp_ = self.clock_.now()
+ self.update_internal_params(updated_params)
+ return SetParametersResult(successful=True)
+
+ def update_internal_params(self, updated_params):
+ self.params_ = updated_params
+
+ def declare_params(self):
+ updated_params = self.get_params()
+ # declare all parameters and give default values to non-required ones
+ if not self.node_.has_parameter(self.prefix_ + "joints_names"):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.STRING_ARRAY
+ self.node_.declare_parameter(self.prefix_ + "joints_names", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "positions_to_target_list"):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.STRING_ARRAY
+ self.node_.declare_parameter(self.prefix_ + "positions_to_target_list", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "positions_to_home_list"):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.STRING_ARRAY
+ self.node_.declare_parameter(self.prefix_ + "positions_to_home_list", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "time_to_wait_on_target"):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.INTEGER
+ self.node_.declare_parameter(self.prefix_ + "time_to_wait_on_target", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "joints_controller_name"):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.STRING
+ self.node_.declare_parameter(self.prefix_ + "joints_controller_name", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "gripper_controller_name"):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.STRING
+ self.node_.declare_parameter(self.prefix_ + "gripper_controller_name", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "gripper_close"):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.DOUBLE
+ self.node_.declare_parameter(self.prefix_ + "gripper_close", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "gripper_open"):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.DOUBLE
+ self.node_.declare_parameter(self.prefix_ + "gripper_open", parameter, descriptor)
+
+ # TODO: need validation
+ # get parameters and fill struct fields
+ param = self.node_.get_parameter(self.prefix_ + "joints_names")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.joints_names = param.value
+ param = self.node_.get_parameter(self.prefix_ + "positions_to_target_list")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.positions_to_target_list = param.value
+ param = self.node_.get_parameter(self.prefix_ + "positions_to_home_list")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.positions_to_home_list = param.value
+ param = self.node_.get_parameter(self.prefix_ + "time_to_wait_on_target")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.time_to_wait_on_target = param.value
+ param = self.node_.get_parameter(self.prefix_ + "joints_controller_name")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.joints_controller_name = param.value
+ param = self.node_.get_parameter(self.prefix_ + "gripper_controller_name")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.gripper_controller_name = param.value
+ param = self.node_.get_parameter(self.prefix_ + "gripper_close")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.gripper_close = param.value
+ param = self.node_.get_parameter(self.prefix_ + "gripper_open")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.gripper_open = param.value
+
+ # declare and set all dynamic parameters
+ for value in updated_params.positions_to_target_list:
+ updated_params.positions_to_target.add_entry(value)
+ entry = updated_params.positions_to_target.get_entry(value)
+ param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+ if not self.node_.has_parameter(self.prefix_ + param_name):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.DOUBLE_ARRAY
+ self.node_.declare_parameter(param_name, parameter, descriptor)
+ param = self.node_.get_parameter(param_name)
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ entry.positions = param.value
+ for value in updated_params.positions_to_target_list:
+ updated_params.positions_to_target.add_entry(value)
+ entry = updated_params.positions_to_target.get_entry(value)
+ param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+ if not self.node_.has_parameter(self.prefix_ + param_name):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.INTEGER
+ self.node_.declare_parameter(param_name, parameter, descriptor)
+ param = self.node_.get_parameter(param_name)
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ entry.time_to_target = param.value
+ for value in updated_params.positions_to_home_list:
+ updated_params.positions_to_home.add_entry(value)
+ entry = updated_params.positions_to_home.get_entry(value)
+ param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+ if not self.node_.has_parameter(self.prefix_ + param_name):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.DOUBLE_ARRAY
+ self.node_.declare_parameter(param_name, parameter, descriptor)
+ param = self.node_.get_parameter(param_name)
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ entry.positions = param.value
+ for value in updated_params.positions_to_home_list:
+ updated_params.positions_to_home.add_entry(value)
+ entry = updated_params.positions_to_home.get_entry(value)
+ param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+ if not self.node_.has_parameter(self.prefix_ + param_name):
+ descriptor = ParameterDescriptor(description="", read_only=False)
+ parameter = rclpy.Parameter.Type.INTEGER
+ self.node_.declare_parameter(param_name, parameter, descriptor)
+ param = self.node_.get_parameter(param_name)
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ entry.time_to_target = param.value
+
+ self.update_internal_params(updated_params)
diff --git a/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.yaml b/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.yaml
new file mode 100644
index 0000000..0098953
--- /dev/null
+++ b/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.yaml
@@ -0,0 +1,50 @@
+grasp_service_parameters:
+ joints_names: {
+ type: string_array,
+ }
+
+ positions_to_target_list: {
+ type: string_array,
+ }
+
+ positions_to_target:
+ __map_positions_to_target_list:
+ positions: {
+ type: double_array,
+ }
+ time_to_target: {
+ type: int,
+ }
+
+ positions_to_home_list: {
+ type: string_array,
+ }
+
+ positions_to_home:
+ __map_positions_to_home_list:
+ positions: {
+ type: double_array,
+ }
+ time_to_target: {
+ type: int,
+ }
+
+ time_to_wait_on_target: {
+ type: int,
+ }
+
+ joints_controller_name: {
+ type: string,
+ }
+
+ gripper_controller_name: {
+ type: string,
+ }
+
+ gripper_close: {
+ type: double,
+ }
+
+ gripper_open: {
+ type: double,
+ }
diff --git a/ros2_grasp_service/setup.py b/ros2_grasp_service/setup.py
index 939189e..56b51b6 100644
--- a/ros2_grasp_service/setup.py
+++ b/ros2_grasp_service/setup.py
@@ -1,9 +1,12 @@
+from generate_parameter_library_py.setup_helper import generate_parameter_module
from setuptools import find_packages, setup
-import os
from glob import glob
+import os
package_name = "ros2_grasp_service"
+generate_parameter_module("grasp_service_parameters", "ros2_grasp_service/grasp_service_parameters.yaml")
+
setup(
name=package_name,
version="1.0.0",
diff --git a/ros2_grasp_service_demo/open_manipulator/open_manipulator_bringup/config/grasp_service_config.yaml b/ros2_grasp_service_demo/open_manipulator/open_manipulator_bringup/config/grasp_service_config.yaml
index 411f044..094f7a8 100644
--- a/ros2_grasp_service_demo/open_manipulator/open_manipulator_bringup/config/grasp_service_config.yaml
+++ b/ros2_grasp_service_demo/open_manipulator/open_manipulator_bringup/config/grasp_service_config.yaml
@@ -12,21 +12,20 @@ grasp_service:
- second_pos
positions_to_target:
- first_pos: [0.50, 0.0, 0.0, 0.0]
- second_pos: [-0.50, 0.0, 0.0, 0.0]
-
- times_for_positions_to_target:
- first_pos: 2
- second_pos: 4
+ first_pos:
+ positions: [0.50, 0.0, 0.0, 0.0]
+ time_to_target: 2
+ second_pos:
+ positions: [-0.50, 0.0, 0.0, 0.0]
+ time_to_target: 4
positions_to_home_list:
- first_pos
positions_to_home:
- first_pos: [0.50, 0.0, 0.0, 0.0]
-
- times_for_positions_to_home:
- first_pos: 2
+ first_pos:
+ positions: [0.50, 0.0, 0.0, 0.0]
+ time_to_target: 2
time_to_wait_on_target: 5