initial commit

launch/control.launch.py

@@ -0,0 +1,87 @@
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+
+# from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration
+
+
+def create_spawner(controller_name, namespace, condition=None, inactive=False):
+    args = [controller_name, "-c", f"{namespace}/controller_manager"]
+    if inactive:
+        args.append("--inactive")
+    return Node(
+        package="controller_manager",
+        executable="spawner",
+        namespace=namespace,
+        arguments=args,
+        condition=condition,
+    )
+
+
+def launch_setup(context, *args, **kwargs):
+    namespace = LaunchConfiguration("namespace").perform(context)
+    control_strategy = LaunchConfiguration("control_strategy").perform(context)
+    control_space = LaunchConfiguration("control_space").perform(context)
+    has_gripper = LaunchConfiguration("has_gripper").perform(context)
+    interactive_control = LaunchConfiguration("interactive_control").perform(context)
+
+    controllers_config = {
+        "joint_state_broadcaster": True,
+        "gripper_controller": has_gripper == "true",
+        "joint_trajectory_controller": control_strategy == "position"
+        and control_space == "joint",
+        "cartesian_motion_controller": control_strategy == "position"
+        and control_space == "task",
+        "cartesian_force_controller": control_strategy == "effort"
+        and control_space == "task",
+        "joint_effort_controller": control_strategy == "effort"
+        and control_space == "joint",
+        "force_torque_sensor_broadcaster": control_strategy == "effort"
+        and control_space == "task",
+        "motion_control_handle": interactive_control == "true"
+        and control_space == "task"
+        and control_strategy == "position",
+    }
+
+    nodes_to_start = []
+    for controller_name, should_start in controllers_config.items():
+        if should_start:
+            nodes_to_start.append(create_spawner(controller_name, namespace))
+
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = [
+        DeclareLaunchArgument(
+            "namespace", default_value="", description="A robot's namespace"
+        ),
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Control type: 'task' for Cartesian, 'joint' for joint space control.",
+        ),
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Control strategy: 'position', 'velocity', or 'effort'.",
+        ),
+        DeclareLaunchArgument(
+            "has_gripper",
+            default_value="true",
+            description="Whether to activate the gripper_controller.",
+        ),
+
+        DeclareLaunchArgument(
+            "interactive_control",
+            default_value="true",
+            description="Whether to activate the gripper_controller.",
+        ),
+    ]
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

launch/view_auboi5.launch.py

@@ -0,0 +1,104 @@
+
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    pkg_share = FindPackageShare("aubo_ros2_description")
+    default_rviz_config_path = PathJoinSubstitution(
+        [pkg_share, "rviz", "view_rizon.rviz"]
+    )
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("aubo_ros2_description"), "urdf", "aubo_i5.urdf.xacro"]
+            ),
+            " ",
+            "name:=",
+            "aubo_i5",
+        ]
+    )
+
+    # Robot state publisher
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{"robot_description": robot_description_content}],
+    )
+
+    # package_path = get_package_share_directory("aubo_ros2_description")
+    # joint_state_publisher_node = Node(
+    #     package="joint_state_publisher",
+    #     executable="joint_state_publisher",
+    #     name="joint_state_publisher",
+    #     parameters=[package_path + "/config/initial_positions.yaml"],
+    #     # condition=UnlessCondition(LaunchConfiguration("gui")),
+    # )
+
+    # joint_state_publisher_gui_node = Node(
+    #     package="joint_state_publisher_gui",
+    #     executable="joint_state_publisher_gui",
+    #     name="joint_state_publisher_gui",
+    #     condition=IfCondition(LaunchConfiguration("gui")),
+    # )
+
+    # RViz
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
+        arguments=["-d", LaunchConfiguration("rvizconfig")],
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                name="aubo_type",
+                default_value="aubo_i5",
+                description="Type of the aubo robot.",
+                choices=["aubo_i3", "aubo_i5", "aubo_i10"],
+            ),
+            DeclareLaunchArgument(
+                name="gui",
+                default_value="False",
+                description="Flag to enable joint_state_publisher_gui",
+            ),
+            DeclareLaunchArgument(
+                name="rvizconfig",
+                default_value=default_rviz_config_path,
+                description="Absolute path to rviz config file",
+            ),
+            robot_state_publisher_node,
+            # joint_state_publisher_node,
+            # joint_state_publisher_gui_node,
+            rviz_node,
+        ]
+    )