1
$\begingroup$

Note: This is my first post here so please forgive me if I'm missing any good forum etiquette

I'm currently setting up a Gazebo (Fortress) simulation for a UR10e arm w/ a Robotiq 2f-85 Gripper. I've created my own moveit_config package for the UR10e & Gripper. Everything seems to be working okay, however I've noticed a few oddities that I can't seem to wrap my head around as to why they're happening:

Clip of what's described below: https://drive.google.com/file/d/1WLu9c-1NqJ-t_MM-giOjj7X37VxOI_n5/view?usp=sharing

  1. When I click "plan", I can't actually view the planned path of the arm (see video). I'm unsure if this is because it is happening too fast or because it's not happening at all. It's confusing as when I click execute the motion executes slowly as you would normally expect.

  2. Although I can see the interactive markers and interact with them, when I move them around the arm doesn't actually move, and the markers simply snap back into their original position after letting go of my cursor (see video).

I've tried looking around for a solution for a few days now but can't seem to find anything describing these specific problems I'm having.

For reference, the following is the errors / warnings from terminal output I get when running my program (I'm running everything from a launch file):

[rviz2-4] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.

[move_group-1] [ERROR] [1718906625.985203140] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates

[rviz2-4] [ERROR] [1718906629.195134199] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available

In addition, this is the launch file I wrote:

import xacro
from os.path import join
from launch import LaunchDescription
from launch.actions import (
    IncludeLaunchDescription,
    RegisterEventHandler,
    AppendEnvironmentVariable,
)
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    # MoveIt! Configuration
    moveit_config = (
        MoveItConfigsBuilder("ur10e")
        .robot_description(
            file_path="config/ur10e_robotiq.urdf.xacro",
        )
        .robot_description_semantic(file_path="config/ur10e_robotiq.srdf")
        .robot_description_kinematics(file_path="config/kinematics.yaml")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .trajectory_execution(file_path="config/moveit_controllers.yaml")
        .planning_pipelines(pipelines=["ompl"])
        .to_moveit_configs()
    )
    moveit_config_dict = moveit_config.to_dict()
    moveit_config_dict.update({"use_sim_time": True})

    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[moveit_config_dict],
        arguments=["--ros-args", "--log-level", "info"],
    )

    # Configure URDF file
    urdf_pkg_share = get_package_share_directory("ur10e_description")
    urdf_path_local = "urdf/ur10e_robotiq_2f_85.urdf.xacro"
    urdf_path_global = join(urdf_pkg_share, urdf_path_local)
    robot_description_raw = xacro.process_file(urdf_path_global).toxml()

    world_path = (
        "/home/willnatsan/ros2/ur_copy_ws/src/ur10e/ur10e_description/worlds/A23.sdf"
    )

    # Launch Gazebo
    gz_pkg_share = get_package_share_directory("ros_gz_sim")

    set_env_vars_resources = AppendEnvironmentVariable(
        "GZ_SIM_RESOURCE_PATH",
        join("/home/willnatsan/ros2/ur_copy_ws/src/ur10e/ur10e_description/worlds/"),
    )

    gzserver_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(join(gz_pkg_share, "launch", "gz_sim.launch.py")),
        launch_arguments={
            "gz_args": ["-s -v4 ", world_path],
            "on_exit_shutdown": "true",
        }.items(),
    )

    gzclient_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(join(gz_pkg_share, "launch", "gz_sim.launch.py")),
        launch_arguments={"gz_args": "-g -v4 "}.items(),
    )

    spawn_x = "1.655"
    spawn_y = "1.883"
    spawn_z = "0.85"
    spawn_yaw = "1.57"

    # Configure nodes to launch
    static_tf_node = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=[
            spawn_x,
            spawn_y,
            spawn_z,
            spawn_yaw,
            "0.0",
            "0.0",
            "world",
            "ur10e_base_link",
        ],
    )

    rsp_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description_raw, "use_sim_time": True}],
        output="both",
    )

    rviz_config_file = join(urdf_pkg_share, "rviz", "view_robot.rviz")

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        output="log",
        name="rviz2",
        arguments=["-d", rviz_config_file],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
            {"use_sim_time": True},
        ],
    )

    robot_name = "ur10e"

    create_node = Node(
        package="ros_gz_sim",
        executable="create",
        arguments=[
            "-topic",
            "robot_description",
            "-name",
            robot_name,
            # '-world', world_path,
            "-x",
            spawn_x,
            "-y",
            spawn_y,
            "-z",
            spawn_z,
            "-Y",
            spawn_yaw,
        ],
        parameters=[{"use_sim_time": True}],
        output="screen",
    )

    # Spawn controller nodes

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

    joint_trajectory_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_trajectory_controller",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    robotiq_gripper_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "robotiq_gripper_controller",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    delay_joint_state_broadcaster_spawner = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=create_node,
            on_exit=[joint_state_broadcaster_spawner],
        )
    )

    delay_joint_trajectory_controller_spawner = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=joint_state_broadcaster_spawner,
            on_exit=[
                joint_trajectory_controller_spawner,
                robotiq_gripper_controller_spawner,
            ],
        )
    )

    return LaunchDescription(
        [
            move_group_node,
            static_tf_node,
            rsp_node,
            rviz_node,
            set_env_vars_resources,
            gzserver_cmd,
            gzclient_cmd,
            create_node,
            delay_joint_state_broadcaster_spawner,
            delay_joint_trajectory_controller_spawner,
        ]
    )

I've been struggling with this for what feels like forever now so any help or guidance would be greatly appreciated. Please let me know if any further information or clarification would be helpful. Thank you in advance!

Improve this question
$\endgroup$

1 Answer 1

Reset to default
1
$\begingroup$

I managed to figure out the issue myself after some more trial and error. Posting the solution here in case anyone else might run into the same issue.

It's honestly a really simple fix: Only add the "use_sim_time" parameter in moveit_config. Adding the parameter in the other nodes causes the interactive marker to not work as expected.

Hope that helps anyone who might stumble across this!

Improve this answer
$\endgroup$
2
  • $\begingroup$ This is not a good solution to whatever problem your setup ran into - using different ros clocks for different ros nodes is going to create other problems for you in the future. Perhaps you've found a bug - note that the system time-of-day clock is available if the developer really needed it during a simulation (but this seems highly unlikely to me.) $\endgroup$
    – Mike973
    Commented Jul 6 at 13:10
  • $\begingroup$ Hmm yeah that's a good point. I did find it odd because a lot of the examples I found for simulating w/ Gazebo pointed out the same thing and had "use_sim_time" for each node. When I do that though I just get the behaviour I noticed in my original question. If you have other ideas about what else may be causing the problem that'd be really appreciated. $\endgroup$
    – willnatsan
    Commented Jul 8 at 1:09

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.