Import example code and basic prototype

2022-10-18 17:20:50 +03:00
parent ed00da86c3
commit ae44ce87e9
33 changed files with 3882 additions and 0 deletions
--- a/launch/ex_py_follow_target.launch.py
+++ b/launch/ex_py_follow_target.launch.py
@@ -0,0 +1,105 @@
+#!/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("ign_moveit2_examples"),
+                        "launch",
+                        "default.launch.py",
+                    ]
+                )
+            ),
+            launch_arguments=[
+                ("world_type", "follow_target"),
+                ("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="ign_moveit2_examples",
+            executable="ex_follow_target.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.",
+        ),
+    ]