Merge pull request #114 from clearpathrobotics/humble-1.0RC

Humble 1.0 RC

.github/workflows/ci.yml

@@ -45,7 +45,6 @@ jobs:
             clearpath_description
             clearpath_generator_common
             clearpath_mounts_description
-            clearpath_platform
             clearpath_platform_description
             clearpath_sensors_description
   clearpath_common_src_ci:
@@ -67,7 +66,6 @@ jobs:
             clearpath_description
             clearpath_generator_common
             clearpath_mounts_description
-            clearpath_platform
             clearpath_platform_description
             clearpath_sensors_description
           vcs-repo-file-url: dependencies.repos

clearpath_common/CMakeLists.txt

@@ -2,4 +2,10 @@ cmake_minimum_required(VERSION 3.5)
 
 project(clearpath_common NONE)
 find_package(ament_cmake REQUIRED)
-ament_package()
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

clearpath_common/package.xml

@@ -16,7 +16,6 @@
   <exec_depend>clearpath_control</exec_depend>
   <exec_depend>clearpath_description</exec_depend>
   <exec_depend>clearpath_generator_common</exec_depend>
-  <exec_depend>clearpath_platform</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

clearpath_control/launch/control.launch.py

@@ -100,7 +100,7 @@ def launch_setup(context, *args, **kwargs):
                     'platform' in controller):
                 continue
             controllers.append(Node(
-                name='test',
+                name=controller,
                 package='controller_manager',
                 executable='spawner',
                 arguments=['--controller-manager-timeout', '60', controller],

clearpath_generator_common/CMakeLists.txt

@@ -16,6 +16,7 @@ install(PROGRAMS
   ${PROJECT_NAME}/discovery_server/generate_discovery_server
   ${PROJECT_NAME}/bash/generate_bash
   ${PROJECT_NAME}/semantic_description/generate_semantic_description
+  ${PROJECT_NAME}/vcan/generate_vcan
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -50,6 +51,7 @@ if(BUILD_TESTING)
     test/test_generator_bash.py
     test/test_generator_description.py
     test/test_generator_discovery_server.py
+    test/test_generator_vcan.py
   )
   foreach(_test_path ${_pytest_tests})
     get_filename_component(_test_name ${_test_path} NAME_WE)

clearpath_generator_common/clearpath_generator_common/common.py

@@ -297,8 +297,9 @@ def __init__(self,
             self.setup_path, self.MANIPULATORS_PATH, self.LAUNCH_PATH)
 
         # Packages
-        self.pkg_clearpath_platform = Package('clearpath_platform')
         self.pkg_clearpath_sensors = Package('clearpath_sensors')
+        self.pkg_clearpath_common = Package('clearpath_common')
+        self.pkg_clearpath_hardware_interfaces = Package('clearpath_hardware_interfaces')
         self.pkg_clearpath_manipulators = Package('clearpath_manipulators')
         self.pkg_clearpath_platform_description = Package('clearpath_platform_description')
         self.pkg_clearpath_sensors_description = Package('clearpath_sensors_description')

clearpath_generator_common/clearpath_generator_common/description/manipulators.py

@@ -36,6 +36,7 @@
     KinovaGen3Dof6,
     KinovaGen3Dof7,
     KinovaGen3Lite,
+    UniversalRobots
 )
 from clearpath_config.manipulators.types.grippers import (
     Kinova2FLite,
@@ -82,8 +83,8 @@ def rpy(self) -> List[float]:
             return self.manipulator.rpy
 
     class ArmDescription(BaseDescription):
-        IP = 'ip'
-        PORT = 'port'
+        IP = BaseArm.IP_ADDRESS
+        PORT = BaseArm.IP_PORT
 
         def __init__(self, arm: BaseArm) -> None:
             super().__init__(arm)
@@ -108,10 +109,21 @@ def __init__(self, arm: BaseArm) -> None:
                 self.parameters[self.GRIPPER_JOINT] = (
                     arm.gripper.name + self.GRIPPER_NAMES[arm.gripper.get_manipulator_model()])
 
+    class UniversalRobotsDescription(ArmDescription):
+        UR_TYPE = 'ur_type'
+        IP = UniversalRobots.IP_ADDRESS
+        PORT = UniversalRobots.IP_PORT
+
+        def __init__(self, arm: BaseArm) -> None:
+            super().__init__(arm)
+            self.parameters.pop(self.PORT)
+            self.parameters.update(arm.get_urdf_parameters())
+
     MODEL = {
         KinovaGen3Dof6.MANIPULATOR_MODEL: KinovaArmDescription,
         KinovaGen3Dof7.MANIPULATOR_MODEL: KinovaArmDescription,
         KinovaGen3Lite.MANIPULATOR_MODEL: KinovaArmDescription,
+        UniversalRobots.MANIPULATOR_MODEL: UniversalRobotsDescription,
     }
 
     def __new__(cls, manipulator: BaseManipulator) -> BaseManipulator:

clearpath_generator_common/clearpath_generator_common/description/sensors.py

@@ -32,6 +32,7 @@
 from typing import List
 
 from clearpath_config.sensors.types.cameras import (
+    AxisCamera,
     BaseCamera,
     FlirBlackfly,
     IntelRealsense,
@@ -149,6 +150,16 @@ def __init__(self, sensor: BaseCamera) -> None:
                 self.UPDATE_RATE: sensor.fps
             })
 
+    class AxisCameraDescription(CameraDescription):
+        MODEL = 'model'
+
+        def __init__(self, sensor: AxisCamera) -> None:
+            super().__init__(sensor)
+
+            self.parameters.update({
+                self.MODEL: sensor.device_type
+            })
+
     class IntelRealsenseDescription(CameraDescription):
         IMAGE_WIDTH = 'image_width'
         IMAGE_HEIGHT = 'image_height'
@@ -176,6 +187,7 @@ def __init__(self, sensor: StereolabsZed) -> None:
         SickLMS1XX.SENSOR_MODEL: Lidar2dDescription,
         IntelRealsense.SENSOR_MODEL: IntelRealsenseDescription,
         FlirBlackfly.SENSOR_MODEL: CameraDescription,
+        AxisCamera.SENSOR_MODEL: AxisCameraDescription,
         Microstrain.SENSOR_MODEL: ImuDescription,
         VelodyneLidar.SENSOR_MODEL: Lidar3dDescription,
         CHRoboticsUM6.SENSOR_MODEL: ImuDescription,

clearpath_generator_common/clearpath_generator_common/launch/generator.py

@@ -66,7 +66,7 @@ def __init__(self,
 
         self.platform_launch_file = LaunchFile(
             name='platform',
-            package=self.pkg_clearpath_platform,
+            package=self.pkg_clearpath_common,
             args=[
               ('setup_path', self.setup_path),
               ('use_sim_time', 'false'),

clearpath_generator_common/clearpath_generator_common/param/platform.py

@@ -57,7 +57,8 @@ class PlatformParam():
 
     class BaseParam():
         CLEARPATH_CONTROL = 'clearpath_control'
-        CLEARPATH_PLATFORM = 'clearpath_platform'
+        CLEARPATH_HARDWARE_INTERFACES = 'clearpath_hardware_interfaces'
+        CLEARPATH_SENSORS = 'clearpath_sensors'
 
         def __init__(self,
                      parameter: str,
@@ -70,7 +71,10 @@ def __init__(self,
             self.param_path = param_path
 
             # Clearpath Platform Package
-            self.clearpath_platform_package = Package(self.CLEARPATH_PLATFORM)
+            self.clearpath_sensors_package = Package(
+                self. CLEARPATH_SENSORS)
+            self.clearpath_hardware_interfaces_package = Package(
+                self.CLEARPATH_HARDWARE_INTERFACES)
             self.clearpath_control_package = Package(self.CLEARPATH_CONTROL)
 
             # Default parameter file
@@ -168,7 +172,7 @@ def __init__(self,
                      clearpath_config: ClearpathConfig,
                      param_path: str) -> None:
             super().__init__(parameter, clearpath_config, param_path)
-            self.default_parameter_file_package = self.clearpath_platform_package
+            self.default_parameter_file_package = self.clearpath_sensors_package
             self.default_parameter_file_path = 'config'
 
     class LocalizationParam(BaseParam):

clearpath_generator_common/clearpath_generator_common/vcan/generate_vcan

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <[email protected]>
+# @copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+
+from clearpath_generator_common.common import BaseGenerator
+from clearpath_generator_common.vcan.generator import VirtualCANGenerator
+
+
+def main():
+    setup_path = BaseGenerator.get_args()
+    dsg = VirtualCANGenerator(setup_path)
+    dsg.generate()
+
+
+if __name__ == '__main__':
+    main()

clearpath_generator_common/clearpath_generator_common/vcan/generator.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <[email protected]>
+# @copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+from clearpath_config.common.types.platform import Platform
+from clearpath_generator_common.bash.writer import BashWriter
+from clearpath_generator_common.common import BaseGenerator, BashFile
+
+PLATFORMS = [
+    Platform.DD100,
+    Platform.DD150,
+    Platform.DO100,
+    Platform.DO150,
+    Platform.R100,
+]
+
+
+class VirtualCANGenerator(BaseGenerator):
+
+    ROS_DISTRO_PATH = '/opt/ros/humble/'
+
+    def generate(self) -> None:
+        # Generate vcan start up script
+        self.generate_vcan_start()
+
+    def generate_vcan_start(self) -> None:
+        # Generate vcan start up script
+        vcan_start = BashFile(filename='vcan-start', path=self.setup_path)
+        bash_writer = BashWriter(vcan_start)
+
+        # Check platform
+        if self.clearpath_config.get_platform_model() in PLATFORMS:
+            port = 11412
+            serial = '/dev/ttycan0'
+            can = 'vcan0'
+            baud = 's8'
+            bash_writer.write(
+                f'/bin/sh -e /usr/sbin/clearpath-vcan-bridge '
+                f'-p {port} '
+                f'-d {serial} '
+                f'-v {can} '
+                f'-b {baud}'
+            )
+        else:
+            bash_writer.add_echo(
+                'No vcan bridge required.' +
+                'If this was launched as a service then the service will now end.'
+            )
+
+        bash_writer.close()

clearpath_generator_common/package.xml

@@ -17,7 +17,6 @@
   <exec_depend>clearpath_config</exec_depend>
   <exec_depend>clearpath_control</exec_depend>
   <exec_depend>clearpath_description</exec_depend>
-  <exec_depend>clearpath_platform</exec_depend>
   <exec_depend>clearpath_manipulators</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

clearpath_generator_common/test/test_generator_vcan.py

@@ -0,0 +1,57 @@
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <[email protected]>
+# @copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+import os
+import shutil
+
+from ament_index_python.packages import get_package_share_directory
+from clearpath_generator_common.vcan.generator import VirtualCANGenerator
+
+
+class TestRobotLaunchGenerator:
+
+    def test_samples(self):
+        errors = []
+        share_dir = get_package_share_directory('clearpath_config')
+        sample_dir = os.path.join(share_dir, 'sample')
+        for sample in os.listdir(sample_dir):
+            # Create Clearpath Directory
+            src = os.path.join(sample_dir, sample)
+            dst = os.path.join(os.environ['HOME'], '.clearpath', 'robot.yaml')
+            shutil.rmtree(os.path.dirname(dst), ignore_errors=True)
+            os.makedirs(os.path.dirname(dst), exist_ok=True)
+            shutil.copy(src, dst)
+            # Generate
+            try:
+                rlg = VirtualCANGenerator(os.path.dirname(dst))
+                rlg.generate()
+            except Exception as e:
+                errors.append("Sample '%s' failed to load: '%s'" % (
+                    sample,
+                    e.args[0],
+                ))
+        assert not errors, 'Errors: %s' % '\n'.join(errors)

clearpath_manipulators/config/kinematics/arm.yaml

@@ -1,4 +1,5 @@
 ${name}:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.0050000000000000001
-  kinematics_solver_timeout: 0.0050000000000000001
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3

clearpath_manipulators/config/kinematics/gripper.yaml

@@ -1,4 +1,5 @@
 ${name}:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.0050000000000000001
-  kinematics_solver_timeout: 0.0050000000000000001
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3