From c86d1ae30ad3bb29c32a13978b8e071b017c4ea6 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 17 Jan 2022 20:31:21 +0400 Subject: [PATCH] :wrench: Add launch files for robossembler robot2 --- .../launch/rasmt_moveit.launch.py | 165 ++++++++++++++++++ rasmt_support/launch/rasmt_control.launch.py | 78 +++++++++ rasmt_support/launch/rasmt_gazebo.launch.py | 62 +++++++ .../launch/robossembler_bringup.launch.py | 92 ++++++++++ 4 files changed, 397 insertions(+) create mode 100644 rasmt_moveit_config/launch/rasmt_moveit.launch.py create mode 100644 rasmt_support/launch/rasmt_control.launch.py create mode 100644 rasmt_support/launch/rasmt_gazebo.launch.py create mode 100644 robossembler/launch/robossembler_bringup.launch.py diff --git a/rasmt_moveit_config/launch/rasmt_moveit.launch.py b/rasmt_moveit_config/launch/rasmt_moveit.launch.py new file mode 100644 index 0000000..226f83f --- /dev/null +++ b/rasmt_moveit_config/launch/rasmt_moveit.launch.py @@ -0,0 +1,165 @@ +import os +import yaml + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from ament_index_python import get_package_share_directory + + +def load_yaml(package_name: str, file_path: str): + 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 f: + return yaml.safe_load(f) + except EnvironmentError: + return None + + +def load_file(package_name: str, file_path: str) -> str: + package_path = get_package_share_directory(package_name) + absolut_file_path = os.path.join(package_path, file_path) + + try: + with open(absolut_file_path, "r") as f: + return f.read() + except EnvironmentError: + return None + +def generate_launch_description(): + + # Launch arguments + launch_args = [] + + launch_args.append( + DeclareLaunchArgument( + name="robot_description", + description="Robot description XML file." + ) + ) + + """launch_args.append( + DeclareLaunchArgument( + name="robot_name", + default_value="rasmt", + description="" + ) + )""" + + launch_args.append( + DeclareLaunchArgument( + name="sim", + default_value="true", + description="Launch robot in simulation or on real setup." + ) + ) + + # Evaluate frequently used variables + #robot_name = LaunchConfiguration("robot_name").perform() + + # Configure robot_description + robot_description = {"robot_description": LaunchConfiguration("robot_description")} + + # Robot semantics SRDF + robot_description_semantic = { + "robot_description_semantic": load_file("rasmt_moveit_config", "config/rasmt.srdf") + } + + # Kinematics + kinematics_yaml = load_yaml("rasmt_moveit_config", "config/kinematics.yaml") + + # Update group name + + # Joint limits + robot_description_planning = { + "robot_description_planning": PathJoinSubstitution( + [ + FindPackageShare("rasmt_moveit_config"), + "config/joint_limits.yaml" + ] + ) + } + + # Planning + ompl_yaml = load_yaml("rasmt_moveit_config", "config/ompl_planning.yaml") + + planning = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints", + "start_state_max_bounds_error": 0.1 + } + } + + # Trajectory execution + trajectory_execution = {"allow_trajectory_execution": True, + "moveit_manage_controllers": True} + + # Controllers + controllers_yaml = load_yaml( + "rasmt_moveit_config", + "config/rasmt_moveit_controllers.yaml" + ) + + moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"} + + # Planning scene + planning_scene_monitor_parameters = {"publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True} + + # Time configuration + use_sim_time = {"use_sime_time": LaunchConfiguration("sim")} + + # Prepare move group node + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + arguments=["--ros-args"], + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + robot_description_planning, + ompl_yaml, + planning, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + use_sim_time + ] + ) + + # RViz + rviz_config = PathJoinSubstitution([FindPackageShare("rasmt_moveit_config"), "config/rasmt_moveit.rviz"]) + + rviz = Node( + package="rviz2", + executable="rviz2", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + planning, + use_sim_time + ], + arguments=[ + '-d', rviz_config + ] + ) + + launch_nodes = [] + launch_nodes.append(rviz) + launch_nodes.append(move_group_node) + + + + return LaunchDescription(launch_args + launch_nodes) \ No newline at end of file diff --git a/rasmt_support/launch/rasmt_control.launch.py b/rasmt_support/launch/rasmt_control.launch.py new file mode 100644 index 0000000..776d412 --- /dev/null +++ b/rasmt_support/launch/rasmt_control.launch.py @@ -0,0 +1,78 @@ +from launch.launch_description import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import UnlessCondition +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + # Launch arguments + launch_args = [] + + launch_args.append( + DeclareLaunchArgument( + name="robot_description", + description="Robot description XML file." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="sim", + default_value="true", + description="Launch robot in simulation or on real setup." + ) + ) + + # Configure robot_description + robot_description = {"robot_description": LaunchConfiguration("robot_description")} + + # Load controllers from YAML configuration file + controller_configurations = PathJoinSubstitution([ + FindPackageShare("rasmt_support"), + "config", + "rasmt_ros2_controllers.yaml" + ]) + + # Prepare controller manager and other required nodes + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, controller_configurations], + output="screen", + condition=UnlessCondition(LaunchConfiguration("sim")) + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description] + ) + + joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner.py", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + controller_arm = Node( + package="controller_manager", + executable="spawner.py", + arguments=["rasmt_arm_controller", "--controller-manager", "/controller_manager"], + ) + controller_hand = Node( + package="controller_manager", + executable="spawner.py", + arguments=["rasmt_hand_controller", "--controller-manager", "/controller_manager"], + ) + + return LaunchDescription( + launch_args + [ + controller_manager, + robot_state_publisher, + joint_state_broadcaster, + controller_arm, + controller_hand + ]) \ No newline at end of file diff --git a/rasmt_support/launch/rasmt_gazebo.launch.py b/rasmt_support/launch/rasmt_gazebo.launch.py new file mode 100644 index 0000000..13da416 --- /dev/null +++ b/rasmt_support/launch/rasmt_gazebo.launch.py @@ -0,0 +1,62 @@ +from launch.launch_description import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, FindExecutable, Command +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +import xacro +import os +from ament_index_python import get_package_share_directory + + +def generate_launch_description(): + + launch_args = [] + launch_args.append(DeclareLaunchArgument( + name="robot_name", + default_value="rasmt", + description="Set robot name in gazebo env" + )) + + # Launch Gazebo + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("gazebo_ros"), + "launch", + "gazebo.launch.py" + ]) + ) + ) + + """xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro") + # get error if xacro file if missing + assert os.path.exists(xacro_file), "The xacro file of rasmt.xacro doesnt exist"+str(xacro_file)""" + + #sdf_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","cube5x.sdf") + + + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=[ + "-topic", "/robot_description", + "-entity", LaunchConfiguration("robot_name") + ], + output="screen" + ) + """cube_spawn = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=[ + "-file", sdf_file, + "-entity", "cube_station" + ] + )""" + + launch_nodes = [] + launch_nodes.append(gazebo) + launch_nodes.append(spawn_entity) + #launch_nodes.append(cube_spawn) + + return LaunchDescription(launch_args + launch_nodes) \ No newline at end of file diff --git a/robossembler/launch/robossembler_bringup.launch.py b/robossembler/launch/robossembler_bringup.launch.py new file mode 100644 index 0000000..cfc3280 --- /dev/null +++ b/robossembler/launch/robossembler_bringup.launch.py @@ -0,0 +1,92 @@ +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.launch_description import LaunchDescription +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions.launch_configuration import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python import get_package_share_directory +import xacro +import os + +def generate_launch_description(): + launch_args = [] + launch_args.append( + DeclareLaunchArgument( + name="grip", + default_value="true", + description="On or OFF gripper for launch" + ) + ) + launch_args.append( + DeclareLaunchArgument( + name="sim", + default_value="true", + description="On or OFF simulation" + ) + ) + launch_args.append( + DeclareLaunchArgument( + name="robot_name", + default_value="rasmt", + description="Robot name" + ) + ) + + # get xacro file path + xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro") + # get error if xacro file if missing + assert os.path.exists(xacro_file), "The xacro file of neptun_description doesnt exist"+str(xacro_file) + # parse xacro file from file with arguments + robot_description = xacro.process_file(xacro_file, mappings={'grip':"true",'sim':"false",'robot_name':"rasmt"}) + # convert file to xml format + robot_description_content = robot_description.toxml() + + control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("rasmt_support"), + "launch", + "rasmt_control.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content), + ("sim", LaunchConfiguration("sim")) + ] + ) + + simulation = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("rasmt_support"), + "launch", + "rasmt_gazebo.launch.py" + ]) + ), launch_arguments=[ + ("robot_name", LaunchConfiguration("robot_name")) + ], + condition=IfCondition(LaunchConfiguration("sim")) + ) + + moveit = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("rasmt_moveit_config"), + "launch", + "rasmt_moveit.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content), + ("sim",LaunchConfiguration("sim")) + ] + ) + + launch_nodes = [] + launch_nodes.append(control) + launch_nodes.append(simulation) + launch_nodes.append(moveit) + + + return LaunchDescription(launch_args + launch_nodes)