$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_slipServo_action.cpp - action server for the 00035 // pr2_gripper_slipServo action command 00036 00037 #include "ros/ros.h" 00038 00039 #include <actionlib/server/action_server.h> 00040 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoAction.h> 00041 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoData.h> 00042 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoCommand.h> 00043 #include <std_srvs/Empty.h> 00044 00045 class Pr2GripperForceServo 00046 { 00047 private: 00048 typedef actionlib::ActionServer<pr2_gripper_sensor_msgs::PR2GripperForceServoAction> GAS; 00049 typedef GAS::GoalHandle GoalHandle; 00050 public: 00051 Pr2GripperForceServo(ros::NodeHandle &n) : 00052 node_(n), 00053 action_server_(node_, "force_servo", 00054 boost::bind(&Pr2GripperForceServo::goalCB, this, _1), 00055 boost::bind(&Pr2GripperForceServo::cancelCB, this, _1)), 00056 has_active_goal_(false) 00057 { 00058 ros::NodeHandle pn("~"); 00059 00060 // we will listen for messages on "state" 00061 pub_controller_command_ = 00062 node_.advertise<pr2_gripper_sensor_msgs::PR2GripperForceServoCommand>("force_servo", 1); 00063 sub_controller_state_ = 00064 node_.subscribe("force_servo_state", 1, &Pr2GripperForceServo::controllerStateCB, this); 00065 00066 watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &Pr2GripperForceServo::watchdog, this); 00067 00068 } 00069 00070 ~Pr2GripperForceServo() 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 bool firstDrop; 00087 GoalHandle active_goal_; 00088 ros::Time goal_received_; 00089 00090 double goal_threshold_; 00091 double stall_velocity_threshold_; 00092 double stall_timeout_; 00093 ros::Time last_movement_time_; 00094 ros::Time action_start_time; 00095 00096 00097 00098 void watchdog(const ros::TimerEvent &e) 00099 { 00100 ros::Time now = ros::Time::now(); 00101 00102 // Aborts the active goal if the controller does not appear to be active. 00103 if (has_active_goal_) 00104 { 00105 bool should_abort = false; 00106 if (!last_controller_state_) 00107 { 00108 should_abort = true; 00109 ROS_WARN("Aborting goal because we have never heard a controller state message."); 00110 } 00111 else if ((now - last_controller_state_->stamp) > ros::Duration(5.0)) 00112 { 00113 should_abort = true; 00114 ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds", 00115 (now - last_controller_state_->stamp).toSec()); 00116 } 00117 00118 if (should_abort) 00119 { 00120 // Marks the current goal as aborted. 00121 active_goal_.setAborted(); 00122 has_active_goal_ = false; 00123 } 00124 } 00125 } 00126 00127 void goalCB(GoalHandle gh) 00128 { 00129 // Cancels the currently active goal. 00130 if (has_active_goal_) 00131 { 00132 // Marks the current goal as canceled. 00133 active_goal_.setCanceled(); 00134 has_active_goal_ = false; 00135 } 00136 00137 gh.setAccepted(); 00138 active_goal_ = gh; 00139 has_active_goal_ = true; 00140 firstDrop = true; // set our first drop flag 00141 goal_received_ = ros::Time::now(); 00142 00143 // Sends the command along to the controller. 00144 pub_controller_command_.publish(active_goal_.getGoal()->command); 00145 00146 last_movement_time_ = ros::Time::now(); 00147 action_start_time = ros::Time::now(); 00148 } 00149 00150 void cancelCB(GoalHandle gh) 00151 { 00152 00153 if (active_goal_ == gh) 00154 { 00155 // stop the real-time motor control 00156 std_srvs::Empty::Request req; 00157 std_srvs::Empty::Response resp; 00158 if(ros::service::exists("stop_motor_output",true)) 00159 { 00160 ROS_INFO("Stopping Motor Output"); 00161 ros::service::call("stop_motor_output",req,resp); 00162 } 00163 00164 // Marks the current goal as canceled. 00165 active_goal_.setCanceled(); 00166 has_active_goal_ = false; 00167 } 00168 } 00169 00170 00171 pr2_gripper_sensor_msgs::PR2GripperForceServoDataConstPtr last_controller_state_; 00172 void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperForceServoDataConstPtr &msg) 00173 { 00174 last_controller_state_ = msg; 00175 ros::Time now = ros::Time::now(); 00176 00177 if (!has_active_goal_) 00178 return; 00179 00180 pr2_gripper_sensor_msgs::PR2GripperForceServoGoal goal; 00181 goal.command = active_goal_.getGoal()->command; 00182 00183 pr2_gripper_sensor_msgs::PR2GripperForceServoFeedback feedback; 00184 feedback.data = *msg; 00185 00186 pr2_gripper_sensor_msgs::PR2GripperForceServoResult result; 00187 result.data = *msg; 00188 00189 00190 // do not check until some dT has expired from message start 00191 double dT = 0.1; 00192 if(feedback.data.stamp.toSec() < action_start_time.toSec()+dT ){} 00193 00194 // if we are actually in a force_servo control state 00195 else if(feedback.data.rtstate.realtime_controller_state == 4) 00196 { 00197 // if the force servo achieved its goal 00198 if ( feedback.data.force_achieved ) 00199 { 00200 active_goal_.setSucceeded(result); 00201 has_active_goal_ = false; 00202 } 00203 00204 } 00205 00206 else 00207 has_active_goal_ = false; 00208 00209 active_goal_.publishFeedback(feedback); 00210 } 00211 }; 00212 00213 00214 int main(int argc, char** argv) 00215 { 00216 ros::init(argc, argv, "force_servo_node"); 00217 ros::NodeHandle node; 00218 Pr2GripperForceServo jte(node); 00219 00220 ros::spin(); 00221 00222 return 0; 00223 }