Bill Finger
b42c2e633e
- Updated Sphere, Cylinder, Box classes to support random color generation by accepting an optional color parameter and implementing random color assignment if none is provided. - Improved documentation formatting in the RbsArm class for better readability and consistency. - Refactored scene management to integrate both Model and Mesh classes, allowing for more flexible object handling. This includes updating object instantiation and adding comments for future color randomization implementation. - Added `trimesh` as a dependency in the setup configuration to ensure proper functionality for mesh handling. - Cleaned up node initialization in the runtime launch setup for improved readability. - Enhanced the default scene configuration by including new ModelData and MeshData objects, with random color generation configured for MeshData. - Renamed `MeshObjectData` to `ModelData` for clarity and introduced a new `MeshData` class to specifically handle mesh properties. - Added a `random_color` attribute to `ObjectRandomizerData` and updated `ObjectData` to include a `color` attribute for better object customization. - Introduced new gripper data classes for better organization and added a `GripperEnum` to categorize different gripper types. - Implemented a new Mesh class for handling 3D mesh objects in Gazebo simulation, including methods for calculating inertia and generating SDF strings, with error handling for missing mesh files.
276 lines
9.6 KiB
Python
276 lines
9.6 KiB
Python
import os
|
|
|
|
import xacro
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import (
|
|
DeclareLaunchArgument,
|
|
IncludeLaunchDescription,
|
|
OpaqueFunction,
|
|
TimerAction,
|
|
)
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
|
|
def launch_setup(context, *args, **kwargs):
|
|
# Initialize Arguments
|
|
robot_type = LaunchConfiguration("robot_type")
|
|
# General arguments
|
|
with_gripper_condition = LaunchConfiguration("with_gripper")
|
|
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
|
|
description_package = LaunchConfiguration("description_package")
|
|
description_file = LaunchConfiguration("description_file")
|
|
robot_name = LaunchConfiguration("robot_name")
|
|
start_joint_controller = LaunchConfiguration("start_joint_controller")
|
|
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
|
|
launch_simulation = LaunchConfiguration("launch_sim")
|
|
launch_moveit = LaunchConfiguration("launch_moveit")
|
|
launch_task_planner = LaunchConfiguration("launch_task_planner")
|
|
launch_perception = LaunchConfiguration("launch_perception")
|
|
moveit_config_package = LaunchConfiguration("moveit_config_package")
|
|
moveit_config_file = LaunchConfiguration("moveit_config_file")
|
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
|
sim_gazebo = LaunchConfiguration("sim_gazebo")
|
|
hardware = LaunchConfiguration("hardware")
|
|
env_manager = LaunchConfiguration("env_manager")
|
|
gazebo_gui = LaunchConfiguration("gazebo_gui")
|
|
gripper_name = LaunchConfiguration("gripper_name")
|
|
|
|
initial_joint_controllers_file_path = os.path.join(
|
|
get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml"
|
|
)
|
|
|
|
xacro_file = os.path.join(
|
|
get_package_share_directory(description_package.perform(context)),
|
|
"urdf",
|
|
description_file.perform(context),
|
|
)
|
|
|
|
robot_description_doc = xacro.process_file(
|
|
xacro_file,
|
|
mappings={
|
|
"gripper_name": gripper_name.perform(context),
|
|
"hardware": hardware.perform(context),
|
|
"simulation_controllers": initial_joint_controllers_file_path,
|
|
"namespace": "",
|
|
},
|
|
)
|
|
|
|
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
|
|
robot_description = {"robot_description": robot_description_content}
|
|
|
|
single_robot_setup = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[
|
|
PathJoinSubstitution(
|
|
[FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"]
|
|
)
|
|
]
|
|
),
|
|
launch_arguments={
|
|
"env_manager": env_manager,
|
|
"with_gripper": with_gripper_condition,
|
|
"gripper_name": gripper_name,
|
|
"controllers_file": initial_joint_controllers_file_path,
|
|
"robot_type": robot_type,
|
|
# "controllers_file": controller_paramfile,
|
|
"cartesian_controllers": cartesian_controllers,
|
|
"description_package": description_package,
|
|
"description_file": description_file,
|
|
"robot_name": robot_type,
|
|
"start_joint_controller": start_joint_controller,
|
|
"initial_joint_controller": initial_joint_controller,
|
|
"launch_simulation": launch_simulation,
|
|
"launch_moveit": launch_moveit,
|
|
"launch_task_planner": launch_task_planner,
|
|
"launch_perception": launch_perception,
|
|
"moveit_config_package": moveit_config_package,
|
|
"moveit_config_file": moveit_config_file,
|
|
"use_sim_time": use_sim_time,
|
|
"sim_gazebo": sim_gazebo,
|
|
"hardware": hardware,
|
|
"launch_controllers": "true",
|
|
"gazebo_gui": gazebo_gui,
|
|
"robot_description": robot_description_content,
|
|
}.items(),
|
|
)
|
|
|
|
rbs_runtime = Node(
|
|
package="rbs_runtime",
|
|
executable="runtime",
|
|
parameters=[robot_description, {"use_sim_time": True}],
|
|
)
|
|
|
|
clock_bridge = Node(
|
|
package="ros_gz_bridge",
|
|
executable="parameter_bridge",
|
|
arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
|
|
output="screen",
|
|
)
|
|
|
|
delay_robot_control_stack = TimerAction(period=10.0, actions=[single_robot_setup])
|
|
|
|
nodes_to_start = [rbs_runtime, clock_bridge, delay_robot_control_stack]
|
|
return nodes_to_start
|
|
|
|
|
|
def generate_launch_description():
|
|
declared_arguments = []
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_type",
|
|
description="Type of robot by name",
|
|
choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
|
default_value="rbs_arm",
|
|
)
|
|
)
|
|
# General arguments
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"controllers_file",
|
|
default_value="rbs_arm0_controllers.yaml",
|
|
description="YAML file with the controllers configuration.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"description_package",
|
|
default_value="rbs_arm",
|
|
description="Description package with robot URDF/XACRO files. Usually the argument \
|
|
is not set, it enables use of a custom description.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"description_file",
|
|
default_value="rbs_arm_modular.xacro",
|
|
description="URDF/XACRO description file with the robot.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_name",
|
|
default_value="arm0",
|
|
description="Name for robot, used to apply namespace for specific robot in multirobot setup",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"start_joint_controller",
|
|
default_value="false",
|
|
description="Enable headless mode for robot control",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"initial_joint_controller",
|
|
default_value="joint_trajectory_controller",
|
|
description="Robot controller to start.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"moveit_config_package",
|
|
default_value="rbs_arm",
|
|
description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
|
|
is not set, it enables use of a custom moveit config.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"moveit_config_file",
|
|
default_value="rbs_arm.srdf.xacro",
|
|
description="MoveIt SRDF/XACRO description file with the robot.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_sim_time",
|
|
default_value="true",
|
|
description="Make MoveIt to use simulation time.\
|
|
This is needed for the trajectory planing in simulation.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"gripper_name",
|
|
default_value="rbs_gripper",
|
|
choices=["rbs_gripper", ""],
|
|
description="choose gripper by name (leave empty if hasn't)",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"with_gripper", default_value="true", description="With gripper or not?"
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"sim_gazebo", default_value="true", description="Gazebo Simulation"
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"env_manager", default_value="true", description="Launch env_manager?"
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"launch_sim",
|
|
default_value="true",
|
|
description="Launch simulator (Gazebo)?\
|
|
Most general arg",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"launch_moveit", default_value="false", description="Launch moveit?"
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"launch_perception", default_value="false", description="Launch perception?"
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"launch_task_planner",
|
|
default_value="false",
|
|
description="Launch task_planner?",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"cartesian_controllers",
|
|
default_value="true",
|
|
description="Load cartesian\
|
|
controllers?",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"hardware",
|
|
choices=["gazebo", "mock"],
|
|
default_value="gazebo",
|
|
description="Choose your harware_interface",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"launch_controllers",
|
|
default_value="true",
|
|
description="Launch controllers?",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"gazebo_gui", default_value="true", description="Launch gazebo with gui?"
|
|
)
|
|
)
|
|
|
|
return LaunchDescription(
|
|
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
|
)
|