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


pr2_gripper_action
Author(s): Stuart Glaser
autogenerated on Fri Jan 3 2014 11:42:06