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
00031
00032
00033
00034
00035
00036
00037 #include "ros/ros.h"
00038
00039 #include <actionlib/server/action_server.h>
00040 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactAction.h>
00041 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactData.h>
00042 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactCommand.h>
00043 #include <std_srvs/Empty.h>
00044
00045 class Pr2GripperFindContact
00046 {
00047 private:
00048 typedef actionlib::ActionServer<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> GAS;
00049 typedef GAS::GoalHandle GoalHandle;
00050 public:
00051 Pr2GripperFindContact(ros::NodeHandle &n) :
00052 node_(n),
00053 action_server_(node_, "find_contact",
00054 boost::bind(&Pr2GripperFindContact::goalCB, this, _1),
00055 boost::bind(&Pr2GripperFindContact::cancelCB, this, _1)),
00056 has_active_goal_(false)
00057 {
00058 ros::NodeHandle pn("~");
00059
00060
00061 pub_controller_command_ =
00062 node_.advertise<pr2_gripper_sensor_msgs::PR2GripperFindContactCommand>("find_contact", 1);
00063 sub_controller_state_ =
00064 node_.subscribe("contact_state", 1, &Pr2GripperFindContact::controllerStateCB, this);
00065
00066 watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &Pr2GripperFindContact::watchdog, this);
00067
00068 }
00069
00070 ~Pr2GripperFindContact()
00071 {
00072 pub_controller_command_.shutdown();
00073 sub_controller_state_.shutdown();
00074 watchdog_timer_.stop();
00075 }
00076
00077 private:
00078
00079 ros::NodeHandle node_;
00080 GAS action_server_;
00081 ros::Publisher pub_controller_command_;
00082 ros::Subscriber sub_controller_state_;
00083 ros::Timer watchdog_timer_;
00084
00085 bool has_active_goal_;
00086 GoalHandle active_goal_;
00087 ros::Time goal_received_;
00088
00089 double stall_timeout_;
00090 ros::Time last_movement_time_;
00091 ros::Time action_start_time;
00092
00093
00094 void watchdog(const ros::TimerEvent &e)
00095 {
00096 ros::Time now = ros::Time::now();
00097
00098
00099 if (has_active_goal_)
00100 {
00101 bool should_abort = false;
00102 if (!last_controller_state_)
00103 {
00104 should_abort = true;
00105 ROS_WARN("Aborting goal because we have never heard a controller state message.");
00106 }
00107 else if ((now - last_controller_state_->stamp) > ros::Duration(5.0))
00108 {
00109 should_abort = true;
00110 ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
00111 (now - last_controller_state_->stamp).toSec());
00112 }
00113
00114 if (should_abort)
00115 {
00116
00117 active_goal_.setAborted();
00118 has_active_goal_ = false;
00119 }
00120 }
00121 }
00122
00123 void goalCB(GoalHandle gh)
00124 {
00125
00126 if (has_active_goal_)
00127 {
00128
00129 active_goal_.setCanceled();
00130 has_active_goal_ = false;
00131 }
00132
00133 gh.setAccepted();
00134 active_goal_ = gh;
00135 has_active_goal_ = true;
00136 goal_received_ = ros::Time::now();
00137
00138
00139 if(active_goal_.getGoal()->command.zero_fingertip_sensors)
00140 {
00141 std_srvs::Empty::Request req;
00142 std_srvs::Empty::Response resp;
00143 if(ros::service::exists("zero_fingertip_sensors",true))
00144 {
00145 ROS_INFO("updating zeros!");
00146 ros::service::call("zero_fingertip_sensors",req,resp);
00147 }
00148 }
00149
00150
00151 pub_controller_command_.publish(active_goal_.getGoal()->command);
00152
00153 last_movement_time_ = ros::Time::now();
00154 action_start_time = ros::Time::now();
00155 }
00156
00157 void cancelCB(GoalHandle gh)
00158 {
00159
00160 if (active_goal_ == gh)
00161 {
00162
00163 std_srvs::Empty::Request req;
00164 std_srvs::Empty::Response resp;
00165 if(ros::service::exists("stop_motor_output",true))
00166 {
00167 ROS_INFO("Stopping Motor Output");
00168 ros::service::call("stop_motor_output",req,resp);
00169 }
00170
00171
00172 active_goal_.setCanceled();
00173 has_active_goal_ = false;
00174 }
00175 }
00176
00177
00178 pr2_gripper_sensor_msgs::PR2GripperFindContactDataConstPtr last_controller_state_;
00179 void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactDataConstPtr &msg)
00180 {
00181 last_controller_state_ = msg;
00182 ros::Time now = ros::Time::now();
00183
00184 if (!has_active_goal_)
00185 return;
00186
00187 pr2_gripper_sensor_msgs::PR2GripperFindContactGoal goal;
00188 goal.command = active_goal_.getGoal()->command;
00189
00190 pr2_gripper_sensor_msgs::PR2GripperFindContactFeedback feedback;
00191 feedback.data = *msg;
00192
00193 pr2_gripper_sensor_msgs::PR2GripperFindContactResult result;
00194 result.data = *msg;
00195
00196
00197 double dT = 0.2;
00198 if(feedback.data.stamp.toSec() < action_start_time.toSec()+dT ){}
00199
00200
00201
00202 else if(feedback.data.rtstate.realtime_controller_state == 5 || feedback.data.rtstate.realtime_controller_state == 3)
00203 {
00204 if(feedback.data.contact_conditions_met)
00205 {
00206 active_goal_.setSucceeded(result);
00207 has_active_goal_ = false;
00208 }
00209 }
00210
00211 else
00212 has_active_goal_ = false;
00213
00214 active_goal_.publishFeedback(feedback);
00215 }
00216 };
00217
00218
00219 int main(int argc, char** argv)
00220 {
00221 ros::init(argc, argv, "find_contact_node");
00222 ros::NodeHandle node;
00223 Pr2GripperFindContact jte(node);
00224
00225 ros::spin();
00226
00227 return 0;
00228 }