🔧 Add launch files for robossembler robot2
This commit is contained in:
parent
6fa0c2d42c
commit
c86d1ae30a
4 changed files with 397 additions and 0 deletions
165
rasmt_moveit_config/launch/rasmt_moveit.launch.py
Normal file
165
rasmt_moveit_config/launch/rasmt_moveit.launch.py
Normal file
|
|
@ -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)
|
||||
78
rasmt_support/launch/rasmt_control.launch.py
Normal file
78
rasmt_support/launch/rasmt_control.launch.py
Normal file
|
|
@ -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
|
||||
])
|
||||
62
rasmt_support/launch/rasmt_gazebo.launch.py
Normal file
62
rasmt_support/launch/rasmt_gazebo.launch.py
Normal file
|
|
@ -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)
|
||||
92
robossembler/launch/robossembler_bringup.launch.py
Normal file
92
robossembler/launch/robossembler_bringup.launch.py
Normal file
|
|
@ -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)
|
||||
Loading…
Add table
Add a link
Reference in a new issue