Resource manager accessing wrong hw_interface
I am trying to setup a separate hw_interface for the gripper and robot however after launch I get error message from the resource_manager
on components not accepting new command resource combinations.
Is there a way to have both controllers up and running at the same time with each controller having different number of joints to command and control interfaces?
The errors I am getting:
[ros2_control_node-3] [ERROR] [1652677000.851280800] [resource_manager]: Component 'INOBOTGripper' did not accept new command resource combination:
[ros2_control_node-3] Start interfaces:
[ros2_control_node-3] [
[ros2_control_node-3] link_1_joint/position
[ros2_control_node-3] link_2_joint/position
[ros2_control_node-3] link_3_joint/position
[ros2_control_node-3] link_4_joint/position
[ros2_control_node-3] link_5_joint/position
[ros2_control_node-3] link_6_joint/position
[ros2_control_node-3] ]
[ros2_control_node-3] Stop interfaces:
[ros2_control_node-3] [
[ros2_control_node-3] ]
[ros2_control_node-3]
[ros2_control_node-3] [ERROR] [1652677000.851357300] [controller_manager]: Could not switch controllers since prepare command mode switch was rejected.
[ros2_control_node-3] [INFO] [1652677000.851627600] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner.py-6] [INFO] [1652677000.853083100] [spawner_arm_joint_trajectory_controller]: Failed to start controller
[ERROR] [spawner.py-6]: process has died [pid 32327, exit code 1, cmd '/opt/ros/foxy/lib/controller_manager/spawner.py arm_joint_trajectory_controller --controller-manager /controller_manager --ros-args'].
[ros2_control_node-3] [INFO] [1652677001.244891200] [controller_manager]: Configuring controller 'gripper_controller'
[ros2_control_node-3] [INFO] [1652677001.246297200] [gripper_controller]: configure successful
[spawner.py-7] [INFO] [1652677001.246709200] [spawner_joint_state_broadcaster]: Configured and started joint_state_broadcaster
[ros2_control_node-3] [ERROR] [1652677001.252742000] [resource_manager]: Component 'INOBOTModularSystem' did not accept new command resource combination:
[ros2_control_node-3] Start interfaces:
[ros2_control_node-3] [
[ros2_control_node-3] right_finger_joint/position
[ros2_control_node-3] ]
[ros2_control_node-3] Stop interfaces:
[ros2_control_node-3] [
[ros2_control_node-3] ]
[ros2_control_node-3]
[ros2_control_node-3] [ERROR] [1652677001.252929900] [controller_manager]: Could not switch controllers since prepare command mode switch was rejected.
[spawner.py-5] [INFO] [1652677001.256148300] [spawner_gripper_controller]: Failed to start controller
[INFO] [spawner.py-7]: process has finished cleanly [pid 32329]
[ERROR] [spawner.py-5]: process has died [pid 32325, exit code 1, cmd '/opt/ros/foxy/lib/con
The content of my config files and urdf is as follows:
ros2_controllers.yaml
controller_manager:
ros__parameters:
update_rate: 10 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
arm_joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
gripper_controller:
type: forward_command_controller/ForwardCommandController
arm_joint_trajectory_controller:
ros__parameters:
command_interfaces:
- position
state_interfaces:
- position
joints:
- link_1_joint
- link_2_joint
- link_3_joint
- link_4_joint
- link_5_joint
- link_6_joint
state_publish_rate: 10.0
action_monitor_rate: 10.0
allow_partial_joints_goal: false
open_loop_control: true
allow_integration_in_goal_trajectories: true
constraints:
stopped_velocity_tolerance: 0.01
goal_time: 0.0
gripper_controller:
ros__parameters:
joints:
- right_finger_joint
interface_name: position
ros2_control tag in urdf files:
in <robot name="robot_arm">
:
<ros2_control name="INOBOTModularSystem" type="system">
<hardware>
<plugin>inobot_control_hardware/INOBOTSystemMultiInterfaceHardware</plugin>
<param name="example_param_hw_start_duration_sec">0.0</param>
<param name="example_param_hw_stop_duration_sec">3.0</param>
<param name="example_param_hw_slowdown">0</param>
</hardware>
<joint name="link_1_joint">
<command_interface name="position">
<param name="min"> -100 </param>
<param name="max"> 100 </param>
</command_interface>
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<command_interface name="acceleration">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity"/>
<state_interface name="acceleration"/>
</joint>
...
<joint name="link_6_joint">
<command_interface name="position">
<param name="min"> -100 </param>
<param name="max"> 100 </param>
</command_interface>
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<command_interface name="acceleration">
<param name="min">-1</param>
<param ...