Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031
00032 #include <actionlib/client/simple_action_client.h>
00033 #include <std_msgs/Empty.h>
00034
00035 #include "pr2_gripper_click/GripperClickPickupAction.h"
00036 #include "pr2_gripper_click/GripperClickPlaceAction.h"
00037
00038 class GraspPlanningGripperClick
00039 {
00040 private:
00041 ros::NodeHandle root_nh_;
00042
00043 ros::NodeHandle priv_nh_;
00044
00045 actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPickupAction> *pickup_action_client_;
00046 actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPlaceAction> *place_action_client_;
00047
00048 std::string pickup_action_topic_;
00049 std::string place_action_topic_;
00050
00051 ros::Subscriber sub_;
00052
00053 void triggerCb(const std_msgs::EmptyConstPtr& msg)
00054 {
00055 while(!pickup_action_client_->waitForServer(ros::Duration(2.0)) && priv_nh_.ok())
00056 {
00057 ROS_INFO("Waiting for action client");
00058 }
00059
00060 pr2_gripper_click::GripperClickPickupGoal click_goal;
00061 ROS_INFO("Sending goal");
00062 pickup_action_client_->sendGoal(click_goal);
00063 while (!pickup_action_client_->waitForResult(ros::Duration(0.5)) && priv_nh_.ok())
00064 {
00065 }
00066 if (!priv_nh_.ok())
00067 {
00068 pickup_action_client_->cancelGoal();
00069 return;
00070 }
00071 if (pickup_action_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00072 {
00073 ROS_INFO("The gripper click grasp action has not succeeded;");
00074 }
00075 else
00076 {
00077 ROS_INFO("Gripper click grasp has succeeded;");
00078 }
00079 }
00080
00081 public:
00082 GraspPlanningGripperClick() :
00083 root_nh_(""), priv_nh_("~")
00084 {
00085
00086 priv_nh_.param<std::string>("pickup_action_topic", pickup_action_topic_, "/gripper_click_pickup_ui");
00087 pickup_action_client_ = new actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPickupAction>
00088 (pickup_action_topic_, true);
00089
00090 priv_nh_.param<std::string>("place_action_topic", place_action_topic_, "/gripper_click_place_ui");
00091 place_action_client_ = new actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPlaceAction>
00092 (place_action_topic_, true);
00093
00094
00095 sub_ = root_nh_.subscribe("trigger", 1, &GraspPlanningGripperClick::triggerCb, this);
00096
00097 ROS_INFO("Grasp planning gripper click node started");
00098 }
00099 ~GraspPlanningGripperClick()
00100 {
00101 delete pickup_action_client_;
00102 delete place_action_client_;
00103 }
00104 };
00105
00106 int main(int argc, char **argv)
00107 {
00108 ros::init(argc, argv, "grasp_planning_gripper_click_node");
00109 GraspPlanningGripperClick node;
00110 ros::spin();
00111 return 0;
00112 }