pr2_gripper_findContact_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_findContact_action.cpp - action server for sending
00035 //         findContact commands to the roobt
00036 
00037 #include "ros/ros.h"
00038 
00039 #include <actionlib/server/action_server.h>
00040 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactAction.h>
00041 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactData.h>
00042 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactCommand.h>
00043 #include <std_srvs/Empty.h>
00044 
00045 class Pr2GripperFindContact
00046 {
00047 private:
00048   typedef actionlib::ActionServer<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> GAS;
00049   typedef GAS::GoalHandle GoalHandle;
00050 public:
00051   Pr2GripperFindContact(ros::NodeHandle &n) :
00052     node_(n),
00053     action_server_(node_, "find_contact",
00054                    boost::bind(&Pr2GripperFindContact::goalCB, this, _1),
00055                    boost::bind(&Pr2GripperFindContact::cancelCB, this, _1)),
00056     has_active_goal_(false)
00057   {
00058     ros::NodeHandle pn("~");
00059 
00060     // we will listen for messages on "state" and send messages on "find_contact"
00061     pub_controller_command_ =
00062       node_.advertise<pr2_gripper_sensor_msgs::PR2GripperFindContactCommand>("find_contact", 1);
00063     sub_controller_state_ =
00064       node_.subscribe("contact_state", 1, &Pr2GripperFindContact::controllerStateCB, this);
00065 
00066     watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &Pr2GripperFindContact::watchdog, this);
00067 
00068   }
00069 
00070   ~Pr2GripperFindContact()
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   GoalHandle active_goal_;
00087   ros::Time goal_received_;
00088 
00089   double stall_timeout_;
00090   ros::Time last_movement_time_;
00091   ros::Time action_start_time;
00092 
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_->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_->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 
00138     // update our zero values for 0.25 seconds
00139     if(active_goal_.getGoal()->command.zero_fingertip_sensors)
00140     {
00141       std_srvs::Empty::Request req;
00142       std_srvs::Empty::Response resp;
00143       if(ros::service::exists("zero_fingertip_sensors",true))
00144       {
00145         ROS_INFO("updating zeros!");
00146         ros::service::call("zero_fingertip_sensors",req,resp);
00147       }
00148     }
00149     
00150     // Sends the command along to the controller.
00151     pub_controller_command_.publish(active_goal_.getGoal()->command);
00152     
00153     last_movement_time_ = ros::Time::now();
00154     action_start_time = ros::Time::now();
00155   }
00156 
00157   void cancelCB(GoalHandle gh)
00158   {
00159     
00160     if (active_goal_ == gh)
00161     {
00162       // stop the real-time motor control
00163       std_srvs::Empty::Request req;
00164       std_srvs::Empty::Response resp;
00165       if(ros::service::exists("stop_motor_output",true))
00166       {
00167         ROS_INFO("Stopping Motor Output");
00168         ros::service::call("stop_motor_output",req,resp);
00169       }
00170 
00171       // Marks the current goal as canceled.
00172       active_goal_.setCanceled();
00173       has_active_goal_ = false;
00174     }
00175   }
00176 
00177 
00178   pr2_gripper_sensor_msgs::PR2GripperFindContactDataConstPtr last_controller_state_;
00179   void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactDataConstPtr &msg)
00180   {
00181     last_controller_state_ = msg;
00182     ros::Time now = ros::Time::now();
00183 
00184     if (!has_active_goal_)
00185       return;
00186 
00187     pr2_gripper_sensor_msgs::PR2GripperFindContactGoal goal;
00188     goal.command = active_goal_.getGoal()->command;
00189 
00190     pr2_gripper_sensor_msgs::PR2GripperFindContactFeedback feedback;
00191     feedback.data = *msg;
00192 
00193     pr2_gripper_sensor_msgs::PR2GripperFindContactResult result;
00194     result.data = *msg;
00195 
00196     // do not check until some dT has expired from message start
00197     double dT = 0.2;
00198     if(feedback.data.stamp.toSec() < action_start_time.toSec()+dT ){}
00199 
00200 
00201     // if we are actually in a find_contact control state or positoin control state
00202     else if(feedback.data.rtstate.realtime_controller_state == 5 || feedback.data.rtstate.realtime_controller_state == 3)
00203     {
00204       if(feedback.data.contact_conditions_met)
00205       {
00206         active_goal_.setSucceeded(result);
00207         has_active_goal_ = false;
00208       }
00209     }
00210 
00211     else
00212       has_active_goal_ = false;
00213 
00214     active_goal_.publishFeedback(feedback);
00215   }
00216 };
00217 
00218 
00219 int main(int argc, char** argv)
00220 {
00221   ros::init(argc, argv, "find_contact_node");
00222   ros::NodeHandle node;
00223   Pr2GripperFindContact jte(node);
00224 
00225   ros::spin();
00226 
00227   return 0;
00228 }


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