pr2_gripper_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 
00030 // Author: Stuart Glaser
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 
00039 class Pr2GripperAction
00040 {
00041 private:
00042   typedef actionlib::ActionServer<pr2_controllers_msgs::Pr2GripperCommandAction> GAS;
00043   typedef GAS::GoalHandle GoalHandle;
00044 public:
00045   Pr2GripperAction(ros::NodeHandle &n) :
00046     node_(n),
00047     action_server_(node_, "gripper_action",
00048                    boost::bind(&Pr2GripperAction::goalCB, this, _1),
00049                    boost::bind(&Pr2GripperAction::cancelCB, this, _1),
00050                    false),
00051     has_active_goal_(false)
00052   {
00053     ros::NodeHandle pn("~");
00054 
00055     pn.param("goal_threshold", goal_threshold_, 0.01);
00056     pn.param("stall_velocity_threshold", stall_velocity_threshold_, 1e-6);
00057     pn.param("stall_timeout", stall_timeout_, 0.1);
00058 
00059     pub_controller_command_ =
00060       node_.advertise<pr2_controllers_msgs::Pr2GripperCommand>("command", 1);
00061     sub_controller_state_ =
00062       node_.subscribe("state", 1, &Pr2GripperAction::controllerStateCB, this);
00063 
00064     watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &Pr2GripperAction::watchdog, this);
00065     action_server_.start();
00066   }
00067 
00068   ~Pr2GripperAction()
00069   {
00070     pub_controller_command_.shutdown();
00071     sub_controller_state_.shutdown();
00072     watchdog_timer_.stop();
00073   }
00074 
00075 private:
00076 
00077   ros::NodeHandle node_;
00078   GAS action_server_;
00079   ros::Publisher pub_controller_command_;
00080   ros::Subscriber sub_controller_state_;
00081   ros::Timer watchdog_timer_;
00082 
00083   bool has_active_goal_;
00084   GoalHandle active_goal_;
00085   ros::Time goal_received_;
00086 
00087   double min_error_seen_;
00088   double goal_threshold_;
00089   double stall_velocity_threshold_;
00090   double stall_timeout_;
00091   ros::Time last_movement_time_;
00092 
00093   void watchdog(const ros::TimerEvent &e)
00094   {
00095     ros::Time now = ros::Time::now();
00096 
00097     // Aborts the active goal if the controller does not appear to be active.
00098     if (has_active_goal_)
00099     {
00100       bool should_abort = false;
00101       if (!last_controller_state_)
00102       {
00103         should_abort = true;
00104         ROS_WARN("Aborting goal because we have never heard a controller state message.");
00105       }
00106       else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
00107       {
00108         should_abort = true;
00109         ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
00110                  (now - last_controller_state_->header.stamp).toSec());
00111       }
00112 
00113       if (should_abort)
00114       {
00115         // Marks the current goal as aborted.
00116         active_goal_.setAborted();
00117         has_active_goal_ = false;
00118       }
00119     }
00120   }
00121 
00122   void goalCB(GoalHandle gh)
00123   {
00124     // Cancels the currently active goal.
00125     if (has_active_goal_)
00126     {
00127       // Marks the current goal as canceled.
00128       active_goal_.setCanceled();
00129       has_active_goal_ = false;
00130     }
00131 
00132     gh.setAccepted();
00133     active_goal_ = gh;
00134     has_active_goal_ = true;
00135     goal_received_ = ros::Time::now();
00136     min_error_seen_ = 1e10;
00137 
00138     // Sends the command along to the controller.
00139     pub_controller_command_.publish(active_goal_.getGoal()->command);
00140     last_movement_time_ = ros::Time::now();
00141   }
00142 
00143   void cancelCB(GoalHandle gh)
00144   {
00145     if (active_goal_ == gh)
00146     {
00147       // Stops the controller.
00148       if (last_controller_state_)
00149       {
00150         pr2_controllers_msgs::Pr2GripperCommand stop;
00151         stop.position = last_controller_state_->process_value;
00152         stop.max_effort = 0.0;
00153         pub_controller_command_.publish(stop);
00154       }
00155 
00156       // Marks the current goal as canceled.
00157       active_goal_.setCanceled();
00158       has_active_goal_ = false;
00159     }
00160   }
00161 
00162 
00163 
00164   pr2_controllers_msgs::JointControllerStateConstPtr last_controller_state_;
00165   void controllerStateCB(const pr2_controllers_msgs::JointControllerStateConstPtr &msg)
00166   {
00167     last_controller_state_ = msg;
00168     ros::Time now = ros::Time::now();
00169 
00170     if (!has_active_goal_)
00171       return;
00172 
00173     // Ensures that the controller is tracking my setpoint.
00174     if (fabs(msg->set_point - active_goal_.getGoal()->command.position) > goal_threshold_)
00175     {
00176       if (now - goal_received_ < ros::Duration(1.0))
00177       {
00178         return;
00179       }
00180       else
00181       {
00182         ROS_ERROR("Cancelling goal: Controller is trying to achieve a different setpoint.");
00183         active_goal_.setCanceled();
00184         has_active_goal_ = false;
00185       }
00186     }
00187 
00188 
00189     pr2_controllers_msgs::Pr2GripperCommandFeedback feedback;
00190     feedback.position = msg->process_value;
00191     feedback.effort = msg->command;
00192     feedback.reached_goal = false;
00193     feedback.stalled = false;
00194 
00195     pr2_controllers_msgs::Pr2GripperCommandResult result;
00196     result.position = msg->process_value;
00197     result.effort = msg->command;
00198     result.reached_goal = false;
00199     result.stalled = false;
00200 
00201     if (fabs(msg->process_value - active_goal_.getGoal()->command.position) < goal_threshold_)
00202     {
00203       feedback.reached_goal = true;
00204 
00205       result.reached_goal = true;
00206       active_goal_.setSucceeded(result);
00207       has_active_goal_ = false;
00208     }
00209     else
00210     {
00211       // Determines if the gripper has stalled.
00212       if (fabs(msg->process_value_dot) > stall_velocity_threshold_)
00213       {
00214         last_movement_time_ = ros::Time::now();
00215       }
00216       else if ((ros::Time::now() - last_movement_time_).toSec() > stall_timeout_ &&
00217                active_goal_.getGoal()->command.max_effort != 0.0)
00218       {
00219         feedback.stalled = true;
00220 
00221         result.stalled = true;
00222         active_goal_.setAborted(result);
00223         has_active_goal_ = false;
00224       }
00225     }
00226     active_goal_.publishFeedback(feedback);
00227   }
00228 };
00229 
00230 
00231 int main(int argc, char** argv)
00232 {
00233   ros::init(argc, argv, "gripper_action_node");
00234   ros::NodeHandle node;
00235   Pr2GripperAction jte(node);
00236 
00237   ros::spin();
00238 
00239   return 0;
00240 }


pr2_gripper_action
Author(s): Stuart Glaser
autogenerated on Thu Aug 27 2015 14:22:57