Humble 1.3 Release

clearpath_config/manipulators/manipulators.py

@@ -43,27 +43,32 @@
 
 class MoveItConfig(BaseConfig):
     ENABLE = 'enable'
+    DELAY = 'delay'
     ROS_PARAMETERS = 'ros_parameters'
 
     TEMPLATE = {
         ENABLE: ENABLE,
+        DELAY: DELAY,
         ROS_PARAMETERS: ROS_PARAMETERS
     }
 
     KEYS = flip_dict(TEMPLATE)
 
     DEFAULTS = {
         ENABLE: False,
+        DELAY: 5.0,
         ROS_PARAMETERS: {}
     }
 
     def __init__(
             self,
             config: dict = {},
             enable: bool = DEFAULTS[ENABLE],
+            delay: float = DEFAULTS[DELAY],
             ros_parameters: dict = DEFAULTS[ROS_PARAMETERS]
             ) -> None:
         self.enable = enable
+        self.delay = delay
         self.ros_parameters = ros_parameters
         if config:
             self.from_dict(config)
@@ -76,6 +81,15 @@ def enable(self) -> bool:
     def enable(self, value: bool) -> None:
         self._enable = bool(value)
 
+    @property
+    def delay(self) -> float:
+        return self._delay
+
+    @delay.setter
+    def delay(self, value: float) -> None:
+        assert value > 0, f'MoveIt delay must be greater than 0. Got {value}'
+        self._delay = value
+
     @property
     def ros_parameters(self) -> dict:
         return self._ros_parameters

clearpath_config/manipulators/types/arms.py

@@ -42,7 +42,6 @@ class BaseArm(BaseManipulator):
     DEFAULT_IP_ADDRESS = "192.168.131.40"
     DEFAULT_IP_PORT = 10000
 
-    URDF_PARAMETERS = {}
     END_EFFECTOR_LINK = "end_effector"
 
     def __init__(
@@ -65,8 +64,6 @@ def __init__(
         self.ip = IP(ip)
         # IP Port
         self.port = Port(port)
-        # URDF Parameters
-        self.urdf_parameters = dict(self.URDF_PARAMETERS)
 
     @classmethod
     def get_ip_from_idx(cls, idx: int) -> str:
@@ -107,9 +104,6 @@ def to_dict(self) -> dict:
             d['gripper'] = self.gripper.to_dict()
         else:
             d['gripper'] = None
-        for k, v in self.urdf_parameters.items():
-            if v:
-                d[k] = v
         return d
 
     def from_dict(self, d: dict) -> None:
@@ -120,21 +114,11 @@ def from_dict(self, d: dict) -> None:
             self.gripper.from_dict(d['gripper'])
             self.gripper.set_name('%s_gripper' % self.get_name())
             if 'parent' not in d['gripper']:
-                self.gripper.set_parent('%s_end_effector_link' % self.get_name())
+                self.gripper.set_parent(f'{self.get_name()}_{self.END_EFFECTOR_LINK}')
         if self.IP_ADDRESS in d:
             self.ip = d[self.IP_ADDRESS]
         if self.IP_PORT in d:
             self.port = d[self.IP_PORT]
-        for k in self.urdf_parameters:
-            if k in d:
-                self.urdf_parameters[k] = d[k]
-
-    def get_urdf_parameters(self) -> dict:
-        d = {}
-        for k, v in self.urdf_parameters.items():
-            if v:
-                d[k] = v
-        return d
 
 
 class KinovaGen3Dof6(BaseArm):

clearpath_config/manipulators/types/grippers.py

@@ -54,11 +54,17 @@ class Kinova2FLite(BaseGripper):
 class Robotiq2F85(BaseGripper):
     MANIPULATOR_MODEL = "robotiq_2f_85"
     JOINT_COUNT = 1
+    URDF_PARAMETERS = {
+        'com_port': ''
+    }
 
 
 class Robotiq2F140(BaseGripper):
     MANIPULATOR_MODEL = "robotiq_2f_140"
     JOINT_COUNT = 1
+    URDF_PARAMETERS = {
+        'com_port': ''
+    }
 
 
 class Gripper():

clearpath_config/manipulators/types/manipulator.py

@@ -93,6 +93,7 @@ class BaseManipulator(IndexedAccessory):
     ROS_PARAMETERS = {}
     ROS_PARAMETERS_TEMPLATE = {}
     JOINT_COUNT = 0
+    URDF_PARAMETERS = {}
 
     class ROSParameter:
         def __init__(
@@ -119,6 +120,8 @@ def __init__(
         # ROS Parameters
         self.ros_parameters_template = ros_parameters_template
         self.ros_parameters = ros_parameters
+        # URDF Parameters
+        self.urdf_parameters = dict(self.URDF_PARAMETERS)
         super().__init__(idx, name, parent, xyz, rpy)
 
     def to_dict(self) -> dict:
@@ -131,6 +134,9 @@ def to_dict(self) -> dict:
         d['poses'] = []
         for pose in self.poses:
             d['poses'].append(pose.to_dict())
+        for k, v in self.urdf_parameters.items():
+            if v:
+                d[k] = v
         return d
 
     def from_dict(self, d: dict) -> None:
@@ -144,6 +150,9 @@ def from_dict(self, d: dict) -> None:
             self.set_ros_parameters(d['ros_parameters'])
         if 'poses' in d:
             self.poses = d['poses']
+        for k in self.urdf_parameters:
+            if k in d:
+                self.urdf_parameters[k] = d[k]
 
     @classmethod
     def get_manipulator_model(cls) -> str:
@@ -219,3 +228,10 @@ def poses(self, pose_list: List) -> None:
             manipulator_pose.from_dict(pose)
             poses_.append(manipulator_pose)
         self._poses = poses_
+
+    def get_urdf_parameters(self) -> dict:
+        d = {}
+        for k, v in self.urdf_parameters.items():
+            if v:
+                d[k] = v
+        return d