Why does SimpleActionClient abort?

asked 2015-04-01 18:22:04 -0600

dinosaur gravatar image

I'm loosely following this tutorial to write a pr2 gripper client that can open and close the gripper.

Many of my actions are being mysteriously aborted, even though there is nothing stopping the gripper from closing. (I'm using a simulated pr2 in gazebo, in an empty world.)

I have a few questions:

  1. What could cause the action to abort? Is there documentation on this?
  2. How do I prevent it from aborting?

I'm including my code below, in case there is some issue with the code that is causing the unexpected action abortion.

gripper_demo.cpp

#include <ros/ros.h>
#include <unistd.h>
#include <ctime>

#include "pr2_pick_manipulation/gripper.h"

using pr2_pick_manipulation::Gripper;

int main(int argc, char **argv) {
  ros::init(argc, argv, "gripper_demo");
  ros::NodeHandle node_handle;

  Gripper gripper("r_gripper_controller/gripper_action");
  double positions[] = {0.01, 0.04, 0.08};

  std::clock_t start = std::clock();
  for(int i = 0; i < sizeof(positions) / sizeof(double); i++) {
    printf("Setting gripper position to %0.2f\n", positions[i]);
    bool status = gripper.setPosition(positions[i]);
    std::clock_t current = std::clock();
    printf("Seconds elapsed from start %0.05f\n\n",
           (current - start) / (double) CLOCKS_PER_SEC);
  }

gripper.h

#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
#include <ros/ros.h>

#ifndef _PR2_PICK_MANIPULATION_GRIPPER_H_
#define _PR2_PICK_MANIPULATION_GRIPPER_H_

namespace pr2_pick_manipulation {
typedef actionlib::SimpleActionClient<
  pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;

class Gripper {
 private:
  GripperClient* gripper_client_;
 public:
  Gripper(const std::string& action_name);
  ~Gripper();
  bool setPosition(double position);
};
};  // namespace pr2_pick_manipulation

#endif

gripper.cpp

#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
#include <ros/ros.h>
#include <unistd.h>

#include "pr2_pick_manipulation/gripper.h"

using actionlib::SimpleClientGoalState;

namespace pr2_pick_manipulation {
Gripper::Gripper(const std::string& action_name) {
  gripper_client_ = new GripperClient(action_name, true);
  while(!gripper_client_->waitForServer()){
    ROS_INFO("Waiting for the %s action server to come up",
             action_name.c_str());
  }
}

Gripper::~Gripper() {
  delete gripper_client_;
}

bool Gripper::setPosition(double position) {
  pr2_controllers_msgs::Pr2GripperCommandGoal goal;
  goal.command.position = position;
  goal.command.max_effort = -1.0;  // Do not limit effort (negative)

  ros::Duration five_seconds(5.0);
  gripper_client_->sendGoal(goal);
  bool finished = gripper_client_->waitForResult(five_seconds);
  printf("Finished: %s\n", finished ? "yes" : "no");
  SimpleClientGoalState state = gripper_client_->getState();
  printf("State: %s\n", state.toString().c_str());
  if(state == SimpleClientGoalState::SUCCEEDED) {
    return true;
  } else {
    return false;
  }
}
};  // namespace pr2_pick_manipulation

output of gripper_demo

I ran the gripper demo, with the right gripper starting open. Watching the robot's gripper in gazebo, the gripper starts to close a little, then immediately opens back up again. Console output below.

$ rosrun pr2_tutorials gripper_demo --screen
Setting gripper position to 0.01
Finished: yes
State: ABORTED
Seconds elapsed from start 0.02000

Setting gripper position to 0.04
Finished: yes
State: ABORTED
Seconds elapsed from start 0.04000

Setting gripper position to 0.08
Finished: yes
State: SUCCEEDED
Seconds elapsed from start 0.04000
edit retag flag offensive close merge delete

Comments

I now it's been like a year now, but I'm facing the same problem, not sure why. My action client is aborting the gripper goals I send, but the nevertheless the grippe keeps moving to find the goal, it like only the code rejects the action.

Rodrigo gravatar image Rodrigo  ( 2016-04-22 10:37:21 -0600 )edit
1

I think I figured out it was due to the nature of Gazebo. Gazebo doesn't simulate the real world perfectly, and gripper actions is one of the ways. iirc, when I ran the same code on a real robot, it didn't abort.

dinosaur gravatar image dinosaur  ( 2016-04-22 11:35:02 -0600 )edit

I was about to post that, I don't know if the same problem would be present in an actual PR2. Is good to know that's only a thing in Gazebo.

Rodrigo gravatar image Rodrigo  ( 2016-04-22 12:03:28 -0600 )edit