gazebo_ros2_control failed to load controllers
Tl;dr
How to load controllers with the gazebo_ros2_plugin
?
Greetings. I am using a Universal Robot UR series robot for a real application as well as simulation within Gazebo. Since the developers did not support simulation in the ROS Foxy branch I modified the driver an the robots description. This works reasonably well (ROS Foxy driver + ROS Galactic description) with further modifications to the hardware interface.
My question/problem however is in regard to ros2_control
and gazebo_ros2_control
. The driver loads ros2_control
(controller_manager
and the respective controllers), this seems to be fine for the real robot.
Since gazebo_ros2_control
also instantiates the controller_manager
I've gotten an error in the past. In order to use the plugin inside the URDF fully, I would like it to load the defined controllers as well. This seems to be done within the demos, since no controller_manager
node is started within the launch file and the controller parameters are given by the YAML configuration file. When I try to load my controllers this way, no controller is loaded at all, what am I missing?
A snipped of my controllers YAML:
controller_manager:
ros__parameters:
update_rate: 60 # Hz
use_sim_time: True
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
io_and_status_controller:
type: ur_controllers/GPIOController
speed_scaling_state_broadcaster:
type: ur_controllers/SpeedScalingStateBroadcaster
force_torque_sensor_broadcaster:
type: ur_controllers/ForceTorqueStateBroadcaster
joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
scaled_joint_trajectory_controller:
type: ur_controllers/ScaledJointTrajectoryController
speed_scaling_state_broadcaster:
ros__parameters:
state_publish_rate: 60.0
use_sim_time: True
force_torque_sensor_broadcaster:
ros__parameters:
sensor_name: tcp_fts_sensor
state_interface_names:
- force.x
- force.y
- force.z
- torque.x
- torque.y
- torque.z
frame_id: tool0
topic_name: ft_data
use_sim_time: True
The snipped of my XACRO:
<xacro:if value="$(arg sim_gazebo)">
<!-- Gazebo plugins -->
<gazebo reference="world">
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(arg simulation_controllers)</parameters>
</plugin>
</gazebo>
</xacro:if>
Can you add your log from the Gazebo, launched with
--verbose
flag? Perhaps here we can find more info useful for debugging process