Merge pull request #25 from a2s-institute/mock-test

Mock test

README.md

@@ -20,15 +20,7 @@ To launch the system use:
 ros2 launch solo12_bringup robot.launch.py
 ```
 
-Launch arguments:
-- `stand`: Spawn robot on stand (default true).
-- `use_rviz`: Run rviz (default true).
-
-Example:
-
-```
-ros2 launch solo12_bringup robot.launch.py stand:=true use_rviz:=false
-```
+to get more details of the availables launch arguments see [solo12_bringup](./solo12_bringup/README.md) package.
 
 ## Packages
 

solo12_bringup/README.md

@@ -0,0 +1,53 @@
+# `solo12_bringup`
+
+This is the main package to launch all the components.
+
+## Usage: `robot.launch.py`
+
+To launch the full system is `robot.launch.py`, example:
+
+```bash
+ros2 launch solo12_bringup robot.launch.py
+```
+
+Launch arguments:
+- `use_sim`: Start Gazebo (default true), otherwise start hardware.
+- `start_rviz`: Run rviz (default true).
+- `rviz_config_file`: Custom rviz config file (default `solo12_description/rviz/solo12.rviz`)
+- `robot_name`: Robot name (default "solo12"). For now just set the gazebo entity name, but this can be used later to spawn multiples robot.
+- `stand`: Spawn robot on stand (default true).
+
+Example:
+
+```
+ros2 launch solo12_bringup robot.launch.py stand:=true use_rviz:=false
+```
+
+## `state_publisher.launch.py`
+
+This launch file is the **main and only** source for the robot_description. If you want to make use of the `robot_description`, please subscribe to the topic `/robot_description`.
+
+Launch arguments:
+- `use_sim_time`: Sync ros and gazebo time (default true)
+
+## `controllers.launch.py`
+
+Launch all the components related with `ros2_control`.
+
+Launch arguments:
+- `use_sim_time`: Sync ros and gazebo time (default true)
+
+## `sim_gazebo.launch.py`
+
+Start the gazebo simulation environment. Spawn the robot and the stand.
+
+Launch arguments:
+- `stand`: spawn stand model into gazebo (default true).
+- `robot_name`: robot entity name in gazebo (default "solo12")
+
+## `start_rviz.launch.py`
+
+Start rviz.
+
+Launch arguments:
+- `rviz_config_file`: Custom cofiguration file to start rviz (no default value)

solo12_bringup/launch/controllers.launch.py

@@ -0,0 +1,62 @@
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction, DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+def launch_args(context):
+
+    declared_args = []
+
+    declared_args.append(
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true"
+        )
+    )
+
+    return declared_args
+
+
+def launch_setup(context):
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("solo12_description"),
+            "config",
+            "solo12_controllers.yaml"
+        ]
+    )
+
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[
+            robot_controllers
+        ],
+        output="both",
+        emulate_tty=True
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+        parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}]
+    )
+
+    return [
+        ros2_control_node,
+        joint_state_broadcaster_spawner
+    ]
+
+
+def generate_launch_description():
+
+    ld = LaunchDescription()
+
+    ld.add_action(OpaqueFunction(function=launch_args))
+
+    ld.add_action(OpaqueFunction(function=launch_setup))
+
+    return ld

solo12_bringup/launch/robot.launch.py

@@ -1,157 +1,130 @@
-import os
-import xacro
-from launch.event_handlers import OnProcessExit
-from launch.launch_context import LaunchContext
 from launch import LaunchDescription
+from launch.actions import OpaqueFunction, DeclareLaunchArgument, IncludeLaunchDescription
 from launch.conditions import IfCondition
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
 from launch_ros.substitutions import FindPackageShare
-from ament_index_python.packages import get_package_share_directory
 
-def generate_launch_description():
+def launch_args(context):
 
-    declared_arguments = []
+    declared_args = []
 
-    declared_arguments.append(
+    declared_args.append(
         DeclareLaunchArgument(
-            "use_rviz",
+            "use_sim",
             default_value="true",
-            description="Start RViz2 automatically with this launch file.")
+            description="Start robot in simulation, otherwise launch for physical hardware."
+        )
     )
 
-    declared_arguments.append(
+    declared_args.append(
         DeclareLaunchArgument(
-            "stand",
+            "start_rviz",
             default_value="true",
-            description="Spawn stand for the robot."
+            description="Start RViz2 automatically with this launch file."
         )
     )
 
-    use_rviz = LaunchConfiguration("use_rviz")
-    stand_arg = LaunchConfiguration("stand")
-
-    rviz_config_file = PathJoinSubstitution(
-        [
-            FindPackageShare("solo12_description"),
-            "rviz",
-            "solo12.rviz"
-        ]
-    )
-
-    # Get URDF path
-    urdf_path = os.path.join(
-        get_package_share_directory("solo12_description"),
-        'urdf',
-        'solo12.urdf.xacro'
-    )
-
-    # Spawn stand as a different entity.
-    #
-    # The first part of the command parse the urdf file
-    # using the `xacro` command, this is required to resolved
-    # the path of the meshes in the urdf file.
-    # The second part use the `spawn_entity.py` node with 
-    # the option `-stdin` that get the urdf from the standard
-    # input.
-    spawn_stand = ExecuteProcess(
-        cmd=[
-            "xacro",
-            PathJoinSubstitution(
+    declared_args.append(
+        DeclareLaunchArgument(
+            "rviz_config_file",
+            default_value=PathJoinSubstitution(
                 [
                     FindPackageShare("solo12_description"),
-                    "urdf",
-                    "stand.urdf"
+                    "rviz",
+                    "solo12.rviz"
                 ]
             ),
-            "|",
-            "ros2", "run", "gazebo_ros", "spawn_entity.py", "-stdin", "-entity", "stand"
-        ],
-        output="both",
-        shell=True,
-        condition=IfCondition(stand_arg)
+            description="Cofiguration file for rviz"
+        )
     )
 
-    # robot description parameter composition
-    robot_description = {"robot_description": xacro.process_file(urdf_path).toxml()}
+    declared_args.append(
+        DeclareLaunchArgument(
+            "robot_name",
+            default_value="solo12",
+            description="Robot name."
+        )
+    )
 
-    gazebo_configuration = PathJoinSubstitution(
-        [
-            FindPackageShare("solo12_description"),
-            "config",
-            "gazebo.yaml"
-        ]
+    declared_args.append(
+        DeclareLaunchArgument(
+            "stand",
+            default_value="true",
+            description="Spawn the robot in a stand."
+        )
     )
 
-    gazebo = IncludeLaunchDescription(
+    return declared_args
+
+def launch_setup(context):
+
+    state_publisher = IncludeLaunchDescription(
         PathJoinSubstitution(
             [
-                FindPackageShare("gazebo_ros"),
+                FindPackageShare("solo12_bringup"),
                 "launch",
-                "gazebo.launch.py"
+                "state_publisher.launch.py"
             ]
         ),
         launch_arguments={
-            "extra_gazebo_args": f"--ros-args --params-file {gazebo_configuration}"
+            "use_sim_time": LaunchConfiguration("use_sim")
         }.items()
     )
 
-    spawn_entity_node = Node(
-        package="gazebo_ros",
-        executable="spawn_entity.py",
-        name="spawn_entity",
-        arguments=['-topic', "robot_description",
-                   "-x", "0.0",
-                   "-y", "0.0",
-                   "-z", "0.54", 
-                   "-entity", "solo12"]
+    sim_gazebo = IncludeLaunchDescription(
+        PathJoinSubstitution(
+            [
+                FindPackageShare("solo12_bringup"),
+                "launch",
+                "sim_gazebo.launch.py"
+            ]
+        ),
+        launch_arguments={
+            "robot_name": LaunchConfiguration("robot_name"),
+            "stand": LaunchConfiguration("stand")
+        }.items(),
+        condition=IfCondition(LaunchConfiguration("use_sim"))
+    )
+
+    controllers = IncludeLaunchDescription(
+        PathJoinSubstitution(
+            [
+                FindPackageShare("solo12_bringup"),
+                "launch",
+                "controllers.launch.py"
+            ]
+        ),
+        launch_arguments={
+            "use_sim_time": LaunchConfiguration("use_sim")
+        }.items()
     )
 
-    joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
-        parameters=[{"use_sim_time": True}]
+    rviz = IncludeLaunchDescription(
+        PathJoinSubstitution(
+            [
+                FindPackageShare("solo12_bringup"),
+                "launch",
+                "start_rviz.launch.py",
+            ]
+        ),
+        launch_arguments={
+            "rviz_config_file": LaunchConfiguration("rviz_config_file")
+        }.items(),
     )
 
+    return [
+        state_publisher,
+        sim_gazebo,
+        controllers,
+        rviz
+    ]
 
-    robot_state_pub_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        output="both",
-        parameters=[
-            robot_description,
-            {"use_sim_time": True}
-        ],
-    )
+def generate_launch_description():
 
-    rviz_node = Node(
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        output="log",
-        arguments=["-d", rviz_config_file],
-        condition=IfCondition(use_rviz)
-    )
+    ld = LaunchDescription()
 
-    # Delay rviz start after `joint_state_broadcaster`
-    delay_rviz = RegisterEventHandler(
-        event_handler=OnProcessExit(
-            target_action=joint_state_broadcaster_spawner,
-            on_exit=[rviz_node],
-        )
-    )
+    ld.add_action(OpaqueFunction(function=launch_args))
 
+    ld.add_action(OpaqueFunction(function=launch_setup))
 
-    return LaunchDescription(
-        declared_arguments + 
-        [
-            gazebo,
-            robot_state_pub_node,
-            spawn_entity_node,
-            spawn_stand,
-            joint_state_broadcaster_spawner,
-            delay_rviz
-        ]
-    )
+    return ld