$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 00030 // Author: Stu Glaser (modified by Joe Romano from pr2_gripper_action) 00031 00032 #include "ros/ros.h" 00033 00034 #include <actionlib/server/action_server.h> 00035 #include <pr2_controllers_msgs/JointControllerState.h> 00036 #include <pr2_controllers_msgs/Pr2GripperCommand.h> 00037 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h> 00038 #include <std_srvs/Empty.h> 00039 00040 class Pr2GripperAction 00041 { 00042 private: 00043 typedef actionlib::ActionServer<pr2_controllers_msgs::Pr2GripperCommandAction> GAS; 00044 typedef GAS::GoalHandle GoalHandle; 00045 00046 public: 00047 Pr2GripperAction(ros::NodeHandle &n) : 00048 node_(n), 00049 action_server_(node_, "gripper_action", 00050 boost::bind(&Pr2GripperAction::goalCB, this, _1), 00051 boost::bind(&Pr2GripperAction::cancelCB, this, _1)), 00052 has_active_goal_(false) 00053 { 00054 ros::NodeHandle pn("~"); 00055 00056 //pn.param("goal_threshold", goal_threshold_, 0.01); 00057 pn.param("stall_velocity_threshold", stall_velocity_threshold_, 1e-6); 00058 pn.param("stall_timeout", stall_timeout_, 0.1); 00059 00060 // we will listen for messages on "state" and send messages on "command" 00061 pub_controller_command_ = 00062 node_.advertise<pr2_controllers_msgs::Pr2GripperCommand>("command", 1); 00063 sub_controller_state_ = 00064 node_.subscribe("state", 1, &Pr2GripperAction::controllerStateCB, this); 00065 00066 watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &Pr2GripperAction::watchdog, this); 00067 } 00068 00069 ~Pr2GripperAction() 00070 { 00071 pub_controller_command_.shutdown(); 00072 sub_controller_state_.shutdown(); 00073 watchdog_timer_.stop(); 00074 } 00075 00076 private: 00077 00078 ros::NodeHandle node_; 00079 GAS action_server_; 00080 ros::Publisher pub_controller_command_; 00081 ros::Subscriber sub_controller_state_; 00082 ros::Timer watchdog_timer_; 00083 00084 bool has_active_goal_; 00085 GoalHandle active_goal_; 00086 ros::Time goal_received_; 00087 00088 double min_error_seen_; 00089 double goal_threshold_; 00090 double stall_velocity_threshold_; 00091 double stall_timeout_; 00092 ros::Time last_movement_time_; 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_->header.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_->header.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 min_error_seen_ = 1e10; 00138 00139 // Sends the command along to the controller. 00140 pub_controller_command_.publish(active_goal_.getGoal()->command); 00141 last_movement_time_ = ros::Time::now(); 00142 } 00143 00144 void cancelCB(GoalHandle gh) 00145 { 00146 if (active_goal_ == gh) 00147 { 00148 /* 00149 // Stops the controller. 00150 if (last_controller_state_) 00151 { 00152 pr2_controllers_msgs::Pr2GripperCommand stop; 00153 stop.position = last_controller_state_->process_value; 00154 stop.max_effort = 0.0; 00155 pub_controller_command_.publish(stop); 00156 } 00157 */ 00158 // stop the real-time motor control 00159 std_srvs::Empty::Request req; 00160 std_srvs::Empty::Response resp; 00161 if(ros::service::exists("stop_motor_output",true)) 00162 { 00163 ROS_INFO("Stopping Motor Output"); 00164 ros::service::call("stop_motor_output",req,resp); 00165 } 00166 00167 // Marks the current goal as canceled. 00168 active_goal_.setCanceled(); 00169 has_active_goal_ = false; 00170 } 00171 } 00172 00173 00174 00175 pr2_controllers_msgs::JointControllerStateConstPtr last_controller_state_; 00176 void controllerStateCB(const pr2_controllers_msgs::JointControllerStateConstPtr &msg) 00177 { 00178 last_controller_state_ = msg; 00179 ros::Time now = ros::Time::now(); 00180 00181 if (!has_active_goal_) 00182 return; 00183 00184 // grab the goal threshold off the param server 00185 if(!node_.getParam("position_servo_position_tolerance", goal_threshold_)) 00186 ROS_ERROR("No position_servo_position_tolerance given in namespace: '%s')", node_.getNamespace().c_str()); 00187 00188 00189 // Ensures that the controller is tracking my setpoint. 00190 if (fabs(msg->set_point - active_goal_.getGoal()->command.position) > stall_velocity_threshold_) 00191 { 00192 if (now - goal_received_ < ros::Duration(1.0)) 00193 { 00194 return; 00195 } 00196 else 00197 { 00198 ROS_ERROR("Cancelling goal: Controller is trying to achieve a different setpoint."); 00199 active_goal_.setCanceled(); 00200 has_active_goal_ = false; 00201 } 00202 } 00203 00204 00205 pr2_controllers_msgs::Pr2GripperCommandFeedback feedback; 00206 feedback.position = msg->process_value; 00207 feedback.effort = msg->command; 00208 feedback.reached_goal = false; 00209 feedback.stalled = false; 00210 00211 pr2_controllers_msgs::Pr2GripperCommandResult result; 00212 result.position = msg->process_value; 00213 result.effort = msg->command; 00214 result.reached_goal = false; 00215 result.stalled = false; 00216 00217 // check if we've reached the goal 00218 if (fabs(msg->process_value - active_goal_.getGoal()->command.position) < goal_threshold_) 00219 { 00220 feedback.reached_goal = true; 00221 00222 result.reached_goal = true; 00223 active_goal_.setSucceeded(result); 00224 has_active_goal_ = false; 00225 } 00226 else 00227 { 00228 // Determines if the gripper has stalled. 00229 if (fabs(msg->process_value_dot) > stall_velocity_threshold_) 00230 { 00231 last_movement_time_ = ros::Time::now(); 00232 } 00233 else if ((ros::Time::now() - last_movement_time_).toSec() > stall_timeout_ && 00234 active_goal_.getGoal()->command.max_effort != 0.0) 00235 { 00236 feedback.stalled = true; 00237 00238 result.stalled = true; 00239 active_goal_.setAborted(result); 00240 has_active_goal_ = false; 00241 } 00242 } 00243 active_goal_.publishFeedback(feedback); 00244 } 00245 }; 00246 00247 00248 int main(int argc, char** argv) 00249 { 00250 ros::init(argc, argv, "gripper_action_node"); 00251 ros::NodeHandle node; 00252 Pr2GripperAction jte(node); 00253 00254 ros::spin(); 00255 00256 return 0; 00257 }