Ros2 humble port

summit_xl_gazebo/CMakeLists.txt

@@ -1,35 +1,12 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(summit_xl_gazebo)
 
-
-find_package(catkin REQUIRED COMPONENTS 
-    roscpp
-    std_srvs
-    std_msgs
-    tf
-    gazebo_ros
-    joint_state_controller
-    velocity_controllers
-    gazebo_ros_control
-)
-
-
-###################################
-## catkin specific configuration ##
-###################################
-catkin_package(
-
-)
-
-
-#############
-## Install ##
-#############
-
+find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY launch worlds rviz
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DIRECTORY launch worlds config
+  DESTINATION share/${PROJECT_NAME}
 )
 
-
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

summit_xl_gazebo/config/controller.yml

@@ -0,0 +1,50 @@
+controller_manager:
+  ros__parameters:
+    use_sim_time: true
+    update_rate: 200
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    robotnik_base_control:
+      type: diff_drive_controller/DiffDriveController
+
+joint_state_broadcaster:
+  ros__parameters:
+    use_sim_time: true
+    publish_rate: 100.0
+
+robotnik_base_control:
+  ros__parameters:
+    left_wheel_names:
+      - front_left_wheel_joint
+      - rear_left_wheel_joint
+    right_wheel_names:
+      - front_right_wheel_joint
+      - rear_right_wheel_joint
+    wheel_separation: 0.419
+    wheel_radius: 0.11
+    wheel_separation_multiplier: 1.0
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+    odom_frame_id: odom
+    base_frame_id: base_footprint
+    enable_odom_tf: true
+    cmd_vel_timeout: 0.1
+    publish_rate: 50.0
+    linear:
+      x:
+        has_velocity_limits: true
+        max_velocity: 1.5
+        has_acceleration_limits: true
+        max_acceleration: 2.5
+        has_jerk_limits: false
+        max_jerk: 0.0
+    angular:
+      z:
+        has_velocity_limits: true
+        max_velocity: 3.0
+        has_acceleration_limits: true
+        max_acceleration: 2.5
+        has_jerk_limits: false
+        max_jerk: 0.0

summit_xl_gazebo/config/gazebo.yml

@@ -0,0 +1,3 @@
+gazebo:
+  ros__parameters:
+    publish_rate: 1000.0 # /clock publish rate

summit_xl_gazebo/launch/default.launch.py

@@ -0,0 +1,165 @@
+# Copyright (c) 2023, Robotnik Automation S.L.L.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * 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.
+#     * Neither the name of the Robotnik Automation S.L.L. 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 Robotnik Automation S.L.L. 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 launch
+import launch_ros
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+#from robotnik_common.launch import RewrittenYaml
+
+# Environment variables
+#  USE_SIM_TIME: Use simulation (Gazebo) clock if true
+#  NAMESPACE: Namespace of the node stack.
+#  ROBOT_ID: Frame id of the robot. (e.g. vectornav_link).
+#  WORLD: World to load.
+
+def read_params(ld : launch.LaunchDescription):
+  environment = launch.substitutions.LaunchConfiguration('environment')
+  use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time')
+  robot_id = launch.substitutions.LaunchConfiguration('robot_id')
+  namespace = launch.substitutions.LaunchConfiguration('namespace')
+  world_name = launch.substitutions.LaunchConfiguration('world_name')
+  world = launch.substitutions.LaunchConfiguration('world')
+
+  ld.add_action(launch.actions.DeclareLaunchArgument(
+    name='environment',
+    description='Read parameters from environment variables',
+    choices=['true', 'false'],
+    default_value='true',
+  ))
+
+  ld.add_action(launch.actions.DeclareLaunchArgument(
+    name='use_sim_time',
+    description='Use simulation (Gazebo) clock if true',
+    choices=['true', 'false'],
+    default_value='true',
+  ))
+
+  ld.add_action(launch.actions.DeclareLaunchArgument(
+    name='robot_id',
+    description='Id of the robot',
+    default_value='robot',
+  ))
+
+  ld.add_action(launch.actions.DeclareLaunchArgument(
+    name='namespace',
+    description='Namespace of the node stack',
+    default_value=robot_id,
+  ))
+
+  ld.add_action(launch.actions.DeclareLaunchArgument(
+    name='world_name',
+    description='Name of the world to load',
+    default_value='empty',
+  ))
+
+  ld.add_action(launch.actions.DeclareLaunchArgument(
+    name='world',
+    description='World to load',
+    default_value=[launch_ros.substitutions.FindPackageShare('summit_xl_gazebo'), '/worlds/', world_name, '.world']
+  ))
+
+  ret = {}
+
+  if environment == 'false':
+    ret = {
+      'use_sim_time': use_sim_time,
+      'namespace': namespace,
+      'robot_id': robot_id,
+      'world': world,
+    }
+  else:
+    if 'USE_SIM_TIME' in os.environ:
+      ret['use_sim_time'] = os.environ['USE_SIM_TIME']
+    else:
+      ret['use_sim_time'] = use_sim_time
+    if 'NAMESPACE' in os.environ:
+      ret['namespace'] = os.environ['NAMESPACE']
+    elif 'ROBOT_ID' in os.environ:
+      ret['namespace'] = os.environ['ROBOT_ID']
+    else:
+      ret['namespace'] = namespace
+    if 'ROBOT_ID' in os.environ:
+      ret['robot_id'] = os.environ['ROBOT_ID']
+    else:
+      ret['robot_id'] = robot_id
+    if 'WORLD' in os.environ:
+      ret['world'] = os.environ['WORLD']
+    elif 'WORLD_NAME' in os.environ:
+      ret['world'] = [launch_ros.substitutions.FindPackageShare('summit_xl_gazebo'), '/worlds/', os.environ['WORLD_NAME'], '.world']
+    else:
+      ret['world'] = world
+
+  return ret
+
+
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+  ld = launch.LaunchDescription()
+  summit_xl_gazebo = get_package_share_directory('summit_xl_gazebo')
+  gazebo_ros = get_package_share_directory('gazebo_ros')
+
+  params = read_params(ld)
+
+  ld.add_action(launch.actions.IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(
+      os.path.join(gazebo_ros, 'launch', 'gzserver.launch.py')
+    ),
+    launch_arguments={
+      'verbose': 'false',
+      'world': params['world'],
+      'paused': 'false',
+      'init': 'true',
+      'factory': 'true',
+      'force_system': 'true',
+      'params_file': os.path.join(summit_xl_gazebo, 'config','gazebo.yml'),
+    }.items(),
+  ))
+
+  ld.add_action(launch.actions.IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(
+      os.path.join(gazebo_ros, 'launch', 'gzclient.launch.py')
+    ),
+    launch_arguments={
+      'verbose': 'false',
+      }.items(),
+  ))
+
+  ld.add_action(launch.actions.IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(
+      os.path.join(summit_xl_gazebo, 'launch', 'spawn.launch.py')
+    ),
+    launch_arguments={
+      'use_sim_time': params['use_sim_time'],
+      'robot_id': params['robot_id'],
+      'namespace': params['namespace'],
+      'pos_x': '1.0',
+      'pos_y': '1.0',
+      }.items(),
+  ))
+
+  return ld

summit_xl_gazebo/launch/description.launch.py

@@ -0,0 +1,109 @@
+# Copyright (c) 2023, Robotnik Automation S.L.L.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * 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.
+#     * Neither the name of the Robotnik Automation S.L.L. 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 Robotnik Automation S.L.L. 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
+import launch
+import launch_ros
+from ament_index_python.packages import get_package_share_directory
+
+from robotnik_common.launch import RewrittenYaml
+
+def read_params(ld : launch.LaunchDescription):
+    use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time')
+    controllers_file = launch.substitutions.LaunchConfiguration('controllers_file')
+    robot_id = launch.substitutions.LaunchConfiguration('robot_id')
+
+    # Declare the launch options
+    ld.add_action(launch.actions.DeclareLaunchArgument(
+        name='use_sim_time',
+        description='Use simulation (Gazebo) clock if true',
+        choices=['true', 'false'],
+        default_value='true')
+    )
+
+    ld.add_action(launch.actions.DeclareLaunchArgument(
+        name='controllers_file',
+        description='ROS 2 controller file.',
+        default_value=[get_package_share_directory('summit_xl_gazebo'), '/config/controller.yml'])
+    )
+
+    ld.add_action(launch.actions.DeclareLaunchArgument(
+        name='robot_id',
+        description='Robot ID used to create the robot namespace',
+        default_value='robot')
+    )
+
+    # Parse the launch options
+    return {
+        'use_sim_time': use_sim_time,
+        'robot_description_path': os.path.join(get_package_share_directory('summit_xl_description'), 'robots', 'summit_xl.urdf.xacro'),
+        'robot_id': robot_id,
+        'controllers_file': controllers_file,
+    }
+
+def generate_launch_description():
+
+    ld = launch.LaunchDescription()
+
+    params = read_params(ld)
+
+    config_file_rewritten = RewrittenYaml(
+        source_file=params['controllers_file'],
+        param_rewrites={},
+        root_key=[params['robot_id'],],
+        convert_types=True,
+    )
+
+    robot_description_content = launch.substitutions.Command(
+        [
+            launch.substitutions.PathJoinSubstitution(
+                [launch.substitutions.FindExecutable(name="xacro")]),
+            " ",
+            params['robot_description_path'],
+            " robot_id:=", params['robot_id'],
+            " robot_ns:=", params['robot_id'],
+            " config_controllers:=", config_file_rewritten,
+        ]
+    )
+
+    # Create parameter
+    robot_description_param = launch_ros.descriptions.ParameterValue(robot_description_content, value_type=str)
+
+    robot_state_publisher = launch_ros.actions.Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'use_sim_time': params['use_sim_time'],
+            'robot_description': robot_description_param,
+            'publish_frequency': 100.0,
+            'frame_prefix': [params['robot_id'], '/'],
+        }],
+    )
+
+    ld.add_action(robot_state_publisher)
+
+    return ld