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/PR2GripperEventDetectorAction.h>
00041 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorData.h>
00042 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorCommand.h>
00043
00044
00045 class Pr2GripperEventDetector
00046 {
00047 private:
00048 typedef actionlib::ActionServer<pr2_gripper_sensor_msgs::PR2GripperEventDetectorAction> GAS;
00049 typedef GAS::GoalHandle GoalHandle;
00050 public:
00051 Pr2GripperEventDetector(ros::NodeHandle &n) :
00052 node_(n),
00053 action_server_(node_, "event_detector",
00054 boost::bind(&Pr2GripperEventDetector::goalCB, this, _1),
00055 boost::bind(&Pr2GripperEventDetector::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::PR2GripperEventDetectorCommand>("event_detector", 1);
00063 sub_controller_state_ =
00064 node_.subscribe("event_detector_state", 1, &Pr2GripperEventDetector::controllerStateCB, this);
00065
00066 watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &Pr2GripperEventDetector::watchdog, this);
00067
00068 }
00069
00070 ~Pr2GripperEventDetector()
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 ros::Time action_start_time;
00085
00086 bool has_active_goal_;
00087 GoalHandle active_goal_;
00088 ros::Time goal_received_;
00089
00090 double stall_timeout_;
00091 ros::Time last_movement_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 pub_controller_command_.publish(gh.getGoal()->command);
00140
00141 last_movement_time_ = ros::Time::now();
00142 action_start_time = ros::Time::now();
00143 }
00144
00145 void cancelCB(GoalHandle gh)
00146 {
00147
00148 if (active_goal_ == gh)
00149 {
00150
00151 if (last_controller_state_)
00152 {
00153
00154 }
00155
00156
00157 active_goal_.setCanceled();
00158 has_active_goal_ = false;
00159 }
00160 }
00161
00162
00163 pr2_gripper_sensor_msgs::PR2GripperEventDetectorDataConstPtr last_controller_state_;
00164 void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperEventDetectorDataConstPtr &msg)
00165 {
00166 last_controller_state_ = msg;
00167 ros::Time now = ros::Time::now();
00168
00169 if (!has_active_goal_)
00170 return;
00171
00172 pr2_gripper_sensor_msgs::PR2GripperEventDetectorFeedback feedback;
00173 feedback.data = *msg;
00174
00175 pr2_gripper_sensor_msgs::PR2GripperEventDetectorResult result;
00176 result.data = *msg;
00177
00178
00179
00180 double dT = 0.1;
00181 if(feedback.data.trigger_conditions_met && (feedback.data.stamp.toSec() > action_start_time.toSec()+dT ))
00182 {
00183 ROS_INFO("Event Detector Triggered: slip %d, acceleration %d",feedback.data.slip_event,feedback.data.acceleration_event);
00184 active_goal_.setSucceeded(result);
00185 has_active_goal_ = false;
00186 }
00187
00188 active_goal_.publishFeedback(feedback);
00189 }
00190 };
00191
00192
00193 int main(int argc, char** argv)
00194 {
00195 ros::init(argc, argv, "find_contact_node");
00196 ros::NodeHandle node;
00197 Pr2GripperEventDetector jte(node);
00198
00199 ros::spin();
00200
00201 return 0;
00202 }