Why does SimpleActionClient abort?
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:
- What could cause the action to abort? Is there documentation on this?
- 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
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.
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.
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.