$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_findContact_action.cpp - action server for sending 00035 // findContact commands to the roobt 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 // we will listen for messages on "state" and send messages on "find_contact" 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 // 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 // update our zero values for 0.25 seconds 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 // Sends the command along to the controller. 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 // stop the real-time motor control 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 // Marks the current goal as canceled. 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 // do not check until some dT has expired from message start 00197 double dT = 0.2; 00198 if(feedback.data.stamp.toSec() < action_start_time.toSec()+dT ){} 00199 00200 00201 // if we are actually in a find_contact control state or positoin control state 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 }