pr2_doors_actions: action_unlatch_handle.cpp Source File

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Wim Meeussen */
00036 
00037 #include "pr2_doors_actions/action_unlatch_handle.h"
00038 #include "pr2_doors_common/door_functions.h"
00039 
00040 
00041 using namespace tf;
00042 using namespace KDL;
00043 using namespace ros;
00044 using namespace std;
00045 using namespace door_handle_detector;
00046 using namespace pr2_doors_common;
00047 using namespace actionlib;
00048 
00049 static const string fixed_frame = "odom_combined";
00050 
00051 
00052 UnlatchHandleAction::UnlatchHandleAction(tf::TransformListener& tf) :
00053   tf_(tf),
00054   action_server_(ros::NodeHandle(), 
00055                  "unlatch_handle", 
00056                  boost::bind(&UnlatchHandleAction::execute, this, _1))
00057 {
00058   tff_pub_ = node_.advertise<tff_controller::TaskFrameFormalism>("r_arm_cartesian_tff_controller/command", 10);
00059 };
00060 
00061 
00062 
00063 UnlatchHandleAction::~UnlatchHandleAction()
00064 {};
00065 
00066 
00067 
00068 void UnlatchHandleAction::execute(const  door_msgs::DoorGoalConstPtr& goal)
00069 { 
00070   ROS_INFO("UnlatchHandleAction: execute");
00071 
00072   // push/pull dir
00073   double door_dir = getDoorDir(goal->door);
00074   double handle_dir = getHandleDir(goal->door);
00075 
00076   // stop
00077   tff_stop_.mode.linear.x = tff_stop_.FORCE;
00078   tff_stop_.mode.linear.y = tff_stop_.FORCE;
00079   tff_stop_.mode.linear.z = tff_stop_.FORCE;
00080   tff_stop_.mode.angular.x = tff_stop_.FORCE;
00081   tff_stop_.mode.angular.y = tff_stop_.FORCE;
00082   tff_stop_.mode.angular.z = tff_stop_.FORCE;
00083   
00084   tff_stop_.value.linear.x = 0.0;
00085   tff_stop_.value.linear.y = 0.0;
00086   tff_stop_.value.linear.z = 0.0;
00087   tff_stop_.value.angular.x = 0.0;
00088   tff_stop_.value.angular.y = 0.0;
00089   tff_stop_.value.angular.z = 0.0;
00090   
00091   // turn handle
00092   tff_handle_.mode.linear.x = tff_handle_.FORCE;
00093   tff_handle_.mode.linear.y = tff_handle_.FORCE;
00094   tff_handle_.mode.linear.z = tff_handle_.FORCE;
00095   tff_handle_.mode.angular.x = tff_handle_.FORCE;
00096   tff_handle_.mode.angular.y = tff_handle_.FORCE;
00097   tff_handle_.mode.angular.z = tff_handle_.POSITION;
00098   
00099   tff_handle_.value.linear.x = 20.0 * door_dir;
00100   tff_handle_.value.linear.y = 0.0;
00101   tff_handle_.value.linear.z = 0.0;
00102   tff_handle_.value.angular.x = 0.0;
00103   tff_handle_.value.angular.y = 0.0;
00104   tff_handle_.value.angular.z = 0.0;
00105   
00106 
00107   // start monitoring tf position
00108   Subscriber sub = node_.subscribe("r_arm_cartesian_tff_controller/state/position", 1,  &UnlatchHandleAction::tffCallback, this);
00109   Duration timeout = Duration().fromSec(3.0);
00110   Duration poll = Duration().fromSec(0.1);
00111   Time start_time = ros::Time::now();
00112   tff_state_received_ = false;
00113   ROS_INFO("UnlatchHandleAction: waiting to receive tff state");
00114   while (!tff_state_received_){
00115     if (start_time + timeout < ros::Time::now()){
00116       ROS_ERROR("UnlatchHandleAction: failed to receive tff state");
00117       action_server_.setPreempted();
00118       return;
00119     }
00120     poll.sleep();
00121   }
00122 
00123   double sleep_time = 0.1;
00124   ros::Duration wait_after_door_moved = Duration().fromSec(1.0);
00125   ros::Time time_door_moved;
00126   action_result_.door = goal->door;
00127 
00128   // turn handle and push door, until door moves forward
00129   ROS_INFO("UnlatchHandleAction: start turning handle");
00130   tff_handle_.value.angular.x = handle_dir * 0.5;
00131   while (time_door_moved == ros::Time() || ros::Time::now() < time_door_moved + wait_after_door_moved){
00132     Duration(sleep_time).sleep();
00133     boost::mutex::scoped_lock lock(tff_mutex_);
00134 
00135     // increase torque to turn handle until door is opened 5 cm
00136     if (fabs(tff_state_.linear.x) < 0.05)
00137       tff_handle_.value.angular.x += handle_dir * 0.5 * sleep_time; // add 0.5 Nm per second
00138     else{
00139       tff_handle_.value.angular.x = 0;
00140       if (time_door_moved == Time())
00141         time_door_moved = ros::Time::now();
00142     }
00143     tff_pub_.publish(tff_handle_);
00144 
00145     // detect when gripper is not on hanlde
00146     if (fabs(tff_state_.angular.x) > M_PI/2.0 || fabs(tff_state_.linear.y) > 0.1 || fabs(tff_state_.linear.z) > 0.1){
00147       //fabs(tff_state_.angular.y) > M_PI/8.0 || fabs(tff_state_.angular.z) > M_PI/8.0){
00148       tff_pub_.publish(tff_stop_);
00149       lock.unlock();
00150       ROS_ERROR("UnlatchHandleAction: Gripper was not on door handle");
00151       action_server_.setPreempted();
00152       return;
00153     }
00154 
00155     // detect when door is locked
00156     if (fabs(tff_handle_.value.angular.x) > 3.5 && fabs(tff_state_.angular.x) < M_PI/6.0){
00157       tff_pub_.publish(tff_stop_);
00158       lock.unlock();
00159       ROS_INFO("UnlatchHandleAction: Door is locked");
00160       action_result_.door.latch_state = door_msgs::Door::LOCKED;
00161       action_server_.setSucceeded(action_result_);  
00162       return;
00163     }
00164 
00165     // check for preemption
00166     if (action_server_.isPreemptRequested()){
00167       ROS_ERROR("UnlatchHandleAction: preempted");
00168       action_server_.setPreempted();
00169       return;
00170     }
00171   }
00172   
00173   // finished
00174   action_result_.door.latch_state = door_msgs::Door::UNLATCHED;
00175   action_server_.setSucceeded(action_result_);  
00176 }
00177 
00178 
00179 void UnlatchHandleAction::tffCallback(const TffConstPtr& tff)
00180 {
00181   boost::mutex::scoped_lock lock(tff_mutex_);
00182   tff_state_ = *tff;
00183   tff_state_received_ = true;
00184 }
00185