$search
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 }