pr2_gripper_event_detector_action.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  * \author Joe Romano
00030  */
00031 
00032 // @author: Joe Romano
00033 // @email: joeromano@gmail.com 
00034 // @brief: pr2_gripper_event_detector_action.cpp - action server for the pr2_gripper_event_detector
00035 //         action command
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     // we will listen for messages on "state" and send messages on "find_contact"
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     // Aborts the active goal if the controller does not appear to be active.
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         // Marks the current goal as aborted.
00117         active_goal_.setAborted();
00118         has_active_goal_ = false;
00119       }
00120     }
00121   }
00122 
00123   void goalCB(GoalHandle gh)
00124   {
00125     // Cancels the currently active goal.
00126     if (has_active_goal_)
00127     {
00128       // Marks the current goal as canceled.
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     // Sends command to trigger controller
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       // Stops the controller
00151       if (last_controller_state_)
00152       {
00153 
00154       }
00155 
00156       // Marks the current goal as canceled.
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     // check if our success-condition is met, but only listen to feedback messages
00179     // that were published some dT after the action was started
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 }


pr2_gripper_sensor_action
Author(s): Joe Romano
autogenerated on Thu Jun 6 2019 18:02:10