Using Grippers in Stage

edit

Hey,

I have a simulation in Stage with multiple robots that are equipped with grippers:

define simpleGripper gripper
(
  # gripper properties
  paddle_size [ 0.66 0.1 0.4 ]
  paddle_state [ "open" "down" ]
   autosnatch 0

  # model properties
  size [ 0.2 0.3 0.2 ]
)

Now I'd like to tell the robots that they should grab an object and transport it to a specific position. I don't want to use the pr2_controller since it is too complicated. Is there a simple way to write a node that generates goals for the gripper and publishes that goal?

I thought about using the control_msgs/GripperCommand.action, something like that:

#include <control_msgs/GripperCommand.h>
#include <control_msgs/GripperCommandAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<control_msgs::GripperCommandAction> GripperCommandClient;

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

 GripperCommandClient ac("gripper_action", true);
 control_msgs::GripperCommandGoal goal;

 while(!ac.waitForServer(ros::Duration(5.0)))
 {
   ROS_INFO("Waiting for the move_base action server to come up");
 }

 ROS_INFO_STREAM("sending goal...");
 ac.sendGoal(goal);

 ac.waitForResult();

 if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
   ROS_INFO_STREAM("reached its goal!");

 else
   ROS_INFO_STREAM("failed to move to its goal!");
}

But actually I don't know how to address the "gripper_action" in that line: GripperCommandClient ac("gripper_action", true); since there is no topic published for the gripper. And furthermore I have no idea how to set the goal for a gripper.

Thanks in advance! :)