Release/pre devel 0.4

.github/workflows/lint.yml

@@ -47,14 +47,16 @@ jobs:
           ament_flake8 --config ament_flake8.ini \
             movensys_navigation_description/launch \
             movensys_navigation_nav2_config/launch \
-            movensys_navigation_nav2_config/movensys_navigation_nav2_config
+            movensys_navigation_nav2_config/movensys_navigation_nav2_config \
+            movensys_navigation_perception/launch
 
       - name: ament_xmllint (package.xml)
         run: |
           source /opt/ros/${{ matrix.ros_distro }}/setup.bash
           ament_xmllint \
             movensys_navigation_description/package.xml \
-            movensys_navigation_nav2_config/package.xml
+            movensys_navigation_nav2_config/package.xml \
+            movensys_navigation_perception/package.xml
 
       - name: cppcheck (warnings only, never fails)
         run: |

.github/workflows/movensys-navigation.yml

@@ -25,8 +25,6 @@ jobs:
 
       - name: Setup ROS ${{ matrix.ros_distro }}
         uses: ros-tooling/setup-ros@v0.7
-        with:
-          required-ros-distributions: ${{ matrix.ros_distro }}
 
       - name: Setup workspace
         run: |
@@ -36,14 +34,20 @@ jobs:
       - name: Install dependencies
         run: |
           sudo apt-get update
+          sudo apt-get install -y --no-install-recommends \
+            ros-${{ matrix.ros_distro }}-ros-base
           # GH ubuntu-22.04 runners pin LLVM's libunwind, blocking the stock
           # libunwind-dev that libgoogle-glog-dev (via slam-toolbox) needs.
           sudo apt-get install -y libunwind-dev
           sudo rosdep init || true
           rosdep update
           cd ~/movensys_ws
+          # This job only compiles (BUILD_TESTING=OFF), so runtime deps such as
+          # navigation2, ros_gz_sim and rviz2 are not needed.
           rosdep install --from-paths src --ignore-src -y \
-            --rosdistro ${{ matrix.ros_distro }}
+            --rosdistro ${{ matrix.ros_distro }} \
+            -t buildtool -t build -t buildtool_export -t build_export \
+            --skip-keys isaac_ros_visual_slam
 
       - name: Build
         run: |

.github/workflows/test-nav2.yml

@@ -27,8 +27,6 @@ jobs:
 
       - name: Setup ROS ${{ matrix.ros_distro }}
         uses: ros-tooling/setup-ros@v0.7
-        with:
-          required-ros-distributions: ${{ matrix.ros_distro }}
 
       - name: Setup workspace
         run: |
@@ -38,14 +36,18 @@ jobs:
       - name: Install dependencies
         run: |
           sudo apt-get update
+          sudo apt-get install -y --no-install-recommends \
+            ros-${{ matrix.ros_distro }}-ros-base
           # GH ubuntu-22.04 runners pin LLVM's libunwind, blocking the stock
           # libunwind-dev that libgoogle-glog-dev (via slam-toolbox) needs.
           sudo apt-get install -y libunwind-dev
           sudo rosdep init || true
           rosdep update
           cd ~/movensys_ws
           rosdep install --from-paths src --ignore-src -y \
-            --rosdistro ${{ matrix.ros_distro }}
+            --rosdistro ${{ matrix.ros_distro }} \
+            -t buildtool -t build -t buildtool_export -t build_export -t test \
+            --skip-keys isaac_ros_visual_slam
 
       - name: Build
         run: |

.github/workflows/test-perception.yml

@@ -25,8 +25,6 @@ jobs:
 
       - name: Setup ROS ${{ matrix.ros_distro }}
         uses: ros-tooling/setup-ros@v0.7
-        with:
-          required-ros-distributions: ${{ matrix.ros_distro }}
 
       - name: Setup workspace
         run: |
@@ -36,14 +34,18 @@ jobs:
       - name: Install dependencies
         run: |
           sudo apt-get update
+          sudo apt-get install -y --no-install-recommends \
+            ros-${{ matrix.ros_distro }}-ros-base
           # GH ubuntu-22.04 runners pin LLVM's libunwind, blocking the stock
           # libunwind-dev that libgoogle-glog-dev (via slam-toolbox) needs.
           sudo apt-get install -y libunwind-dev
           sudo rosdep init || true
           rosdep update
           cd ~/movensys_ws
           rosdep install --from-paths src/movensys-navigation/movensys_navigation_perception \
-            --ignore-src -y --rosdistro ${{ matrix.ros_distro }}
+            --ignore-src -y --rosdistro ${{ matrix.ros_distro }} \
+            -t buildtool -t build -t buildtool_export -t build_export -t test \
+            --skip-keys isaac_ros_visual_slam
 
       - name: Build
         run: |

.github/workflows/unit-test.yml

@@ -45,7 +45,9 @@ jobs:
           rosdep update
           cd ~/movensys_ws
           rosdep install --from-paths src --ignore-src -y \
-            --rosdistro ${{ matrix.ros_distro }}
+            --rosdistro ${{ matrix.ros_distro }} \
+            -t buildtool -t build -t buildtool_export -t build_export -t test \
+            --skip-keys isaac_ros_visual_slam
 
       - name: colcon build
         run: |
@@ -54,6 +56,7 @@ jobs:
           colcon build --symlink-install \
             --packages-select \
               movensys_navigation_description \
+              movensys_navigation_perception \
               movensys_navigation_nav2_config
 
       - name: colcon test
@@ -63,6 +66,7 @@ jobs:
           source install/setup.bash
           colcon test --packages-select \
               movensys_navigation_description \
+              movensys_navigation_perception \
               movensys_navigation_nav2_config \
             --event-handlers console_cohesion+
           colcon test-result --verbose

doc/3a_manual_simulation.md

@@ -25,9 +25,9 @@ nros ros2 launch movensys_navigation_nav2_config sim_bridge.launch.py use_sim_ti
 
 ### Step 3: Run EKF + RSP
 ```
-nros ros2 launch movensys_navigation_nav2_config base.launch.py use_sim_time:=true
+nros ros2 launch movensys_navigation_nav2_config base.launch.py use_sim_time:=true 
 ```
-add `rsp:=false` if use gazebo (step 1b).
+add `rsp:=false` if use gazebo (step 1b) and add `use_cuvslam:=true` for use cuvslam.
 
 
 

doc/3b_manual_hil.md

@@ -23,7 +23,7 @@ set `use_sim_time:=true`
 ```
 nros ros2 launch movensys_navigation_nav2_config base.launch.py use_sim_time:=true 
 ```
-add `rsp:=false` if use gazebo (step 1b) or ros2_control.
+add `rsp:=false` if use gazebo (step 1b) or ros2_control add `use_cuvslam:=true` for use cuvslam.
 
 
 

doc/3c_manual_real.md

@@ -12,7 +12,7 @@ check `~/workspaces/movensys_ws/src/wmx-r2/doc/launch_<NAVIGATION_MODEL>_navigat
 ```
 nros ros2 launch movensys_navigation_nav2_config base.launch.py
 ```
-add `rsp:=false` if use ros2_control.
+add `rsp:=false` if use ros2_control add `use_cuvslam:=true` for use cuvslam.
 
 
 

doc/4a_mapping_simulation.md

@@ -25,9 +25,9 @@ nros ros2 launch movensys_navigation_nav2_config sim_bridge.launch.py use_sim_ti
 
 ### Step 3: Mapping
 ```
-nros ros2 launch movensys_navigation_nav2_config mapping.launch.py use_sim_time:=true
+nros ros2 launch movensys_navigation_nav2_config mapping.launch.py use_sim_time:=true 
 ```
-add `rsp:=false` if use gazebo (step 1b)
+add `rsp:=false` if use gazebo (step 1b) add `use_cuvslam:=true` for use cuvslam
 
 
 

doc/4b_mapping_hil.md

@@ -25,7 +25,7 @@ set `use_sim_time:=true`
 ```
 nros ros2 launch movensys_navigation_nav2_config mapping.launch.py use_sim_time:=true
 ```
-add `rsp:=false` if use gazebo (step 1b) or ros2_control.
+add `rsp:=false` if use gazebo (step 1b) or ros2_control add `use_cuvslam:=true` for use cuvslam.
 
 
 

doc/4c_mapping_real.md

@@ -17,7 +17,7 @@ check `~/workspaces/movensys_ws/src/wmx-r2/doc/launch_<NAVIGATION_MODEL>_navigat
 ```
 nros ros2 launch movensys_navigation_nav2_config mapping.launch.py
 ```
-add `rsp:=false` if use ros2_control.
+add `rsp:=false` if use ros2_control add `use_cuvslam:=true` for use cuvslam.
 
 
 

doc/5a_navigation_simulation.md

@@ -26,7 +26,7 @@ nros ros2 launch movensys_navigation_nav2_config sim_bridge.launch.py use_sim_ti
 ```
 nros ros2 launch movensys_navigation_nav2_config navigation.launch.py use_sim_time:=true
 ```
-add `rsp:=false` if use gazebo (step 1b).
+add `rsp:=false` if use gazebo (step 1b) add `use_cuvslam:=true` for use cuvslam.
 
 
 

doc/5b_navigation_hil.md

@@ -21,7 +21,7 @@ set `use_sim_time:=true`
 ```
 nros ros2 launch movensys_navigation_nav2_config navigation.launch.py use_sim_time:=true
 ```
-add `rsp:=false` if use gazebo (step 1b) or ros2_control.
+add `rsp:=false` if use gazebo (step 1b) or ros2_control add `use_cuvslam:=true` for use cuvslam.
 
 
 

doc/5c_navigation_real.md

@@ -12,7 +12,7 @@ check `~/workspaces/movensys_ws/src/wmx-r2/doc/launch_<NAVIGATION_MODEL>_navigat
 ```
 nros ros2 launch movensys_navigation_nav2_config navigation.launch.py
 ```
-add `rsp:=false` if use ros2_control.
+add `rsp:=false` if use ros2_control add `use_cuvslam:=true` for use cuvslam.
 
 
 ### Step 3: Set initial pose

docker/isaac-ros_3.2.Dockerfile

@@ -26,6 +26,7 @@ RUN apt-get update && \
         ros-humble-rmw-cyclonedds-cpp \
         ros-humble-tf-transformations \
         ros-humble-isaac-ros-nvblox \
+        ros-humble-isaac-ros-visual-slam \
         ros-humble-isaac-ros-examples \
         ros-humble-isaac-ros-realsense \
         ros-humble-isaac-ros-ess \

docker/isaac-ros_4.1.Dockerfile

@@ -36,6 +36,7 @@ RUN apt-get update && \
 RUN apt-get update && \
     apt-get install -y \
       ros-jazzy-isaac-ros-nvblox \
+      ros-jazzy-isaac-ros-visual-slam \
       ros-jazzy-robot-localization \
       ros-jazzy-slam-toolbox \
       ros-jazzy-navigation2 \

movensys_navigation_description/launch/gazebo_navigation_simulation.launch.py

@@ -19,6 +19,9 @@ def generate_launch_description():
     clock_msg = {'humble': 'ignition.msgs.Clock', 'jazzy': 'gz.msgs.Clock'}[ros_distro]
     imu_msg = {'humble': 'ignition.msgs.IMU', 'jazzy': 'gz.msgs.IMU'}[ros_distro]
     lidar_msg = {'humble': 'ignition.msgs.LaserScan', 'jazzy': 'gz.msgs.LaserScan'}[ros_distro]
+    image_msg = {'humble': 'ignition.msgs.Image', 'jazzy': 'gz.msgs.Image'}[ros_distro]
+    camera_info_msg = {'humble': 'ignition.msgs.CameraInfo',
+                       'jazzy': 'gz.msgs.CameraInfo'}[ros_distro]
 
     world_file = {'humble': 'arena_ign.world', 'jazzy': 'arena_gz.world'}[ros_distro]
 
@@ -69,6 +72,14 @@ def generate_launch_description():
             f'/imu@sensor_msgs/msg/Imu[{imu_msg}',
             f'/lidar_front_left_scan@sensor_msgs/msg/LaserScan[{lidar_msg}',
             f'/lidar_rear_right_scan@sensor_msgs/msg/LaserScan[{lidar_msg}',
+            f'/image_front_infra1/rgb@sensor_msgs/msg/Image[{image_msg}',
+            f'/image_front_infra1/rgb/camera_info@sensor_msgs/msg/CameraInfo[{camera_info_msg}',
+            f'/image_front_infra2/rgb@sensor_msgs/msg/Image[{image_msg}',
+            f'/image_front_infra2/rgb/camera_info@sensor_msgs/msg/CameraInfo[{camera_info_msg}',
+        ],
+        remappings=[
+            ('/image_front_infra1/rgb/camera_info', '/image_front_infra1/camera_info'),
+            ('/image_front_infra2/rgb/camera_info', '/image_front_infra2/camera_info'),
         ],
         output='screen'
     )

movensys_navigation_description/urdf/diffbot/movensys_navigation.urdf

@@ -111,6 +111,50 @@
       </geometry>
     </visual>
   </link>
+  <joint name="camera_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="camera_front_link"/>
+    <origin rpy="0 0 0" xyz="0.4 0.0 0.17"/>
+  </joint>
+  <link name="camera_front_link">
+    <inertial>
+      <mass value="0.072"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.025 0.09 0.025"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.025 0.09 0.025"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="camera_front_infra1_optical_joint" type="fixed">
+    <parent link="camera_front_link"/>
+    <child link="camera_front_infra1_optical_frame"/>
+    <origin rpy="-1.5707963 0 -1.5707963" xyz="0 0 0"/>
+  </joint>
+
+  <link name="camera_front_infra1_optical_frame"/>
+
+  <joint name="camera_front_infra2_optical_joint" type="fixed">
+    <parent link="camera_front_link"/>
+    <child link="camera_front_infra2_optical_frame"/>
+    <origin rpy="-1.5707963 0 -1.5707963" xyz="0 -0.095 0"/>
+  </joint>
+
+  <link name="camera_front_infra2_optical_frame"/>
+
   <link name="base_link">
     <visual>
       <geometry>

movensys_navigation_description/urdf/diffbot/perception.gazebo

@@ -85,6 +85,51 @@
 		</sensor>
 	</gazebo>
 
+
+	<gazebo reference="camera_front_link">
+		<sensor name="camera_front_infra1_sensor" type="camera">
+			<pose>0 0 0 0 0 0</pose>
+			<always_on>true</always_on>
+			<visualize>false</visualize>
+			<update_rate>30</update_rate>
+			<topic>/image_front_infra1/rgb</topic>
+			<gz_frame_id>camera_front_infra1_optical_frame</gz_frame_id>
+			<camera>
+				<horizontal_fov>1.518</horizontal_fov>
+				<image>
+					<width>640</width>
+					<height>360</height>
+					<format>L8</format>
+				</image>
+				<clip>
+					<near>0.1</near>
+					<far>30.0</far>
+				</clip>
+			</camera>
+		</sensor>
+
+		<sensor name="camera_front_infra2_sensor" type="camera">
+			<pose>0 -0.095 0 0 0 0</pose>
+			<always_on>true</always_on>
+			<visualize>false</visualize>
+			<update_rate>30</update_rate>
+			<topic>/image_front_infra2/rgb</topic>
+			<gz_frame_id>camera_front_infra2_optical_frame</gz_frame_id>
+			<camera>
+				<horizontal_fov>1.518</horizontal_fov>
+				<image>
+					<width>640</width>
+					<height>360</height>
+					<format>L8</format>
+				</image>
+				<clip>
+					<near>0.1</near>
+					<far>30.0</far>
+				</clip>
+			</camera>
+		</sensor>
+	</gazebo>
+
 	<gazebo reference="drivewheel_left">
 		<collision>
 			<surface>