top_sensor_kit: supports multiple realsense camera launch

westonrobot · Aug 21, 2024 · d1322bc · d1322bc
1 parent 24df2e3
commit d1322bc
Show file tree

Hide file tree

Showing 13 changed files with 81 additions and 56 deletions.
diff --git a/README.md b/README.md
@@ -92,12 +92,6 @@ The following hardware configurations are supported:
 
     The build process should finish without any errors.
 
-* Install ROS packages **(Make sure you have ros2 installed first)**
-    ```bash
-    $ chmod +x ros2_packages.sh
-    $ ./ros2_packages.sh
-    ```
-
 ## Running the packages
 Sample launch files can be found in the [wr_devkit_bringup](./src/wr_devkit_bringup/) package. They are meant to be used as a starting point for your own development and can be customized to your needs.
 

diff --git a/navigation.repos b/navigation.repos
@@ -19,3 +19,7 @@ repositories:
     type: git
     url: https://github.com/westonrobot/wrp_ros2.git
     version: 30431fd002b7e0a67c295bbf87ab1dc9d5cbb7ee
+  drivers/realsense-ros:
+    type: git
+    url: https://github.com/IntelRealSense/realsense-ros.git
+    version: 7c163180e56172f38700d9f3ac9a4205de03765e
diff --git a/ros2_packages.sh b/ros2_packages.sh
diff --git a/src/kits/README.md b/src/kits/README.md
@@ -1,5 +1,5 @@
 # Overview
-This folder contains "bringup" packages for the different sensor/chassis kits we support for the [Weston Robot Development Kit](https://docs.westonrobot.net/wr_dev_kit/wr_dev_kit.html) (devkit). 
+This folder contains "bringup" packages for the different sensor/chassis kits we support for the [Weston Robot Development Kit](https://docs.westonrobot.com/wr_dev_kit/wr_dev_kit.html) (devkit). 
 
 ## Kits
 * [Chassis](./chassis/)
@@ -15,7 +15,7 @@ This folder contains "bringup" packages for the different sensor/chassis kits we
 * Chassis Kits
   * [UGV Devkit V1.0](./chassis/ugv_devkit_v1_bringup/)
 * Top Sensor Kits
-  * [Mid360 Lidar + IMU](./top/mid360_sensor_kit_bringup/)
+  * [Mid360 Lidar + IMU + Camera](./top/mid360_sensor_kit_bringup/)
 * Base Sensor Kits
   * [W200d ultrasonic](./base/w200d_sensor_kit_bringup/)
 

diff --git a/src/kits/top/mid360_sensor_kit_bringup/README.md b/src/kits/top/mid360_sensor_kit_bringup/README.md
@@ -13,10 +13,17 @@ ros2 launch mid360_sensor_kit_bringup sensor_kit.launch.py
 
 ## Launch Files
 * [sensor_kit.launch.py](./launch/sensor_kit.launch.py)
+  * Sample launch file to launch the top sensor kit sensors with the following configuration
+    * Camera model: realsense_d435 / rgb_camera
+
   | Argument      | Description                  | Default Value |
   | ------------- | ---------------------------- | ------------- |
   | use_namespace | Whether to apply a namespace | False         |
   | namespace     | Top-level namespace          | ""            |
+  | front_camera  | Front camera model           | none          |
+  | rear_camera   | Rear camera model            | none          |
+  | left_camera   | Left camera model            | none          |
+  | right_camera  | Right camera model           | none          |
 
 ## Nodes
 The package will launch the various sensors and their associated driver nodes/supporting nodes

diff --git a/src/kits/top/mid360_sensor_kit_bringup/config/front_d435.param.yaml b/src/kits/top/mid360_sensor_kit_bringup/config/front_d435.param.yaml
@@ -0,0 +1,7 @@
+serial_no: "241122070090"
+rgb_camera.color_profile: 640x480x15
+depth_module.depth_profile: 640x480x15
+depth_module.infra_profile: 640x480x15
+pointcloud.enable: True
+pointcloud.stream_filter: 0
+pointcloud.stream_index_filter: -1
diff --git a/src/kits/top/mid360_sensor_kit_bringup/config/left_d435.param.yaml b/src/kits/top/mid360_sensor_kit_bringup/config/left_d435.param.yaml
@@ -0,0 +1,7 @@
+serial_no: " "
+rgb_camera.color_profile: 640x480x15
+depth_module.depth_profile: 640x480x15
+depth_module.infra_profile: 640x480x15
+pointcloud.enable: True
+pointcloud.stream_filter: 0
+pointcloud.stream_index_filter: -1
diff --git a/src/kits/top/mid360_sensor_kit_bringup/config/rear_d435.param.yaml b/src/kits/top/mid360_sensor_kit_bringup/config/rear_d435.param.yaml
@@ -0,0 +1,7 @@
+serial_no: "244222076237"
+rgb_camera.color_profile: 640x480x15
+depth_module.depth_profile: 640x480x15
+depth_module.infra_profile: 640x480x15
+pointcloud.enable: True
+pointcloud.stream_filter: 0
+pointcloud.stream_index_filter: -1
diff --git a/src/kits/top/mid360_sensor_kit_bringup/config/right_d435.param.yaml b/src/kits/top/mid360_sensor_kit_bringup/config/right_d435.param.yaml
@@ -0,0 +1,7 @@
+serial_no: " "
+rgb_camera.color_profile: 640x480x15
+depth_module.depth_profile: 640x480x15
+depth_module.infra_profile: 640x480x15
+pointcloud.enable: True
+pointcloud.stream_filter: 0
+pointcloud.stream_index_filter: -1
diff --git a/src/kits/top/mid360_sensor_kit_bringup/launch/sensor_kit.launch.py b/src/kits/top/mid360_sensor_kit_bringup/launch/sensor_kit.launch.py
@@ -1,12 +1,13 @@
 from launch import LaunchDescription
-from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.actions import GroupAction, DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.conditions import IfCondition
 from launch.launch_context import LaunchContext
 from launch_ros.actions import Node, PushRosNamespace
 from launch_ros.substitutions import FindPackageShare
 from launch.some_substitutions_type import SomeSubstitutionsType
 from launch.substitution import Substitution
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
 from launch.utilities import perform_substitutions, normalize_to_list_of_substitutions
 from typing import Dict, Text
 
@@ -39,6 +40,7 @@ def perform(self, context: LaunchContext) -> Text:
         return document_string
 
 def generate_launch_description():
+
     # --------- Arguments ---------
     front_camera = LaunchConfiguration("front_camera")
     rear_camera = LaunchConfiguration("rear_camera")
@@ -150,10 +152,33 @@ def generate_launch_description():
         ),
     ])
 
+    # --------- Cameras ---------
+    camera_type = [front_camera, rear_camera, left_camera, right_camera]
+    camera_pos = ['front', 'rear', 'left', 'right']
+
+    camera_bringup = GroupAction([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                PathJoinSubstitution([
+                    FindPackageShare("realsense2_camera"),
+                    "launch",
+                    "rs_launch.py",
+                ])
+            ]),
+            condition=IfCondition(PythonExpression(["'", type, "' == 'realsense_d435'"])),
+            launch_arguments={
+                "camera_name": pos + "_d435",
+                "camera_namespace": pos + "_d435",
+                "config_file": PathJoinSubstitution([FindPackageShare("mid360_sensor_kit_bringup"), 'config', pos + '_d435.param.yaml'])
+            }.items(),
+        ) for type, pos in zip(camera_type, camera_pos)
+    ])
+
     return LaunchDescription([
         declare_use_namespace_arg,
         declare_namespace_arg,
         load_description,
         imu_bringup,
         lidar_bringup,
+        camera_bringup
     ])
diff --git a/src/kits/top/mid360_sensor_kit_bringup/package.xml b/src/kits/top/mid360_sensor_kit_bringup/package.xml
@@ -16,6 +16,17 @@
   <exec_depend>livox_ros_driver2</exec_depend>
   <exec_depend>xacro</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>pointcloud_to_laserscan</exec_depend>
+
+  <!-- Perception -->
+  <exec_depend>librealsense2</exec_depend>
+  <exec_depend>realsense2_camera</exec_depend>
+  <exec_depend>realsense2_description</exec_depend>
+
+  <!-- VSLAM-->
+  <exec_depend>rtabmap_slam</exec_depend>
+  <exec_depend>rtabmap_viz</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/src/wr_devkit_bringup/README.md b/src/wr_devkit_bringup/README.md
@@ -12,18 +12,13 @@ This package is meant to be a starting point for users to build upon and customi
     * Chassis: ugv_devkit_v1
     * Base sensor kit: w200d_sensor_kit
     * Top sensor kit: mid360_sensor_kit
-    * Camera model: realsense_d435 / rgb_camera
 
     | Argument      | Description                  | Default Value    |
     | ------------- | ---------------------------- | ---------------- |
     | use_namespace | Whether to apply a namespace | False            |
     | namespace     | Top-level namespace          | ""               |
     | use_sim_time  | Whether to use sim time      | False            |
     | robot_model   | Whether to use sim time      | ranger_mini_v2   |
-    | front_camera  | Front camera model           | none             |
-    | rear_camera   | Rear camera model            | none             |
-    | left_camera   | Left camera model            | none             |
-    | right_camera  | Right camera model           | none             |
 
 * [wr_devkit_cartographer.launch.py](./launch/slam/wr_devkit_cartographer.launch.py)
   * Sample launch file to perform 2D SLAM using cartographer
@@ -41,7 +36,7 @@ This package is meant to be a starting point for users to build upon and customi
 * [wr_devkit_rtabmap.launch.py](./launch/slam/wr_devkit_cartographer.launch.py)
   * Sample launch file to perform 2D vSLAM using rtabmap
   * Assumes the following configuration by default
-    * Top sensor kit: mid360_sensor_kit + realsense d435
+    * Top sensor kit: mid360_sensor_kit
 
   | Argument           | Description                                 | Default Value |
   | ------------------ | ------------------------------------------- | ------------- |
@@ -72,7 +67,7 @@ This package is meant to be a starting point for users to build upon and customi
   * Sample launch file to perform 2D navigation using nav2
     * Robot base: Scout mini
     * Chassis: ugv_devkit_v1
-    * Top sensor kit: mid360_sensor_kit + realsense d435
+    * Top sensor kit: mid360_sensor_kit
 
   | Argument       | Description                  | Default Value                                        |
   | -------------- | ---------------------------- | ---------------------------------------------------- |

diff --git a/src/wr_devkit_bringup/launch/platform/wr_devkit_platform.launch.py b/src/wr_devkit_bringup/launch/platform/wr_devkit_platform.launch.py
@@ -185,26 +185,6 @@ def generate_launch_description():
             launch_arguments={
                 "robot_base": "ranger_mini",
             }.items(),
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([
-                PathJoinSubstitution([
-                    FindPackageShare("realsense2_camera"),
-                    "launch",
-                    "rs_launch.py",
-                ])
-            ]),
-            condition=IfCondition(PythonExpression(["'", front_camera, "' == 'realsense_d435'"])),
-            launch_arguments={
-                "camera_name": "front_d435",
-                "camera_namespace": "front_d435",
-                "rgb_camera.color_profile": "640,480,15",
-                "depth_module.depth_profile": "640,480,15",
-                "depth_module.infra_profile": "640,480,15",
-                "pointcloud.enable": "true",
-                "pointcloud.stream_filter": "0",
-                "pointcloud.stream_index_filter": "-1"
-            }.items(),
         )
     ])