Remove old generated files before generating again

Pass topic namespace to nodes
Added velodyne

clearpath_launch_generator/clearpath_launch_generator/launch_generator

@@ -39,20 +39,45 @@ from clearpath_launch_generator.launch_writer import LaunchWriter, LaunchFile, P
 from clearpath_launch_generator.sensors import SensorLaunch
 
 import getopt
+import glob
 import os
 import sys
+import shutil
 
 
 class LaunchGenerator():
-    def __init__(self, config: File = None,
+    def __init__(self,
+                 config: File = None,
                  output_path: str = '/etc/clearpath/') -> None:
         if config:
             self.config_path = config.get_path()
         else:
             self.config_path = '/etc/clearpath/robot.yaml'
 
         self.launch_path = output_path
-        # Make directories
+
+        # Remove existing directories
+        try:
+            shutil.rmtree(self.launch_path + '/sensors/launch/')
+        except FileNotFoundError:
+            pass
+
+        try:
+            shutil.rmtree(self.launch_path + '/sensors/config/')
+        except FileNotFoundError:
+            pass
+
+        try:
+            shutil.rmtree(self.launch_path + '/platform/launch/')
+        except FileNotFoundError:
+            pass
+
+        try:
+            shutil.rmtree(self.launch_path + '/platform/config/')
+        except FileNotFoundError:
+            pass
+
+        # Make new directories
         os.makedirs(os.path.dirname(self.launch_path + '/sensors/launch/'), exist_ok=True)
         os.makedirs(os.path.dirname(self.launch_path + '/sensors/config/'), exist_ok=True)
         os.makedirs(os.path.dirname(self.launch_path + '/platform/launch/'), exist_ok=True)

clearpath_launch_generator/clearpath_launch_generator/launch_writer.py

@@ -149,7 +149,6 @@ def generate_file(self):
                     self.write('launch_arguments=[', indent_level=2)
                     for key in launch_file.args.keys():
                         value = launch_file.args.get(key)
-                        print(key, value)
                         if isinstance(value, str):
                             self.write('(\'{0}\', \'{1}\'),'.format(key, value), indent_level=3)
                         elif isinstance(value, dict):
@@ -168,3 +167,4 @@ def generate_file(self):
         self.write('return ld')
 
         self.close_file()
+        print('Generated file: {0}'.format(self.launch_file.get_full_path()))

clearpath_launch_generator/clearpath_launch_generator/sensors.py

@@ -51,7 +51,7 @@ class BaseLaunch():
         # Launch arguments
         NAME = 'name'
         PARAMETERS = 'parameters'
-        TOPIC = 'topic'
+        NAMESPACE = 'namespace'
 
         def __init__(self, sensor: BaseSensor, output_path: str = '/etc/clearpath/sensors/') -> None:
             self.sensor = sensor
@@ -69,12 +69,12 @@ def __init__(self, sensor: BaseSensor, output_path: str = '/etc/clearpath/sensor
                 self.get_name(),
                 path=os.path.join(output_path, 'launch'))
             self.sensor_parameters_file = ParameterFile(self.get_name(), path=os.path.join(output_path, 'config'))
-            self.sensor_parameters = self.default_sensor_parameters
+            self.sensor_parameters = sensor.get_ros_parameters()
 
             self.launch_args = {
                 self.NAME: self.get_name(),
                 self.PARAMETERS: self.sensor_parameters_file,
-                self.TOPIC: self.get_topic()
+                self.NAMESPACE: self.get_namespace()
             }
 
             # Set launch args for default launch file
@@ -83,22 +83,29 @@ def __init__(self, sensor: BaseSensor, output_path: str = '/etc/clearpath/sensor
                 package=self.default_sensor_package,
                 args=self.launch_args)
 
-            self.generate_config()
+        @staticmethod
+        def copy_parameters(default_params: dict, params: dict) -> dict:
+            modified_params = default_params
+            for p in params:
+                if p in default_params:
+                    modified_params[p] = params[p]
+            return modified_params
 
         def generate_config(self):
             name = self.get_name()
             model = self.get_model()
-            default_parameters = self.default_sensor_parameters
+            default_parameters = self.default_sensor_parameters[model]['ros__parameters']
 
-            parameters = {name: {'ros__parameters': ''}}
-            parameters[name]['ros__parameters'] = default_parameters[model]['ros__parameters']
-            for p in self.sensor_parameters:
-                if p in default_parameters[model]['ros__parameters']:
-                    parameters[name]['ros__parameters'][p] = self.sensor_parameters[p]
+            parameters = {
+              name: {
+                'ros__parameters': self.copy_parameters(default_parameters, self.sensor_parameters)
+              }
+            }
 
             with open(self.sensor_parameters_file.get_full_path(), 'w+') as f:
                 yaml.dump(parameters, f, yaml.SafeDumper)
-            #ClearpathConfigParser.write_yaml(self.launch_path + '/sensors/' + name + '.yaml', default_parameters)
+
+            print('Generated config: {0}'.format(self.sensor_parameters_file.get_full_path()))
 
         def get_launch_file(self) -> LaunchFile:
             return self.sensor_launch_file
@@ -124,19 +131,57 @@ def get_default_sensor_package(self) -> str:
         def get_launch_args(self) -> dict:
             return self.launch_args
 
-        def get_topic(self) -> str:
-            return self.TOPIC_NAMESPACE + self.sensor.get_topic()
+        def get_namespace(self) -> str:
+            return self.TOPIC_NAMESPACE + self.sensor.get_name()
 
     class HokuyoUST10Launch(BaseLaunch):
+        def __init__(self, sensor: HokuyoUST10, output_path: str = '/etc/clearpath/sensors/') -> None:
+            super().__init__(sensor, output_path)
+            self.generate_config()
+
+    class SickLMS1XXLaunch(BaseLaunch):
+        def __init__(self, sensor: SickLMS1XX, output_path: str = '/etc/clearpath/sensors/') -> None:
+            super().__init__(sensor, output_path)
+            self.generate_config()
+
+    class IntelRealsenseLaunch(BaseLaunch):
+        def __init__(self, sensor: IntelRealsense, output_path: str = '/etc/clearpath/sensors/') -> None:
+            super().__init__(sensor, output_path)
+            self.generate_config()
+
+    class VelodyneLidarLaunch(BaseLaunch):
         def __init__(self, sensor: BaseSensor, output_path: str = '/etc/clearpath/sensors/') -> None:
             super().__init__(sensor, output_path)
-            self.sensor_parameters[self.get_model()]['ros__parameters']['laser_frame_id'] = self.get_name() + '_laser'
             self.generate_config()
 
+        def generate_config(self):
+            name = self.get_name()
+            model = self.get_model()
+            default_driver_parameters = self.default_sensor_parameters[model + '_driver']['ros__parameters']
+            default_pointcloud_parameters = self.default_sensor_parameters[model + '_pointcloud']['ros__parameters']
+            default_laserscan_parameters = self.default_sensor_parameters[model + '_laserscan']['ros__parameters']
+
+            parameters = {
+              name + '_driver': {
+                'ros__parameters': self.copy_parameters(default_driver_parameters, self.sensor_parameters)
+              },
+              name + '_pointcloud': {
+                'ros__parameters': self.copy_parameters(default_pointcloud_parameters, self.sensor_parameters)
+              },
+              name + '_laserscan': {
+                'ros__parameters': self.copy_parameters(default_laserscan_parameters, self.sensor_parameters)
+              },
+            }
+
+            with open(self.sensor_parameters_file.get_full_path(), 'w+') as f:
+                yaml.dump(parameters, f, yaml.SafeDumper)
+
+            print('Generated config: {0}'.format(self.sensor_parameters_file.get_full_path()))
+
     MODEL = {
         HokuyoUST10.SENSOR_MODEL: HokuyoUST10Launch,
-        SickLMS1XX.SENSOR_MODEL: BaseLaunch,
-        IntelRealsense.SENSOR_MODEL: BaseLaunch
+        SickLMS1XX.SENSOR_MODEL: SickLMS1XXLaunch,
+        IntelRealsense.SENSOR_MODEL: IntelRealsenseLaunch
     }
 
     def __new__(cls, sensor: BaseSensor, output_path: str = '/etc/clearpath/sensors/') -> BaseLaunch:

clearpath_platform/launch/platform.launch.py

@@ -51,12 +51,12 @@ def generate_launch_description():
         condition=LaunchConfigurationEquals('platform_model', 'j100'),
         actions=[
             # Wireless Watcher
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(PathJoinSubstitution(
-                    [FindPackageShare('wireless_watcher'), 'launch', 'watcher.launch.py']
-                )),
-                launch_arguments=[('connected_topic', 'platform/wifi_connected')]
-            ),
+            # IncludeLaunchDescription(
+            #     PythonLaunchDescriptionSource(PathJoinSubstitution(
+            #         [FindPackageShare('wireless_watcher'), 'launch', 'watcher.launch.py']
+            #     )),
+            #     launch_arguments=[('connected_topic', 'platform/wifi_connected')]
+            # ),
 
             # MicroROS Agent
             Node(

clearpath_platform/scripts/check.sh

@@ -4,7 +4,7 @@ filename="/etc/clearpath/robot.yaml"
 m1=$(md5sum "$filename")
 
 while true; do
-  sleep 10
+  sleep 1
 
   m2=$(md5sum "$filename")
 

clearpath_platform/services/clearpath-robot.service

@@ -2,12 +2,13 @@
 Description=Application
 
 [Service]
+User=administrator
 Type=simple
 ExecStartPre=/usr/sbin/clearpath-robot-generate
 ExecStart=/usr/sbin/clearpath-robot-check
 RemainAfterExit=yes
 Restart=on-failure
-RestartSec=5
+RestartSec=1
 
 [Install]
 WantedBy=multi-user.target

clearpath_sensors/config/hokuyo_ust10.yaml

@@ -2,11 +2,11 @@ hokuyo_ust10:
   ros__parameters:
     angle_max: 3.14
     angle_min: -3.14
-    ip_address: "192.168.0.10"
+    ip_address: "192.168.131.20"
     ip_port: 10940
     serial_port: "/dev/ttyACM0"
     serial_baud: 115200
-    laser_frame_id: "laser"
+    laser_frame_id: "lidar2d_0_laser"
     calibrate_time: false
     default_user_latency: 0.0
     diagnostics_tolerance: 0.05

clearpath_sensors/config/velodyne_lidar.yaml

@@ -0,0 +1,24 @@
+velodyne_lidar_driver:
+    ros__parameters:
+        device_ip: 192.168.131.20
+        gps_time: false
+        time_offset: 0.0
+        enabled: true
+        read_once: false
+        read_fast: false
+        repeat_delay: 0.0
+        frame_id: velodyne
+        model: VLP16
+        rpm: 600.0
+        port: 2368
+velodyne_lidar_pointcloud:
+    ros__parameters:
+        calibration: /opt/ros/humble/share/velodyne_pointcloud/params/VLP16db.yaml
+        min_range: 0.9
+        max_range: 130.0
+        view_direction: 0.0
+        organize_cloud: true
+velodyne_lidar_laserscan:
+    ros__parameters:
+        ring: -1
+        resolution: 0.007

clearpath_sensors/launch/hokuyo_ust10.launch.py

@@ -10,6 +10,7 @@ def generate_launch_description():
 
     name = LaunchConfiguration('name')
     parameters = LaunchConfiguration('parameters')
+    namespace = LaunchConfiguration('namespace')
     topic = LaunchConfiguration('topic')
 
     arg_name = DeclareLaunchArgument(
@@ -26,7 +27,7 @@ def generate_launch_description():
 
     arg_topic = DeclareLaunchArgument(
         'topic',
-        default_value='scan')
+        default_value=[namespace, '/scan'])
 
     urg_node = Node(
         package='urg_node',

clearpath_sensors/launch/velodyne_lidar.launch.py

@@ -0,0 +1,41 @@
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    name = LaunchConfiguration('name')
+    parameters = LaunchConfiguration('parameters')
+    topic = LaunchConfiguration('topic')
+
+    velodyne_driver_node = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        name=[name, '_driver'],
+        parameters=[parameters],
+        output='screen'
+    )
+
+    velodyne_pointcloud_node = Node(
+        package='velodyne_pointcloud',
+        executable='velodyne_convert_node',
+        name=[name, '_pointcloud'],
+        output='screen',
+        parameters=[parameters]
+    )
+
+    velodyne_laserscan_node = Node(
+        package='velodyne_laserscan',
+        executable='velodyne_laserscan_node',
+        name=[name, '_laserscan'],
+        output='screen',
+        parameters=[parameters]
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(velodyne_driver_node)
+    ld.add_action(velodyne_pointcloud_node)
+    ld.add_action(velodyne_laserscan_node)
+    return ld