Merge pull request #157 from clearpathrobotics/rc/jazzy-2.1

Jazzy 2.1 RC
clearpathrobotics · Jan 31, 2025 · 9096f1b · 9096f1b
2 parents a58bac5 + e93989f
commit 9096f1b
Show file tree

Hide file tree

Showing 27 changed files with 523 additions and 11 deletions.
diff --git a/clearpath_control/config/a200/localization.yaml b/clearpath_control/config/a200/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/a300/localization.yaml b/clearpath_control/config/a300/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/dd100/localization.yaml b/clearpath_control/config/dd100/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/dd150/localization.yaml b/clearpath_control/config/dd150/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/do100/localization.yaml b/clearpath_control/config/do100/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/do150/localization.yaml b/clearpath_control/config/do150/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/generic/localization.yaml b/clearpath_control/config/generic/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/j100/localization.yaml b/clearpath_control/config/j100/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/r100/localization.yaml b/clearpath_control/config/r100/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_control/config/w200/localization.yaml b/clearpath_control/config/w200/localization.yaml
@@ -6,6 +6,7 @@ ekf_node:
     publish_tf: true
     two_d_mode: true
     use_sim_time: false
+    print_diagnostics: true
 
     frequency: 50.0
 

diff --git a/clearpath_generator_common/clearpath_generator_common/description/generator.py b/clearpath_generator_common/clearpath_generator_common/description/generator.py
@@ -205,6 +205,7 @@ def generate_manipulators(self) -> None:
         self.xacro_writer.write_comment('Manipulators')
         self.xacro_writer.write_newline()
         self.generate_arms()
+        self.generate_lifts()
         self.generate_grippers()
 
     def generate_arms(self) -> None:
@@ -257,6 +258,30 @@ def generate_grippers(self) -> None:
 
             self.xacro_writer.write_newline()
 
+    def generate_lifts(self) -> None:
+        lifts = self.clearpath_config.manipulators.get_all_lifts()
+        for lift in lifts:
+            lift_description = ManipulatorDescription(lift)
+
+            self.xacro_writer.write_comment(
+                '{0}'.format(lift_description.name)
+            )
+
+            self.xacro_writer.write_include(
+                package=lift_description.package,
+                file=lift_description.model,
+                path=lift_description.path
+            )
+
+            self.xacro_writer.write_macro(
+                macro='{0}'.format(lift_description.model),
+                parameters=lift_description.parameters,
+                blocks=XacroWriter.add_origin(
+                    lift_description.xyz, lift_description.rpy)
+            )
+
+            self.xacro_writer.write_newline()
+
     def generate_extras(self) -> None:
         self.xacro_writer.write_comment('Extras')
         self.xacro_writer.write_newline()

diff --git a/clearpath_generator_common/clearpath_generator_common/description/manipulators.py b/clearpath_generator_common/clearpath_generator_common/description/manipulators.py
@@ -43,6 +43,10 @@
     Robotiq2F140,
     Robotiq2F85
 )
+from clearpath_config.manipulators.types.lifts import (
+    BaseLift,
+    Ewellix
+)
 from clearpath_config.manipulators.types.manipulator import BaseManipulator
 
 
@@ -119,11 +123,23 @@ def __init__(self, arm: BaseArm) -> None:
             self.parameters.pop(self.PORT)
             self.parameters.update(arm.get_urdf_parameters())
 
+    class LiftDescription(BaseDescription):
+
+        def __init__(self, lift: BaseLift) -> None:
+            super().__init__(lift)
+
+    class EwellixDescription(LiftDescription):
+
+        def __init__(self, lift: BaseLift) -> None:
+            super().__init__(lift)
+            self.parameters.update(lift.get_urdf_parameters())
+
     MODEL = {
         KinovaGen3Dof6.MANIPULATOR_MODEL: KinovaArmDescription,
         KinovaGen3Dof7.MANIPULATOR_MODEL: KinovaArmDescription,
         KinovaGen3Lite.MANIPULATOR_MODEL: KinovaArmDescription,
         UniversalRobots.MANIPULATOR_MODEL: UniversalRobotsDescription,
+        Ewellix.MANIPULATOR_MODEL: EwellixDescription,
     }
 
     def __new__(cls, manipulator: BaseManipulator) -> BaseManipulator:

diff --git a/clearpath_generator_common/clearpath_generator_common/description/sensors.py b/clearpath_generator_common/clearpath_generator_common/description/sensors.py
@@ -101,7 +101,7 @@ def __init__(self, sensor: BaseLidar2D) -> None:
                 self.MAXIMUM_ANGLE: sensor.max_angle,
                 self.MINIMUM_RANGE: 0.05,
                 self.MAXIMUM_RANGE: 25.0,
-                self.UPDATE_RATE: 50
+                self.UPDATE_RATE: 40  # TODO: link to clearpath_config property
             })
 
     class Lidar3dDescription(BaseDescription):
@@ -127,7 +127,7 @@ def __init__(self, sensor: BaseLidar3D) -> None:
                 self.MAXIMUM_ANGLE_V: 0.261799,
                 self.MINIMUM_RANGE: 0.9,
                 self.MAXIMUM_RANGE: 130.0,
-                self.UPDATE_RATE: 50
+                self.UPDATE_RATE: 20  # TODO: link to clearpath_config property
             })
 
     class ImuDescription(BaseDescription):
@@ -137,7 +137,7 @@ def __init__(self, sensor: BaseIMU) -> None:
             super().__init__(sensor)
 
             self.parameters.update({
-                self.UPDATE_RATE: 100
+                self.UPDATE_RATE: sensor.update_rate
             })
 
     class CameraDescription(BaseDescription):
@@ -152,14 +152,12 @@ def __init__(self, sensor: BaseCamera) -> None:
 
     class AxisCameraDescription(CameraDescription):
         MODEL = 'model'
-        UPDATE_RATE = 'update_rate'
 
         def __init__(self, sensor: AxisCamera) -> None:
             super().__init__(sensor)
 
             self.parameters.update({
                 self.MODEL: sensor.device_type,
-                self.UPDATE_RATE: 15,
             })
 
     class IntelRealsenseDescription(CameraDescription):

diff --git a/clearpath_generator_common/clearpath_generator_common/param/manipulators.py b/clearpath_generator_common/clearpath_generator_common/param/manipulators.py
@@ -127,6 +127,25 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                 self.param_file.parameters = merge_dict(
                     self.param_file.parameters, updated_parameters)
 
+            # Lifts
+            for lift in self.clearpath_config.manipulators.get_all_lifts():
+                # Lift Control Parameter File
+                lift_param_file = ParamFile(
+                    name='control',
+                    package=Package('clearpath_manipulators_description'),
+                    path='config/%s/%s' % (
+                        lift.get_manipulator_type(),
+                        lift.get_manipulator_model()),
+                    parameters={}
+                )
+                lift_param_file.read()
+                updated_parameters = replace_dict_items(
+                    lift_param_file.parameters,
+                    {r'${name}': lift.name}
+                )
+                self.param_file.parameters = merge_dict(
+                    self.param_file.parameters, updated_parameters)
+
         def generate_parameter_file(self):
             param_writer = ParamWriter(self.param_file)
             param_writer.write_file()
@@ -258,6 +277,18 @@ def get_kinematics_parameters(self):
                     r'${name}': gripper.name
                 })
                 parameter_file += kinematics_file
+            # Lifts
+            for lift in self.clearpath_config.manipulators.get_all_lifts():
+                kinematics_file = MoveItParamFile(
+                    name=lift.get_manipulator_type(),
+                    path=parameter_directory,
+                    package=parameter_package
+                )
+                kinematics_file.read()
+                kinematics_file.replace({
+                    r'${name}': lift.name
+                })
+                parameter_file += kinematics_file
             parameter_file.add_header('robot_description_kinematics')
             return parameter_file
 
@@ -328,6 +359,26 @@ def get_moveit_controller_parameters(self, use_sim_time: bool = False) -> None:
                     r'${name}': gripper.name
                 })
                 parameter_file += controller_file
+            # Lifts
+            for lift in self.clearpath_config.manipulators.get_all_lifts():
+                controller_file = MoveItParamFile(
+                    name=parameter_name,
+                    path=os.path.join(
+                        parameter_directory,
+                        lift.get_manipulator_type(),
+                        lift.get_manipulator_model()
+                    ),
+                    package=parameter_package
+                )
+                controller_name = lift.name
+                if not use_sim_time:
+                    controller_name = 'manipulators/' + controller_name
+                controller_file.read()
+                controller_file.replace({
+                    r'${controller_name}': controller_name,
+                    r'${name}': lift.name
+                })
+                parameter_file += controller_file
             return parameter_file
 
         # Joint Limits
@@ -375,6 +426,22 @@ def get_joint_limits_parameters(self):
                     r'${name}': gripper.name
                 })
                 parameter_file += controller_file
+            # Lifts
+            for lift in self.clearpath_config.manipulators.get_all_lifts():
+                controller_file = MoveItParamFile(
+                    name=parameter_name,
+                    path=os.path.join(
+                        parameter_directory,
+                        lift.get_manipulator_type(),
+                        lift.get_manipulator_model()
+                    ),
+                    package=parameter_package
+                )
+                controller_file.read()
+                controller_file.replace({
+                    r'${name}': lift.name
+                })
+                parameter_file += controller_file
             parameter_file.add_header('robot_description_planning')
             return parameter_file