Adds andino_slam demo. Wip.

Signed-off-by: Franco Cipollone <[email protected]>

andino_gz_demos/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.7)
+project(andino_gz_demos)
+
+find_package(gazebo QUIET)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    launch
+    rviz
+  DESTINATION
+    share/${PROJECT_NAME}/
+)
+
+ament_package()

andino_gz_demos/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2024, Ekumen 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:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. 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.
+
+3. Neither the name of the copyright holder 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.

andino_gz_demos/launch/andino_slam.launch.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, GroupAction, IncludeLaunchDescription, LogInfo
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression, TextSubstitution
+from launch_ros.actions import Node, PushRosNamespace
+import xacro
+
+from nav2_common.launch import ParseMultiRobotPose
+
+def generate_launch_description():
+    pkg_andino_gz = get_package_share_directory('andino_gz')
+    pkg_andino_gz_demos = get_package_share_directory('andino_gz_demos')
+
+    rviz_arg = DeclareLaunchArgument('rviz', default_value='true', description='Start RViz.')
+    world_name_arg = DeclareLaunchArgument('world_name', default_value='depot.sdf', description='Name of the world to load.')
+    slam_params_file_arg = DeclareLaunchArgument(
+        'slam_params_file',
+        default_value=os.path.join(get_package_share_directory("andino_slam"),
+                                   'config', 'slam_toolbox_online_async.yaml'),
+        description='Full path to the ROS 2 parameters file to use for the slam_toolbox node')
+
+    world_name = LaunchConfiguration('world_name')
+    world_path = PathJoinSubstitution([pkg_andino_gz, 'worlds', world_name])
+
+    group = GroupAction([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_andino_gz, 'launch', 'andino_gz.launch.py')
+            ),
+            launch_arguments={
+                'robots': 'andino={x: 0., y: 0., z: 0.1, yaw: 0.};',
+                'world_name': world_path,
+                'ros_bridge': 'true',
+                'rviz': 'false',
+            }.items(),
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_andino_gz_demos, 'launch', 'include', 'slam_toolbox_online_async.launch.py')
+            ),
+            launch_arguments={
+                'slam_params_file_arg': LaunchConfiguration('slam_params_file')
+            }.items(),
+        ),
+    ])
+      # RViz
+    rviz_node = Node(
+          package='rviz2',
+          executable='rviz2',
+          arguments=['-d', os.path.join(pkg_andino_gz_demos, 'rviz', 'andino_slam.rviz')],
+          parameters=[{'use_sim_time': True}],
+          remappings=[
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+          ],
+          condition=IfCondition(LaunchConfiguration('rviz')),
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(rviz_arg)
+    ld.add_action(world_name_arg)
+    ld.add_action(slam_params_file_arg)
+    ld.add_action(rviz_node)
+    ld.add_action(group)
+    return ld

andino_gz_demos/launch/include/slam_toolbox_online_async.launch.py

@@ -0,0 +1,66 @@
+# BSD 3-Clause License
+
+# Copyright (c) 2024, Ekumen 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:
+
+# 1. Redistributions of source code must retain the above copyright notice, this
+#    list of conditions and the following disclaimer.
+
+# 2. 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.
+
+# 3. Neither the name of the copyright holder 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
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    slam_params_file = LaunchConfiguration('slam_params_file')
+
+    declare_slam_params_file_cmd = DeclareLaunchArgument(
+        'slam_params_file',
+        default_value=os.path.join(get_package_share_directory("andino_slam"),
+                                   'config', 'slam_toolbox_online_async.yaml'),
+        description='Full path to the ROS 2 parameters file to use for the slam_toolbox node')
+
+    start_async_slam_toolbox_node = Node(
+        parameters=[
+          slam_params_file,
+          {
+            'use_sim_time': True
+          }
+        ],
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen')
+
+    ld = LaunchDescription()
+
+    ld.add_action(declare_slam_params_file_cmd)
+    ld.add_action(start_async_slam_toolbox_node)
+
+    return ld

andino_gz_demos/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>andino_gz_demos</name>
+  <version>0.0.1</version>
+  <description>
+    Demos on top of andino_gz simulation.
+  </description>
+  <author email="[email protected]">Franco Cipollone</author>
+  <maintainer email="[email protected]">Franco Cipollone</maintainer>
+  <license file="LICENSE">BSD Clause 3</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>andino_gz</exec_depend>
+  <exec_depend>andino_slam</exec_depend>
+
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>