$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 * \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 }