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


pr2_gripper_sensor_action
Author(s): Joe Romano
autogenerated on Thu Jun 6 2019 18:02:10