Import example code and basic prototype

src/draw_svg/launch/default.launch.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env -S ros2 launch
+"""Launch default world with the default robot (configurable)"""
+
+from os import path
+from typing import List
+
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+
+def generate_launch_description() -> LaunchDescription:
+
+    # Declare all launch arguments
+    declared_arguments = generate_declared_arguments()
+
+    # Get substitution for all arguments
+    world_type = LaunchConfiguration("world_type")
+    robot_type = LaunchConfiguration("robot_type")
+    rviz_config = LaunchConfiguration("rviz_config")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    ign_verbosity = LaunchConfiguration("ign_verbosity")
+    log_level = LaunchConfiguration("log_level")
+
+    # Determine what world/robot combination to launch
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "__world_launch_basename",
+            default_value=["world_", world_type, ".launch.py"],
+        ),
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "__robot_launch_basename",
+            default_value=["robot_", robot_type, ".launch.py"],
+        ),
+    )
+
+    # List of included launch descriptions
+    launch_descriptions = [
+        # Launch Ignition Gazebo with the required ROS<->IGN bridges
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("draw_svg"),
+                        "launch",
+                        "worlds",
+                        LaunchConfiguration("__world_launch_basename"),
+                    ]
+                )
+            ),
+            launch_arguments=[
+                ("use_sim_time", use_sim_time),
+                ("ign_verbosity", ign_verbosity),
+                ("log_level", log_level),
+            ],
+        ),
+        # Spawn robot
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("draw_svg"),
+                        "launch",
+                        "robots",
+                        LaunchConfiguration("__robot_launch_basename"),
+                    ]
+                )
+            ),
+            launch_arguments=[
+                ("use_sim_time", use_sim_time),
+                ("ign_verbosity", ign_verbosity),
+                ("log_level", log_level),
+            ],
+        ),
+        # Launch move_group of MoveIt 2
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare([robot_type, "_moveit_config"]),
+                        "launch",
+                        "move_group.launch.py",
+                    ]
+                )
+            ),
+            launch_arguments=[
+                ("ros2_control_plugin", "ign"),
+                ("ros2_control_command_interface", "effort"),
+                # TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled
+                ("collision_arm", "false"),
+                ("rviz_config", rviz_config),
+                ("use_sim_time", use_sim_time),
+                ("log_level", log_level),
+            ],
+        ),
+    ]
+
+    return LaunchDescription(declared_arguments + launch_descriptions)
+
+
+def generate_declared_arguments() -> List[DeclareLaunchArgument]:
+    """
+    Generate list of all launch arguments that are declared for this launch script.
+    """
+
+    return [
+        # World selection
+        DeclareLaunchArgument(
+            "world_type",
+            default_value="default",
+            description="Name of the world configuration to load.",
+        ),
+        # Robot selection
+        DeclareLaunchArgument(
+            "robot_type",
+            default_value="panda",
+            description="Name of the robot type to use.",
+        ),
+        # Miscellaneous
+        DeclareLaunchArgument(
+            "rviz_config",
+            default_value=path.join(
+                get_package_share_directory("draw_svg"),
+                "rviz",
+                "ign_moveit2_examples.rviz",
+            ),
+            description="Path to configuration for RViz2.",
+        ),
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="If true, use simulated clock.",
+        ),
+        DeclareLaunchArgument(
+            "ign_verbosity",
+            default_value="2",
+            description="Verbosity level for Ignition Gazebo (0~4).",
+        ),
+        DeclareLaunchArgument(
+            "log_level",
+            default_value="warn",
+            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
+        ),
+    ]

src/draw_svg/launch/draw_svg.launch.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env -S ros2 launch
+"""Launch Python example for following a target"""
+
+from os import path
+from typing import List
+
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+
+def generate_launch_description() -> LaunchDescription:
+
+    # Declare all launch arguments
+    declared_arguments = generate_declared_arguments()
+
+    # Get substitution for all arguments
+    robot_type = LaunchConfiguration("robot_type")
+    rviz_config = LaunchConfiguration("rviz_config")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    ign_verbosity = LaunchConfiguration("ign_verbosity")
+    log_level = LaunchConfiguration("log_level")
+
+    # List of included launch descriptions
+    launch_descriptions = [
+        # Launch world with robot (configured for this example)
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("draw_svg"),
+                        "launch",
+                        "default.launch.py",
+                    ]
+                )
+            ),
+            launch_arguments=[
+                ("world_type", "draw_svg"),
+                ("robot_type", robot_type),
+                ("rviz_config", rviz_config),
+                ("use_sim_time", use_sim_time),
+                ("ign_verbosity", ign_verbosity),
+                ("log_level", log_level),
+            ],
+        ),
+    ]
+
+    # List of nodes to be launched
+    nodes = [
+        # Run the example node (Python)
+        Node(
+            package="draw_svg",
+            executable="follow.py",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[{"use_sim_time": use_sim_time}],
+        ),
+        Node(
+            package="draw_svg",
+            executable="draw_svg.py",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[{"use_sim_time": use_sim_time}],
+        ),
+    ]
+
+    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
+
+
+def generate_declared_arguments() -> List[DeclareLaunchArgument]:
+    """
+    Generate list of all launch arguments that are declared for this launch script.
+    """
+
+    return [
+        # Robot selection
+        DeclareLaunchArgument(
+            "robot_type",
+            default_value="panda",
+            description="Name of the robot to use.",
+        ),
+        # Miscellaneous
+        DeclareLaunchArgument(
+            "rviz_config",
+            default_value=path.join(
+                get_package_share_directory("ign_moveit2_examples"),
+                "rviz",
+                "ign_moveit2_examples.rviz",
+            ),
+            description="Path to configuration for RViz2.",
+        ),
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="If true, use simulated clock.",
+        ),
+        DeclareLaunchArgument(
+            "ign_verbosity",
+            default_value="2",
+            description="Verbosity level for Ignition Gazebo (0~4).",
+        ),
+        DeclareLaunchArgument(
+            "log_level",
+            default_value="warn",
+            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
+        ),
+    ]

src/draw_svg/launch/robots/robot_panda.launch.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env -S ros2 launch
+"""Launch script for spawning Franka Emika Panda into Ignition Gazebo world"""
+
+from typing import List
+
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description() -> LaunchDescription:
+
+    # Declare all launch arguments
+    declared_arguments = generate_declared_arguments()
+
+    # Get substitution for all arguments
+    model = LaunchConfiguration("model")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    log_level = LaunchConfiguration("log_level")
+
+    # List of nodes to be launched
+    nodes = [
+        # ros_ign_gazebo_create
+        Node(
+            package="ros_ign_gazebo",
+            executable="create",
+            output="log",
+            arguments=["-file", model, "--ros-args", "--log-level", log_level],
+            parameters=[{"use_sim_time": use_sim_time}],
+        ),
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)
+
+
+def generate_declared_arguments() -> List[DeclareLaunchArgument]:
+    """
+    Generate list of all launch arguments that are declared for this launch script.
+    """
+
+    return [
+        # Model for Ignition Gazebo
+        DeclareLaunchArgument(
+            "model",
+            default_value="panda",
+            description="Name or filepath of model to load.",
+        ),
+        # Miscellaneous
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="If true, use simulated clock.",
+        ),
+        DeclareLaunchArgument(
+            "log_level",
+            default_value="warn",
+            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
+        ),
+    ]

src/draw_svg/launch/worlds/world_default.launch.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env -S ros2 launch
+"""Launch default.sdf and the required ROS<->IGN bridges"""
+
+from typing import List
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+
+def generate_launch_description() -> LaunchDescription:
+
+    # Declare all launch arguments
+    declared_arguments = generate_declared_arguments()
+
+    # Get substitution for all arguments
+    world = LaunchConfiguration("world")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    ign_verbosity = LaunchConfiguration("ign_verbosity")
+    log_level = LaunchConfiguration("log_level")
+
+    # List of included launch descriptions
+    launch_descriptions = [
+        # Launch Ignition Gazebo
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("ros_ign_gazebo"),
+                        "launch",
+                        "ign_gazebo.launch.py",
+                    ]
+                )
+            ),
+            launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
+        ),
+    ]
+
+    # List of nodes to be launched
+    nodes = [
+        # ros_ign_bridge (clock -> ROS 2)
+        Node(
+            package="ros_ign_bridge",
+            executable="parameter_bridge",
+            output="log",
+            arguments=[
+                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
+                "--ros-args",
+                "--log-level",
+                log_level,
+            ],
+            parameters=[{"use_sim_time": use_sim_time}],
+        ),
+    ]
+
+    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
+
+
+def generate_declared_arguments() -> List[DeclareLaunchArgument]:
+    """
+    Generate list of all launch arguments that are declared for this launch script.
+    """
+
+    return [
+        # World for Ignition Gazebo
+        DeclareLaunchArgument(
+            "world",
+            default_value="default.sdf",
+            description="Name or filepath of world to load.",
+        ),
+        # Miscellaneous
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="If true, use simulated clock.",
+        ),
+        DeclareLaunchArgument(
+            "ign_verbosity",
+            default_value="2",
+            description="Verbosity level for Ignition Gazebo (0~4).",
+        ),
+        DeclareLaunchArgument(
+            "log_level",
+            default_value="warn",
+            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
+        ),
+    ]

src/draw_svg/launch/worlds/world_draw_svg.launch.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env -S ros2 launch
+"""Launch worlds/follow_target.sdf and the required ROS<->IGN bridges"""
+
+from os import path
+from typing import List
+
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+
+def generate_launch_description() -> LaunchDescription:
+
+    # Declare all launch arguments
+    declared_arguments = generate_declared_arguments()
+
+    # Get substitution for all arguments
+    world = LaunchConfiguration("world")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    ign_verbosity = LaunchConfiguration("ign_verbosity")
+    log_level = LaunchConfiguration("log_level")
+
+    # List of included launch descriptions
+    launch_descriptions = [
+        # Launch Ignition Gazebo
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("ros_ign_gazebo"),
+                        "launch",
+                        "ign_gazebo.launch.py",
+                    ]
+                )
+            ),
+            launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
+        ),
+    ]
+
+    # List of nodes to be launched
+    nodes = [
+        # ros_ign_bridge (clock -> ROS 2)
+        Node(
+            package="ros_ign_bridge",
+            executable="parameter_bridge",
+            output="log",
+            arguments=[
+                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
+                "--ros-args",
+                "--log-level",
+                log_level,
+            ],
+            parameters=[{"use_sim_time": use_sim_time}],
+        ),
+        # ros_ign_bridge (target pose -> ROS 2)
+        #Node(
+        #    package="ros_ign_bridge",
+        #    executable="parameter_bridge",
+        #    output="log",
+        #    arguments=[
+        #        "/model/target/pose"
+        #        + "@"
+        #        + "geometry_msgs/msg/PoseStamped[ignition.msgs.Pose",
+        #        "--ros-args",
+        #        "--log-level",
+        #        log_level,
+        #    ],
+        #    parameters=[{"use_sim_time": use_sim_time}],
+        #    remappings=[("/model/target/pose", "/target_pose")],
+        #),
+    ]
+
+    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
+
+
+def generate_declared_arguments() -> List[DeclareLaunchArgument]:
+    """
+    Generate list of all launch arguments that are declared for this launch script.
+    """
+
+    return [
+        # World for Ignition Gazebo
+        DeclareLaunchArgument(
+            "world",
+            default_value=path.join(
+                get_package_share_directory("draw_svg"),
+                "worlds",
+                "follow_target.sdf",
+            ),
+            description="Name or filepath of world to load.",
+        ),
+        # Miscellaneous
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="If true, use simulated clock.",
+        ),
+        DeclareLaunchArgument(
+            "ign_verbosity",
+            default_value="2",
+            description="Verbosity level for Ignition Gazebo (0~4).",
+        ),
+        DeclareLaunchArgument(
+            "log_level",
+            default_value="warn",
+            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
+        ),
+    ]