ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Automatize automatic error recovery

asked 2018-09-27 05:16:51 -0500

aPonza gravatar image

ROS kinetic, Ubuntu 16.04, kernel 4.8.15-rt10, libfranka 0.5.0, franka_ros 0.6.0

I'm working with a Franka Emika Panda (docs) and trying to setup a way for the robot to automatically recover from an error state. I implemented a simple action client to call the service for automatic error recovery and I would like to send the goal automatically if the robot is in an error state.

Being an error, I can't try-catch it like I would with an exception. I found that robot_state.current_errors (from franka::RobotState) is an array that's either empty or has the name of the errors at that moment and thought to use that for an if condition to trigger the service call. I have two issues here:

  • I want to iterate through the list of errors for cosmetic purposes to print them and coming from python I thought of a for-in-loop like for (const franka::Errors& e : robot_state.current_errors.iterator()) but I can't manage use it. Is it because it's not implemented for the franka::Errors class?
  • I can't figure out where to get the robot state from: I can't instantiate a franka::Robot because that is already controlled by the franka_control_node; this node doens't seem to provide a way to get the state or something, however franka_hw::FrankaStateInterface does have a getRobotState() from the FrankaStateHandle, which I don't know how to get.
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted

answered 2018-10-24 02:50:22 -0500

aPonza gravatar image

franka_msgs::RobotState is and was published by /franka_control on topic /franka_state_controller/franka_states, however that ended up being not that useful since at that time it was missing a way to see if the robot was in an error state at that moment. I was still missing a piece of the puzzle, and ended up opening an issue. The devs showed 2 ways to proceed and shortly after my post added RobotMode, which ended up being what I'm currently using.

To answer the quetion, then, I added a private franka_msgs::FrankaState::ConstPtr current_franka_state_constptr_;, updated via subscriber callback, and:

uint8_t counter = 0;
for (auto it = &current_franka_state_constptr_->last_motion_errors.joint_position_limits_violation;
     it != &current_franka_state_constptr_->last_motion_errors.instability_detected;
     ++it, ++counter)
  if (*it)
    ROS_ERROR(" - %s", name_franka_errors_[counter].c_str());
  • used the added robot mode for error recovery (done with an action client as franka emika suggests in the docs):
if (current_franka_state_constptr_->robot_mode == franka_msgs::FrankaState::ROBOT_MODE_REFLEX)
edit flag offensive delete link more

Question Tools



Asked: 2018-09-27 05:16:51 -0500

Seen: 638 times

Last updated: Oct 24 '18