Add Gazebo to Moveit planning

asked 2023-03-17 14:51:27 -0500

CB2405 gravatar image

Hello there,

i got this code to launch my Moveit planning - now i want to add a simulaneous gazebo simulation of the robot. How can i do that?

Here is my Code of the Moveit launch

import os from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.actions import ExecuteProcess from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():

declared_arguments = []
declared_arguments.append(
    DeclareLaunchArgument(
        "rviz_config",
        default_value="moveit.rviz",
        description="RViz configuration file",
    )
)

return LaunchDescription(
    declared_arguments + [OpaqueFunction(function=launch_setup)]
)

def launch_setup(context, args, *kwargs):

moveit_config = (
    MoveItConfigsBuilder("kr10zelle")
    .robot_description(file_path="config/KR_10.urdf.xacro")
    #.trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
    .planning_scene_monitor(
        publish_robot_description=True, publish_robot_description_semantic=True
    )
    .planning_pipelines(
        pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"]
    )
    .to_moveit_configs()
)

# 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()],
)

rviz_base = LaunchConfiguration("rviz_config")
rviz_config = PathJoinSubstitution(
    [FindPackageShare("kr10zelle_moveit_config"), "config", rviz_base]
)

# RViz
rviz_node = Node(
    package="rviz2",
    executable="rviz2",
    name="rviz2",
    output="log",
    arguments=["-d", rviz_config],
    parameters=[
        moveit_config.robot_description,
        moveit_config.robot_description_semantic,
        moveit_config.robot_description_kinematics,
        moveit_config.planning_pipelines,
        moveit_config.joint_limits,
    ],
)

# 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", "base_link"],
)

# 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("kr10zelle_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",
)

joint_state_broadcaster_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=[
        "joint_state_broadcaster",
        "--controller-manager-timeout",
        "300",
        "--controller-manager",
        "/controller_manager",
    ],
)

arm_controller_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["kr10_arm_controller", "-c", "/controller_manager"],
)

nodes_to_start = [
    rviz_node,
    static_tf,
    robot_state_publisher,
    run_move_group_node,
    ros2_control_node,
    joint_state_broadcaster_spawner,
    arm_controller_spawner,
]

return nodes_to_start
edit retag flag offensive close merge delete

Comments

Have you found a solution for this? I'm trying to create a similar simulation

Icon45 gravatar image Icon45  ( 2023-07-07 01:14:14 -0500 )edit