search

tenfoldpaper / multipanda_ros2

Apache License 2.0
13 stars 3 forks source link

Deadlock when calling `/panda_gripper_sim_node/gripper_action` #4

Closed Ekanshh closed 1 month ago

Ekanshh commented 1 month ago

Deadlock happen when specifically calling /panda_gripper_sim_node/gripper_action. This is potentially related to #3 on why the moveit is unable to execute the gripper command.

Action Call

ros2 action send_goal /panda_gripper_sim_node/gripper_action control_msgs/action/GripperCommand "{command: {position: 0.01, max_effort: 0.5}}"
Waiting for an action server to become available...
Sending goal:
     command:
  position: 0.01
  max_effort: 0.5

Goal accepted with ID: e77af40788c8413fa3ba33e1c9afb9be

Logs

ros2 launch franka_bringup multimode_sim.launch.py 
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [1340789]
[INFO] [joint_state_publisher-2]: process started with pid [1340791]
[INFO] [franka_control2_node-3]: process started with pid [1340793]
[INFO] [spawner-4]: process started with pid [1340795]
[INFO] [rviz2-5]: process started with pid [1340797]
[robot_state_publisher-1] [INFO] [1719880201.070639997] [robot_state_publisher]: got segment panda_hand
[robot_state_publisher-1] [INFO] [1719880201.070695507] [robot_state_publisher]: got segment panda_hand_tcp
[robot_state_publisher-1] [INFO] [1719880201.070700326] [robot_state_publisher]: got segment panda_leftfinger
[robot_state_publisher-1] [INFO] [1719880201.070703046] [robot_state_publisher]: got segment panda_link0
[robot_state_publisher-1] [INFO] [1719880201.070705571] [robot_state_publisher]: got segment panda_link1
[robot_state_publisher-1] [INFO] [1719880201.070708067] [robot_state_publisher]: got segment panda_link2
[robot_state_publisher-1] [INFO] [1719880201.070710389] [robot_state_publisher]: got segment panda_link3
[robot_state_publisher-1] [INFO] [1719880201.070712763] [robot_state_publisher]: got segment panda_link4
[robot_state_publisher-1] [INFO] [1719880201.070715097] [robot_state_publisher]: got segment panda_link5
[robot_state_publisher-1] [INFO] [1719880201.070717442] [robot_state_publisher]: got segment panda_link6
[robot_state_publisher-1] [INFO] [1719880201.070719688] [robot_state_publisher]: got segment panda_link7
[robot_state_publisher-1] [INFO] [1719880201.070721939] [robot_state_publisher]: got segment panda_link8
[robot_state_publisher-1] [INFO] [1719880201.070724261] [robot_state_publisher]: got segment panda_rightfinger
[franka_control2_node-3] [WARN] [1719880201.075214285] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[franka_control2_node-3] [INFO] [1719880201.075413232] [resource_manager]: Loading hardware 'FrankaMujocoHardwareInterface' 
[franka_control2_node-3] [INFO] [1719880201.080219157] [resource_manager]: Initialize hardware 'FrankaMujocoHardwareInterface' 
[franka_control2_node-3] [INFO] [1719880201.080280396] [FrankaMujocoMultiHardwareInterface]: Initial checks all passed
[joint_state_publisher-2] [INFO] [1719880201.337667172] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
[rviz2-5] [INFO] [1719880201.393195180] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-5] [INFO] [1719880201.393290412] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-5] [INFO] [1719880201.407077224] [rviz2]: Stereo is NOT SUPPORTED
[franka_control2_node-3] [INFO] [1719880201.622710904] [panda_gripper_sim_node]: Started gripper sim
[franka_control2_node-3] Successfully populated obj_box_01
[franka_control2_node-3] Successfully populated sphere_01
[franka_control2_node-3] Successfully populated panda_link0
[franka_control2_node-3] [INFO] [1719880201.627987558] [mujoco_pose_service_server]: mujoco_pose_service_server started
[franka_control2_node-3] [INFO] [1719880201.628266370] [resource_manager]: Successful initialization of hardware 'FrankaMujocoHardwareInterface'
[franka_control2_node-3] [INFO] [1719880201.628349948] [resource_manager]: 'configure' hardware 'FrankaMujocoHardwareInterface' 
[franka_control2_node-3] [INFO] [1719880201.628357418] [resource_manager]: Successful 'configure' of hardware 'FrankaMujocoHardwareInterface'
[franka_control2_node-3] [INFO] [1719880201.628367362] [resource_manager]: 'activate' hardware 'FrankaMujocoHardwareInterface' 
[franka_control2_node-3] [INFO] [1719880201.628371752] [FrankaMujocoMultiHardwareInterface]: Started
[franka_control2_node-3] [INFO] [1719880201.628376465] [resource_manager]: Successful 'activate' of hardware 'FrankaMujocoHardwareInterface'
[franka_control2_node-3] [INFO] [1719880201.632051070] [controller_manager]: franka_control2_node
[franka_control2_node-3] [INFO] [1719880201.632075262] [controller_manager]: update rate is 1000 Hz
[franka_control2_node-3] [INFO] [1719880201.632188267] [controller_manager]: RT kernel is recommended for better performance
[franka_control2_node-3] [INFO] [1719880201.712151523] [controller_manager]: Loading controller 'single_multi_mode_controller'
[franka_control2_node-3] [INFO] [1719880201.721473788] [single_multi_mode_controller]: Finished initializing multi joint impedance example controller for 1 arms
[spawner-4] [INFO] [1719880201.723052071] [spawner_single_multi_mode_controller]: Loaded single_multi_mode_controller
[franka_control2_node-3] [INFO] [1719880201.723550288] [controller_manager]: Configuring controller 'single_multi_mode_controller'
[franka_control2_node-3] [INFO] [1719880201.723643168] [franka_model_semantic_component]: Initialized FrankaRobotModel with params panda, panda/robot_model
[franka_control2_node-3] [INFO] [1719880201.723655261] [single_multi_mode_controller]: Arm set for panda
[franka_control2_node-3] [INFO] [1719880201.723672386] [single_multi_mode_controller]: MultiPandaMultiModeController: Creating controller panda_joint_impedance_controller for resource panda.
[franka_control2_node-3] [INFO] [1719880201.723703269] [single_multi_mode_controller]: MultiPandaMultiModeController: Creating controller panda_cartesian_impedance_controller for resource panda.
[franka_control2_node-3] [INFO] [1719880201.724218174] [single_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller panda_cartesian_impedance_controller for resource panda.
[franka_control2_node-3] [INFO] [1719880201.724249585] [single_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller panda_joint_impedance_controller for resource panda.
[franka_control2_node-3] [INFO] [1719880201.724266861] [single_multi_mode_controller]: MultiPandaMultiModeController: New active control set:
[franka_control2_node-3] panda_cartesian_impedance_controller: panda
[franka_control2_node-3] [INFO] [1719880201.726271483] [FrankaMujocoMultiHardwareInterface]: Preparing command mode switch
[franka_control2_node-3] [INFO] [1719880201.726379976] [FrankaMujocoMultiHardwareInterface]: Performing command mode switch
[franka_control2_node-3] [INFO] [1719880201.726426640] [FrankaMujocoMultiHardwareInterface]: Arm panda current mode: joint_torque
[franka_control2_node-3] panda/robot_state
[franka_control2_node-3] panda/robot_model
[spawner-4] [INFO] [1719880201.728857622] [spawner_single_multi_mode_controller]: Configured and activated single_multi_mode_controller
[INFO] [spawner-4]: process has finished cleanly [pid 1340795]
[franka_control2_node-3] [INFO] [1719880206.989113988] [panda_gripper_sim_node]: Received GripperCommand request
[franka_control2_node-3] [ERROR] [1719880206.989522033] [panda_gripper_sim_node]: GripperServer: Commanding out of range width! max_width = 0.080000 command = 0.200000
[franka_control2_node-3] [INFO] [1719880239.947305568] [panda_gripper_sim_node]: Received GripperCommand request
[franka_control2_node-3] [INFO] [1719880239.947449932] [panda_gripper_sim_node]: Gripper GripperCommand...
Ekanshh commented 1 month ago

The root cause of the deadlock was identified in the codebase. Specifically, this line of code was commented out. This line is responsible for releasing the gripper_state_mutex_ in the onExecuteGripperCommand function before invoking the simGripperMove function.

The simGripperMove function requires access to the shared resource protected by gripper_state_mutex_. However, since the mutex was not released in onExecuteGripperCommand, it led to a deadlock situation where simGripperMove could not proceed, waiting indefinitely for the mutex, which was already locked by the parent function.

This also fixes #3