0
$\begingroup$

ROS2 Humble, Gazebo Classic.

I build a 6-axis robot with moveit setup assistent and try to load demo.launch with gazebo. And I have error:

[ros2_control_node-5] [INFO] [1693236183.459465629] [resource_manager]: Loading hardware 'GazeboSystem' [ros2_control_node-5] terminate called after throwing an instance of 'pluginlib::LibraryLoadException' [ros2_control_node-5] what(): According to the loaded plugin descriptions the class gazebo_ros2_control/GazeboSystem with base class type hardware_interface::SystemInterface does not exist. Declared types are fake_components/GenericSystem mock_components/GenericSystem test_hardware_components/TestSystemCommandModes test_hardware_components/TestTwoJointSystem test_system

This is fanuc.urdf.xacro:

<?xml version="1.0"?> <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fanuc">
    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />

    <!-- Import fanuc urdf file -->
    <xacro:include filename="$(find bricky_description)/urdf/fanuc_m710ic45m/m710ic45m.urdf" />

    <!-- Import control_xacro -->
    <xacro:include filename="fanuc.ros2_control.xacro" />


    <xacro:fanuc_ros2_control name="GazeboSystem" initial_positions_file="$(arg initial_positions_file)"/>

    <gazebo>
        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
            <parameters>$(find bricky_main)/config/ros2_controllers.yaml</parameters>
        </plugin>
    </gazebo>

</robot>

fanuc.ros2_control.xacro:

<?xml version="1.0"?> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="fanuc_ros2_control" params="name initial_positions_file">
        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>

        <ros2_control name="${name}" type="system">
            <hardware>
                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
            </hardware>
            <joint name="joint_1">
                <command_interface name="position"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
            <joint name="joint_2">
                <command_interface name="position"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
            <joint name="joint_3">
                <command_interface name="position"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
            <joint name="joint_4">
                <command_interface name="position"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
            <joint name="joint_5">
                <command_interface name="position"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
            <joint name="joint_6">
                <command_interface name="position"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
        </ros2_control>
    </xacro:macro> </robot>

ros2_controllers.yaml:

controller_manager:   ros__parameters:
    update_rate: 50  # Hz

    robot_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

robot_arm_controller:   ros__parameters:
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    interface_name: position
    command_interfaces:
      - position
      # - velocity
    state_interfaces:
      - position
      - velocity

It works good without gazebo hardware plugin (only Rviz and Moveit), but I want to have gazebo simulation.

Does gazebo_ros2_control/GazeboSystem works with ROS Humble?

Please help me with any ideas what can I do to fix this issue.

Also I saw some weird twitches of robot in Gazebo if I run Gazebo first, then spawn the robot in Gazebo and only then launch launch.demo.py But it does not work anyway and error still remains.

I tried to load controller manually:

ros2 control load_controller --set-state active joint_state_controller

but have no success:

Error loading controller, check controller_manager logs

My full log here:

@Kirill:~$ ros2 launch bricky_main demo.launch.py use_sim_time:=true
[INFO] [launch]: All log files can be found below /home/kirill/.ros/log/2023-08-28-18-48-01-425483-Kirill-70089
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [static_transform_publisher-1]: process started with pid [70090]
[INFO] [robot_state_publisher-2]: process started with pid [70092]
[INFO] [move_group-3]: process started with pid [70094]
[INFO] [rviz2-4]: process started with pid [70096]
[INFO] [ros2_control_node-5]: process started with pid [70098]
[INFO] [spawner-6]: process started with pid [70100]
[INFO] [spawner-7]: process started with pid [70102]
[static_transform_publisher-1] [INFO] [1693237681.971491279] [static_transform_publisher0]: Spinning until stopped - publishing transform
[static_transform_publisher-1] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-1] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-1] from 'world' to 'base_link'
[ros2_control_node-5] [INFO] [1693237681.978944484] [resource_manager]: Loading hardware 'GazeboSystem' 
[ros2_control_node-5] terminate called after throwing an instance of 'pluginlib::LibraryLoadException'
[ros2_control_node-5]   what():  According to the loaded plugin descriptions the class gazebo_ros2_control/GazeboSystem with base class type hardware_interface::SystemInterface does not exist. Declared types are  fake_components/GenericSystem mock_components/GenericSystem test_hardware_components/TestSystemCommandModes test_hardware_components/TestTwoJointSystem test_system
[ros2_control_node-5] Stack trace (most recent call last):
[ros2_control_node-5] #16   Object "", at 0xffffffffffffffff, in 
[ros2_control_node-5] #15   Object "/opt/ros/humble/lib/controller_manager/ros2_control_node", at 0x564698431d84, in 
[robot_state_publisher-2] [INFO] [1693237682.002540791] [robot_state_publisher]: got segment base_link
[robot_state_publisher-2] [INFO] [1693237682.002636195] [robot_state_publisher]: got segment dummy_gripper
[robot_state_publisher-2] [INFO] [1693237682.002650024] [robot_state_publisher]: got segment link_1
[robot_state_publisher-2] [INFO] [1693237682.002660431] [robot_state_publisher]: got segment link_2
[robot_state_publisher-2] [INFO] [1693237682.002671117] [robot_state_publisher]: got segment link_3
[robot_state_publisher-2] [INFO] [1693237682.002681453] [robot_state_publisher]: got segment link_4
[robot_state_publisher-2] [INFO] [1693237682.002691929] [robot_state_publisher]: got segment link_5
[robot_state_publisher-2] [INFO] [1693237682.002702057] [robot_state_publisher]: got segment link_6
[robot_state_publisher-2] [INFO] [1693237682.002712533] [robot_state_publisher]: got segment world
[ros2_control_node-5] #14   Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7f96a6b53e3f]
[ros2_control_node-5] #13   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __libc_start_call_main [0x7f96a6b53d8f]
[ros2_control_node-5] #12   Object "/opt/ros/humble/lib/controller_manager/ros2_control_node", at 0x56469843189e, in 
[ros2_control_node-5] #11   Object "/opt/ros/humble/lib/libcontroller_manager.so", at 0x7f96a720a291, in controller_manager::ControllerManager::ControllerManager(std::shared_ptr<rclcpp::Executor>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rclcpp::NodeOptions const&)
[ros2_control_node-5] #10   Object "/opt/ros/humble/lib/libcontroller_manager.so", at 0x7f96a7209377, in controller_manager::ControllerManager::init_resource_manager(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[ros2_control_node-5] #9    Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7f96a6a58d7f, in hardware_interface::ResourceManager::load_urdf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)
[ros2_control_node-5] #8    Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7f96a6a5740e, in 
[ros2_control_node-5] #7    Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7f96a6a366b0, in 
[ros2_control_node-5] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f96a6e204d7, in __cxa_throw
[ros2_control_node-5] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f96a6e20276, in std::terminate()
[ros2_control_node-5] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f96a6e2020b, in 
[ros2_control_node-5] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f96a6e14b9d, in 
[ros2_control_node-5] #2    Source "./stdlib/abort.c", line 79, in abort [0x7f96a6b527f2]
[ros2_control_node-5] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7f96a6b6c475]
[ros2_control_node-5] #0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
[ros2_control_node-5]     | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
[ros2_control_node-5]       Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7f96a6bc0a7c]
[ros2_control_node-5] Aborted (Signal sent by tkill() 70098 1000)
[move_group-3] [INFO] [1693237682.024140801] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0369452 seconds
[move_group-3] [INFO] [1693237682.024210014] [moveit_robot_model.robot_model]: Loading robot model 'fanuc'...
[move_group-3] [WARN] [1693237682.027610488] [moveit_robot_model.robot_model]: Could not identify parent group for end-effector 'robot_end_effector'
[move_group-3] [INFO] [1693237682.045338380] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-3] [INFO] [1693237682.045513334] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-3] [INFO] [1693237682.045857377] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-3] [INFO] [1693237682.046263578] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-3] [INFO] [1693237682.046286557] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-3] [INFO] [1693237682.046953409] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-3] [INFO] [1693237682.046975410] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-3] [INFO] [1693237682.047251426] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-3] [INFO] [1693237682.047570115] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-3] [WARN] [1693237682.047706587] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-3] [ERROR] [1693237682.047720765] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[move_group-3] [INFO] [1693237682.048820290] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'ompl'
[move_group-3] [INFO] [1693237682.057350320] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-3] [INFO] [1693237682.058797938] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[move_group-3] [INFO] [1693237682.058820707] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[move_group-3] [INFO] [1693237682.058827412] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[move_group-3] [INFO] [1693237682.058846129] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-3] [INFO] [1693237682.058862822] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[move_group-3] [INFO] [1693237682.058868968] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-3] [INFO] [1693237682.058880422] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-3] [INFO] [1693237682.058886987] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was not set. Using default value: 0.020000
[move_group-3] [INFO] [1693237682.058893343] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[move_group-3] [INFO] [1693237682.058904238] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-3] [INFO] [1693237682.058909406] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-3] [INFO] [1693237682.058914714] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-3] [INFO] [1693237682.058920930] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-3] [INFO] [1693237682.058925749] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-3] [INFO] [1693237682.058930568] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-3] [INFO] [1693237682.060190241] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'chomp'
[move_group-3] [ERROR] [1693237682.060924282] [moveit.ros_planning.planning_pipeline]: Exception while loading planner 'chomp_interface/CHOMPPlanner': According to the loaded plugin descriptions the class chomp_interface/CHOMPPlanner with base class type planning_interface::PlannerManager does not exist. Declared types are  ompl_interface/OMPLPlanner pilz_industrial_motion_planner/CommandPlannerAvailable plugins: ompl_interface/OMPLPlanner, pilz_industrial_motion_planner/CommandPlanner
[move_group-3] [INFO] [1693237682.061558309] [moveit_ros.fix_workspace_bounds]: Param 'chomp.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-3] [INFO] [1693237682.061584081] [moveit_ros.fix_start_state_bounds]: Param 'chomp.start_state_max_bounds_error' was set to 0.100000
[move_group-3] [INFO] [1693237682.061591624] [moveit_ros.fix_start_state_bounds]: Param 'chomp.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-3] [INFO] [1693237682.061602519] [moveit_ros.fix_start_state_collision]: Param 'chomp.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-3] [INFO] [1693237682.061609224] [moveit_ros.fix_start_state_collision]: Param 'chomp.jiggle_fraction' was set to 0.050000
[move_group-3] [INFO] [1693237682.061615580] [moveit_ros.fix_start_state_collision]: Param 'chomp.max_sampling_attempts' was not set. Using default value: 100
[move_group-3] [INFO] [1693237682.061625916] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Parameterization'
[move_group-3] [INFO] [1693237682.061631574] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-3] [INFO] [1693237682.061636253] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-3] [INFO] [1693237682.061641002] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-3] [INFO] [1693237682.061645821] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-3] [INFO] [1693237682.061650640] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-3] [ERROR] [1693237682.062490283] [moveit.ros_planning_interface.moveit_cpp]: Failed to initialize planning pipeline 'chomp'.
[move_group-3] [INFO] [1693237682.062846059] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'pilz_industrial_motion_planner'
[move_group-3] [INFO] [1693237682.064334185] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-3] [INFO] [1693237682.067023946] [moveit.pilz_industrial_motion_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP 
[move_group-3] [INFO] [1693237682.067047692] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[move_group-3] [INFO] [1693237682.067860515] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [CIRC]
[move_group-3] [INFO] [1693237682.067878045] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[move_group-3] [INFO] [1693237682.068428401] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [LIN]
[move_group-3] [INFO] [1693237682.068444535] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[move_group-3] [INFO] [1693237682.068941322] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [PTP]
[move_group-3] [INFO] [1693237682.068958364] [moveit.ros_planning.planning_pipeline]: Using planning interface 'Pilz Industrial Motion Planner'
[move_group-3] [INFO] [1693237682.083453472] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for robot_arm_controller
[move_group-3] [INFO] [1693237682.083595670] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-3] [INFO] [1693237682.083614807] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-3] [INFO] [1693237682.083832086] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-3] [INFO] [1693237682.083849965] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-3] [INFO] [1693237682.098606772] [move_group.move_group]: 
[move_group-3] 
[move_group-3] ********************************************************
[move_group-3] * MoveGroup using: 
[move_group-3] *     - ApplyPlanningSceneService
[move_group-3] *     - ClearOctomapService
[move_group-3] *     - CartesianPathService
[move_group-3] *     - ExecuteTrajectoryAction
[move_group-3] *     - GetPlanningSceneService
[move_group-3] *     - KinematicsService
[move_group-3] *     - MoveAction
[move_group-3] *     - MotionPlanService
[move_group-3] *     - QueryPlannersService
[move_group-3] *     - StateValidationService
[move_group-3] ********************************************************
[move_group-3] 
[move_group-3] [INFO] [1693237682.098669001] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-3] [INFO] [1693237682.098681433] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-3] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-3] Loading 'move_group/ClearOctomapService'...
[move_group-3] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-3] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-3] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-3] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-3] Loading 'move_group/MoveGroupMoveAction'...
[move_group-3] Loading 'move_group/MoveGroupPlanService'...
[move_group-3] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-3] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-3] 
[move_group-3] You can start planning now!
[move_group-3] 
[rviz2-4] [INFO] [1693237682.243505541] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-4] [INFO] [1693237682.243605554] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-4] [INFO] [1693237682.257965239] [rviz2]: Stereo is NOT SUPPORTED
[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.
[rviz2-4]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[ERROR] [ros2_control_node-5]: process has died [pid 70098, exit code -6, cmd '/opt/ros/humble/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_8covfil6 --params-file /home/kirill/bricky_ros2/install/bricky_main/share/bricky_main/config/ros2_controllers.yaml'].
[spawner-6] [INFO] [1693237684.203719059] [spawner_robot_arm_controller]: Waiting for '/controller_manager' node to exist
[spawner-7] [INFO] [1693237684.208766411] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist
[rviz2-4] [ERROR] [1693237685.334018168] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-4] [INFO] [1693237685.350041073] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-4] [INFO] [1693237685.838428945] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.438378 seconds
[rviz2-4] [INFO] [1693237685.838470431] [moveit_robot_model.robot_model]: Loading robot model 'fanuc'...
[rviz2-4] [WARN] [1693237685.841740091] [moveit_robot_model.robot_model]: Could not identify parent group for end-effector 'robot_end_effector'
[rviz2-4] [INFO] [1693237685.870563035] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-4] [INFO] [1693237685.872585873] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-4] [INFO] [1693237685.959010565] [interactive_marker_display_94578899224848]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-4] [INFO] [1693237685.965076006] [moveit_ros_visualization.motion_planning_frame]: group robot_arm
[rviz2-4] [INFO] [1693237685.965099403] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'robot_arm' in namespace ''
[rviz2-4] [INFO] [1693237685.970341405] [interactive_marker_display_94578899224848]: Sending request for interactive markers
[rviz2-4] [INFO] [1693237685.973040105] [move_group_interface]: Ready to take commands for planning group robot_arm.
[rviz2-4] [INFO] [1693237685.973897208] [moveit_ros_visualization.motion_planning_frame]: group robot_arm
[rviz2-4] [INFO] [1693237686.002323658] [interactive_marker_display_94578899224848]: Service response received for initialization
[spawner-6] [INFO] [1693237686.216117789] [spawner_robot_arm_controller]: Waiting for '/controller_manager' node to exist
[spawner-7] [INFO] [1693237686.220691262] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist
[spawner-6] [INFO] [1693237688.227489630] [spawner_robot_arm_controller]: Waiting for '/controller_manager' node to exist
[spawner-7] [INFO] [1693237688.232700901] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist
[spawner-6] [INFO] [1693237690.238744345] [spawner_robot_arm_controller]: Waiting for '/controller_manager' node to exist
[spawner-7] [INFO] [1693237690.244827316] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist
[spawner-6] [ERROR] [1693237692.249384240] [spawner_robot_arm_controller]: Controller manager not available
[spawner-7] [ERROR] [1693237692.256139022] [spawner_joint_state_broadcaster]: Controller manager not available
[ERROR] [spawner-6]: process has died [pid 70100, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner robot_arm_controller --ros-args'].
[ERROR] [spawner-7]: process has died [pid 70102, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_state_broadcaster --ros-args'].
$\endgroup$

2 Answers 2

0
$\begingroup$

Have you installed gazebo_ros2_control? Pluginlib from ros2_control_node can't find it. Do the provided demos work?

$\endgroup$
5
  • $\begingroup$ I have installed gazebo_ros2_control. But I don't have demo package, will try it later. Also I run my project on ROS FOXY and it works!, but have the same error in logs. So I still need to find solution for my problem... $\endgroup$ Aug 29, 2023 at 12:23
  • $\begingroup$ it works with foxy but throws the same error? We introduced catching pluginlib errors with humble recently, I don't think this was backported to foxy. $\endgroup$ Sep 1, 2023 at 8:51
  • $\begingroup$ Try to run example_9 from the ros2_control_demos or any example from gazebo_ros2_control_demos. you could also try to run your project from inside the docker we provided with the ros2_control_demos. $\endgroup$ Sep 1, 2023 at 8:53
  • $\begingroup$ Hey, I am also facing the above issue with ros2 humble. I ran the examples you provided, but it seems gazebo_ros2_control is not working. I then removed it and used only ros2_control_node and everything is working now at least in terminals. The robot in gazebo is oscillating and not standing still. I am using the rrbot_transmissions_system_position_only.cpp (as hardware interface from ros2_control_demo_example_8). I really need some help here. $\endgroup$ Jan 27 at 13:28
  • $\begingroup$ Please help me with this - robotics.stackexchange.com/questions/107890/… $\endgroup$ Jan 27 at 15:54
-1
$\begingroup$

Thanks for help and your answers. I almost solve the problems.

  1. In my launch file I run ros2_control_node - it was my firs mistake. I assume it was in conflict with gazebo_ros2_control, which I guess run automatically based on hardware plugin in ros2 control tag in xacro file. I'm not sure, but looks like gazebo_ros2_control can control controllers instead of ros2_control_node when I work with Gazebo. So that's why in Foxy my project worked with Gazebo even with same error in logs. This error was related to ros2_control_node which is not necessary.

  2. In Humble I add parameter {"use_sim_time": True} in 3 nodes: robot_state_publisher, rviz_node_full and run_move_group_node. I'm still not sure all of this nodes required this parameter, but it works.

  3. Last problem is about controller_manager spawner executeble. Based on my code I assume controllers will be starts at the end, but based on my logs above you can see that controller_manager error appears almost at launch start. For now after the full launch completed, I start controllers manually in another terminal. I found 2 various solutions for change loading sequence and will try it.

Now I can move robot in rviz, plan, execute and it moves in Gazebo.

Hope my experience will help to others. My launch file below:

# Import libraries:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler, DeclareLaunchArgument,Shutdown
from launch.conditions import IfCondition, UnlessCondition
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
import xacro
import yaml

def load_file(package_name, file_path):
    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 file:
            return file.read()
    except EnvironmentError:

        return None

def load_yaml(package_name, file_path):
    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 file:
            return yaml.safe_load(file)
    except EnvironmentError:
        return None


def generate_launch_description():

    # *********************** Gazebo *********************** # 
    

    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py'])
             )

    # *********************** MoveIt *********************** # 
    

    rviz_arg = DeclareLaunchArgument("rviz_file", default_value="False", description="Load RVIZ file.")


    robot_description_config = xacro.process_file(
        os.path.join(
            get_package_share_directory("bricky_main"),
            "config",
            "fanuc.urdf.xacro",
        )
    )
    

    robot_description = {"robot_description": robot_description_config.toxml()}


    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-topic', 'robot_description',
                                   '-entity', 'fanuc',"-x", "0.0", "-y", "0.0", "-z", "0.0"],
                        output='screen')


    robot_description_semantic_config = load_file(
        "bricky_main", "config/fanuc.srdf"
    )


    robot_description_semantic = {
        "robot_description_semantic":robot_description_semantic_config
    }


    kinematics_yaml = load_yaml(
        "bricky_main", "config/kinematics.yaml"
    )


    robot_description_kinematics = {"robot_description_kinematics":kinematics_yaml}


    ompl_planning_pipeline_config = {
        "move_group":{
        "planning_plugin": "ompl_interface/OMPLPlanner",
        "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization 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,
        }
    }


    ompl_planning_yaml = load_yaml(
        "bricky_main","config/ompl_planning.yaml"
    )


    ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)


    moveit_simple_controllers_yaml = load_yaml(
        "bricky_main", "config/moveit_controllers.yaml"
    )


    moveit_controllers = {
        "moveit_simple_controller_manager": moveit_simple_controllers_yaml,
        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
    }

    trajectory_execution = {
        "moveit_manage_controllers": True,
        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
        "trajectory_execution.allowed_goal_duration_margin": 0.5,
        "trajectory_execution.allowed_start_tolerance": 0.01,
        "trajectory_execution.allowed_goal_tolerance": 0.01,
    }

    planning_scene_monitor_parameters = {
        "publish_planning_scene": True,
        "publish_geometry_updates": True,
        "publish_state_updates":  True,
        "publish_transforms_updates": True,
    }


    run_move_group_node = Node(
        package = "moveit_ros_move_group",
        executable = "move_group",
        output = "screen",
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_kinematics,
            ompl_planning_pipeline_config,
            trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
            {"use_sim_time": True},
        ],
    )


    load_rviz = LaunchConfiguration("rviz_file")
    rviz_base = os.path.join(get_package_share_directory("bricky_main"),"config")
    rviz_full_config = os.path.join(rviz_base,"moveit.rviz")
    rviz_node_full = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_full_config],
        parameters=[
            robot_description,
            robot_description_semantic,
            ompl_planning_pipeline_config,
            robot_description_kinematics,
            {"use_sim_time": True},
        ],
        condition=UnlessCondition(load_rviz),
    )


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


    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        parameters=[robot_description, {"use_sim_time": True}],
    )


    ros2_controllers_path = os.path.join(
        get_package_share_directory("bricky_main"),
        "config",
        "ros2_controllers.yaml",
    )


    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description,ros2_controllers_path],
        output={
            "stdout": "screen",
            "stderr": "screen",
        }
    )


    declare_use_sim_time_cmd = DeclareLaunchArgument(
        name='use_sim_time',
        default_value='True',
        description='Use simulation (Gazebo) clock if true'
    )


    load_controllers = []
    for controller in [
        "robot_arm_controller",
        "joint_state_controller",
        
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]

    return LaunchDescription(
        [
            # Gazebo nodes:
            gazebo,
            robot_state_publisher,
            spawn_entity,
            static_tf,
            # ROS2_CONTROL:
                        
            # ros2_control_node,
            
            RegisterEventHandler(
                OnProcessExit(
                    target_action = spawn_entity,
                    on_exit = [

                        # MoveIt!2:
                        rviz_arg,
                        declare_use_sim_time_cmd,
                        rviz_node_full,
                        run_move_group_node,
                    ]
                )
            )
        ],
        + load_controllers
    )

To run controllers manually I use commands:

ros2 run controller_manager spawner joint_state_controller
ros2 run controller_manager spawner robot_arm_controller
$\endgroup$
3
  • $\begingroup$ 1. you are right, gazebo_ros2_control runs the controller_manager, no need for ros2_control_node $\endgroup$ Sep 13, 2023 at 6:42
  • $\begingroup$ 2. gazebo_ros2_control gets the time source from gazebo and has use_sim_time set. All controllers loaded by the controller manager inherit this time $\endgroup$ Sep 13, 2023 at 6:42
  • $\begingroup$ 3. The spawner normally waits for the controller_manager to be available. But you can increase the timeout if necessary spawn_joint_state_broadcaster = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager-timeout", "20"], output="screen") Or you can use the RegisterEventHandler with on_exit method, see here $\endgroup$ Jan 31 at 11:57

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.