Added husky 3D lidar demos

rtabmap_demos/launch/husky_sim_demo.launch.py → rtabmap_demos/launch/husky_sim_scan2d_demo.launch.py

@@ -11,7 +11,7 @@
 #
 # Example with gazebo:
 #   1) Launch simulator (husky, nav2 and rtabmap):
-#     $ ros2 launch rtabmap_demos husky_sim_demo.launch.py robot_ns:=a200_0000
+#     $ ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py robot_ns:=a200_0000
 #
 #   2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
 #
@@ -59,7 +59,7 @@ def generate_launch_description():
     viz_launch = PathJoinSubstitution(
         [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
     rtabmap_launch = PathJoinSubstitution(
-        [pkg_rtabmap_demos, 'launch', 'husky_slam.launch.py'])
+        [pkg_rtabmap_demos, 'launch', 'husky_slam2d.launch.py'])
     nav2_launch = PathJoinSubstitution(
         [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
 

rtabmap_demos/launch/husky_sim_scan3d_assemble_demo.launch.py

@@ -0,0 +1,101 @@
+#
+# Requirements: 
+#   - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins
+#   - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml  
+#   - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro:
+#        <horizontal_fov>1.047</horizontal_fov>
+#        <image>
+#           <width>320</width>
+#           <height>240</height>
+#        </image>
+#
+# Example with gazebo:
+#   1) Launch simulator (husky, nav2 and rtabmap):
+#     $ ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py robot_ns:=a200_0000
+#
+#   2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
+#
+#   3) Move the robot:
+#     b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
+#     a) By teleoperating:
+#        $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
+#
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+import os
+
+ARGUMENTS = [
+    DeclareLaunchArgument('rtabmap_viz', default_value='true',
+                          choices=['true', 'false'], description='Start rtabmap_viz.'),
+    DeclareLaunchArgument('world', default_value='warehouse',
+                          description='Ignition World'),
+    DeclareLaunchArgument('robot_ns', default_value='a200_0000',
+                          description='Robot namespace'),
+]
+
+def generate_launch_description():
+    # Directories
+    pkg_clearpath_gz = get_package_share_directory(
+        'clearpath_gz')
+    pkg_clearpath_viz = get_package_share_directory(
+        'clearpath_viz')
+    pkg_rtabmap_demos = get_package_share_directory(
+        'rtabmap_demos')
+    pkg_clearpath_nav2_demos = get_package_share_directory(
+        'clearpath_nav2_demos')
+
+    # Paths
+    sim_launch = PathJoinSubstitution(
+        [pkg_clearpath_gz, 'launch', 'simulation.launch.py'])
+    viz_launch = PathJoinSubstitution(
+        [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
+    rtabmap_launch = PathJoinSubstitution(
+        [pkg_rtabmap_demos, 'launch', 'husky_slam3d_assemble.launch.py'])
+    nav2_launch = PathJoinSubstitution(
+        [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
+
+    sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([sim_launch]),
+        launch_arguments=[
+            ('world', LaunchConfiguration('world')),
+        ]
+    )
+
+    viz = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([viz_launch]),
+        launch_arguments=[
+            ('namespace', LaunchConfiguration('robot_ns')),
+        ]
+    )
+
+    rtabmap = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([rtabmap_launch]),
+        launch_arguments=[
+            ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
+            ('use_sim_time', 'true'),
+            ('robot_ns', LaunchConfiguration('robot_ns'))
+        ]
+    )
+
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([nav2_launch]),
+        launch_arguments=[
+            ('setup_path', os.path.expanduser('~')+'/clearpath/'),
+            ('use_sim_time', 'true'),
+        ]
+    )
+
+    # Create launch description and add actions
+    ld = LaunchDescription(ARGUMENTS)
+    ld.add_action(rtabmap)
+    ld.add_action(sim)
+    ld.add_action(viz)
+    ld.add_action(nav2)
+    return ld

rtabmap_demos/launch/husky_sim_scan3d_demo.launch.py

@@ -0,0 +1,104 @@
+#
+# Requirements: 
+#   - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins
+#   - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml  
+#   - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro:
+#        <horizontal_fov>1.047</horizontal_fov>
+#        <image>
+#           <width>320</width>
+#           <height>240</height>
+#        </image>
+#
+# Example with gazebo:
+#   1) Launch simulator (husky, nav2 and rtabmap):
+#     $ ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py robot_ns:=a200_0000
+#
+#   2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
+#
+#   3) Move the robot:
+#     b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
+#     a) By teleoperating:
+#        $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
+#
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+import os
+
+ARGUMENTS = [
+    DeclareLaunchArgument('rtabmap_viz', default_value='true',
+                          choices=['true', 'false'], description='Start rtabmap_viz.'),
+    DeclareLaunchArgument('localization', default_value='false',
+                          choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'),
+    DeclareLaunchArgument('world', default_value='warehouse',
+                          description='Ignition World'),
+    DeclareLaunchArgument('robot_ns', default_value='a200_0000',
+                          description='Robot namespace'),
+]
+
+def generate_launch_description():
+    # Directories
+    pkg_clearpath_gz = get_package_share_directory(
+        'clearpath_gz')
+    pkg_clearpath_viz = get_package_share_directory(
+        'clearpath_viz')
+    pkg_rtabmap_demos = get_package_share_directory(
+        'rtabmap_demos')
+    pkg_clearpath_nav2_demos = get_package_share_directory(
+        'clearpath_nav2_demos')
+
+    # Paths
+    sim_launch = PathJoinSubstitution(
+        [pkg_clearpath_gz, 'launch', 'simulation.launch.py'])
+    viz_launch = PathJoinSubstitution(
+        [pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
+    rtabmap_launch = PathJoinSubstitution(
+        [pkg_rtabmap_demos, 'launch', 'husky_slam3d.launch.py'])
+    nav2_launch = PathJoinSubstitution(
+        [pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
+
+    sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([sim_launch]),
+        launch_arguments=[
+            ('world', LaunchConfiguration('world')),
+        ]
+    )
+
+    viz = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([viz_launch]),
+        launch_arguments=[
+            ('namespace', LaunchConfiguration('robot_ns')),
+        ]
+    )
+
+    rtabmap = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([rtabmap_launch]),
+        launch_arguments=[
+            ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
+            ('localization', LaunchConfiguration('localization')),
+            ('use_sim_time', 'true'),
+            ('robot_ns', LaunchConfiguration('robot_ns'))
+        ]
+    )
+
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([nav2_launch]),
+        launch_arguments=[
+            ('setup_path', os.path.expanduser('~')+'/clearpath/'),
+            ('use_sim_time', 'true'),
+        ]
+    )
+
+    # Create launch description and add actions
+    ld = LaunchDescription(ARGUMENTS)
+    ld.add_action(rtabmap)
+    ld.add_action(sim)
+    ld.add_action(viz)
+    ld.add_action(nav2)
+    return ld

rtabmap_demos/launch/husky_slam.launch.py → rtabmap_demos/launch/husky_slam2d.launch.py

@@ -9,7 +9,7 @@
 #     $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000
 #
 #   3) Launch SLAM:
-#     $ ros2 launch rtabmap_demos husky_slam.launch.py use_sim_time:=true
+#     $ ros2 launch rtabmap_demos husky_slam2d.launch.py use_sim_time:=true
 #
 #   4) Launch nav2"
 #     $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true
@@ -35,7 +35,7 @@ def generate_launch_description():
     localization = LaunchConfiguration('localization')
     robot_ns = LaunchConfiguration('robot_ns')
 
-    icp_parameters={
+    icp_odom_parameters={
           'odom_frame_id':'icp_odom',
           'guess_frame_id':'odom'
     }
@@ -48,6 +48,7 @@ def generate_launch_description():
           # RTAB-Map's parameters should be strings:
           'Mem/NotLinkedNodesKept':'false',
           'Grid/RangeMin':'0.7', # ignore laser scan points on the robot itself
+          'RGBD/OptimizeMaxError':'2',
     }
 
     # Shared parameters between different nodes
@@ -56,9 +57,10 @@ def generate_launch_description():
           'use_sim_time':use_sim_time,
           # RTAB-Map's parameters should be strings:
           'Reg/Strategy':'1',
-          'Reg/Force3DoF':'true',
+          'Reg/Force3DoF':'true', # we are moving on a 2D flat floor
           'Mem/NotLinkedNodesKept':'false',
-          'Icp/PointToPlaneMinComplexity':'0.04' # to be more robust to long corridors with low geometry
+          'Icp/PointToPlaneMinComplexity':'0.04', # to be more robust to long corridors with low geometry
+          'Icp/MaxTranslation': '1'
     }
 
     remappings=[
@@ -95,7 +97,7 @@ def generate_launch_description():
         Node(
             package='rtabmap_odom', executable='icp_odometry', output='screen',
             namespace=robot_ns,
-            parameters=[icp_parameters, shared_parameters],
+            parameters=[icp_odom_parameters, shared_parameters],
             remappings=remappings,
             arguments=["--ros-args", "--log-level", 'warn']),