mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Some checks are pending
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:pedantic IMAGE:rolling-source]) (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressio… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:rolling-source]) (push) Waiting to run
Format / pre-commit (push) Waiting to run
Remapping node names in ROS2 enforces this name for all (sub)nodes created in a process: https://github.com/ros2/rclcpp/issues/843
106 lines
3.1 KiB
Python
106 lines
3.1 KiB
Python
import os
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import ExecuteProcess
|
|
from launch_ros.actions import Node
|
|
from moveit_configs_utils import MoveItConfigsBuilder
|
|
|
|
|
|
def generate_launch_description():
|
|
moveit_config = (
|
|
MoveItConfigsBuilder("moveit_resources_panda")
|
|
.planning_pipelines(pipelines=["ompl"])
|
|
.robot_description(file_path="config/panda.urdf.xacro")
|
|
.trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
|
|
.to_moveit_configs()
|
|
)
|
|
|
|
# Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation
|
|
move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}
|
|
|
|
# Start the actual move_group node/action server
|
|
run_move_group_node = Node(
|
|
package="moveit_ros_move_group",
|
|
executable="move_group",
|
|
output="screen",
|
|
parameters=[
|
|
moveit_config.to_dict(),
|
|
move_group_capabilities,
|
|
],
|
|
)
|
|
|
|
# RViz
|
|
rviz_config_file = (
|
|
get_package_share_directory("moveit_task_constructor_demo") + "/config/mtc.rviz"
|
|
)
|
|
rviz_node = Node(
|
|
package="rviz2",
|
|
executable="rviz2",
|
|
output="log",
|
|
arguments=["-d", rviz_config_file],
|
|
parameters=[
|
|
moveit_config.robot_description,
|
|
moveit_config.robot_description_semantic,
|
|
moveit_config.robot_description_kinematics,
|
|
moveit_config.planning_pipelines,
|
|
],
|
|
)
|
|
|
|
# Static TF
|
|
static_tf = Node(
|
|
package="tf2_ros",
|
|
executable="static_transform_publisher",
|
|
name="static_transform_publisher",
|
|
output="log",
|
|
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
|
|
)
|
|
|
|
# Publish TF
|
|
robot_state_publisher = Node(
|
|
package="robot_state_publisher",
|
|
executable="robot_state_publisher",
|
|
name="robot_state_publisher",
|
|
output="both",
|
|
parameters=[moveit_config.robot_description],
|
|
)
|
|
|
|
# ros2_control using FakeSystem as hardware
|
|
ros2_controllers_path = os.path.join(
|
|
get_package_share_directory("moveit_resources_panda_moveit_config"),
|
|
"config",
|
|
"ros2_controllers.yaml",
|
|
)
|
|
ros2_control_node = Node(
|
|
package="controller_manager",
|
|
executable="ros2_control_node",
|
|
parameters=[moveit_config.robot_description, ros2_controllers_path],
|
|
output="both",
|
|
)
|
|
|
|
# Load controllers
|
|
load_controllers = []
|
|
for controller in [
|
|
"panda_arm_controller",
|
|
"panda_hand_controller",
|
|
"joint_state_broadcaster",
|
|
]:
|
|
load_controllers += [
|
|
ExecuteProcess(
|
|
cmd=["ros2 run controller_manager spawner {}".format(controller)],
|
|
shell=True,
|
|
output="screen",
|
|
)
|
|
]
|
|
|
|
return LaunchDescription(
|
|
[
|
|
rviz_node,
|
|
static_tf,
|
|
robot_state_publisher,
|
|
run_move_group_node,
|
|
ros2_control_node,
|
|
]
|
|
+ load_controllers
|
|
)
|