38 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h> 39 #include <pr2_gripper_sensor_msgs/PR2GripperReleaseAction.h> 40 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorAction.h> 41 #include <std_srvs/Empty.h> 65 event_client_ =
new EventClient(
"event_detector",
true);
70 ROS_INFO(
"Waiting for the gripper_action action server to come up");
74 ROS_INFO(
"Waiting for the event_detector action server to come up");
84 void open(
double position_open){
86 pr2_controllers_msgs::Pr2GripperCommandGoal
open;
87 open.command.position = position_open;
88 open.command.max_effort = -1.0;
96 void place(
int trigger_conditions,
double acc_trigger,
double slip_trigger){
97 pr2_gripper_sensor_msgs::PR2GripperEventDetectorGoal place_goal;
98 place_goal.command.trigger_conditions = trigger_conditions;
99 place_goal.command.acceleration_trigger_magnitude = acc_trigger;
100 place_goal.command.slip_trigger_magnitude = slip_trigger;
102 ROS_INFO(
"Waiting for object placement contact...");
103 event_client_->
sendGoal(place_goal);
108 ROS_INFO(
"cond met: %d, acc_met: %d, slip_met: %d", event_client_->
getResult()->data.trigger_conditions_met, event_client_->
getResult()->data.acceleration_event, event_client_->
getResult()->data.slip_event);
127 pr2_gripper_sensor_msgs::PR2GripperReleaseFeedback
feedback_;
128 pr2_gripper_sensor_msgs::PR2GripperReleaseResult
result_;
143 void executeCB(
const pr2_gripper_sensor_msgs::PR2GripperReleaseGoalConstPtr &goal)
145 double position_open;
146 if(!nh_.
getParam(
"position_open", position_open))
150 bool prempted =
false;
152 gripper.
place(goal->command.event.trigger_conditions, goal->command.event.acceleration_trigger_magnitude, goal->command.event.slip_trigger_magnitude);
157 ROS_INFO(
"%s: Preempted", action_name_.c_str());
163 gripper.
open(position_open);
166 result_.data.rtstate.realtime_controller_state = feedback_.data.rtstate.realtime_controller_state;
167 ROS_INFO(
"%s: Succeeded", action_name_.c_str());
175 int main(
int argc,
char** argv)
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
pr2_gripper_sensor_msgs::PR2GripperReleaseResult result_
void open(double position_open)
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void place(int trigger_conditions, double acc_trigger, double slip_trigger)
int main(int argc, char **argv)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
ROSCPP_DECL void spin(Spinner &spinner)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
const std::string & getNamespace() const
bool isPreemptRequested()
pr2_gripper_sensor_msgs::PR2GripperReleaseFeedback feedback_
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
actionlib::SimpleActionServer< pr2_gripper_sensor_msgs::PR2GripperReleaseAction > as_
bool getParam(const std::string &key, std::string &s) const
GripperClient * gripper_client_
PR2GripperReleaseAction(std::string name)
EventClient * event_client_
SimpleClientGoalState getState() const
ResultConstPtr getResult() const
~PR2GripperReleaseAction(void)
actionlib::SimpleActionClient< pr2_gripper_sensor_msgs::PR2GripperEventDetectorAction > EventClient
void executeCB(const pr2_gripper_sensor_msgs::PR2GripperReleaseGoalConstPtr &goal)
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > GripperClient