base_client.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, 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: Adam Leeper
00031 
00032 #include <pr2_wrappers/base_client.h>
00033 
00034 namespace pr2_wrappers {
00035 
00036 
00037 BaseClient::BaseClient(ros::NodeHandle &nh, const ros::Duration &timeout, tf::TransformListener *tfl) :
00038     delete_tfl_(false),
00039     move_base_client_("move_base", true),
00040     move_base_active_(false),
00041     last_linear_(0,0,0),
00042     last_angular_(0,0,0),
00043     timeout_(timeout),
00044     local_costmap_reset_srv_("/move_base/clear_costmaps")
00045 {
00046   nh_ = nh;
00047   tfl_ = tfl;
00048 
00049   // Create a new tfl if we didn't receive a pointer to one, and mark it for deletion.
00050   if(!tfl_)
00051   {
00052     tfl_ = new tf::TransformListener();
00053     delete_tfl_ = true;
00054   }
00055 
00056   //set up the publisher for twist messages
00057   cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("base_controller/command", 1);
00058 
00059   //set up the publisher for pose estimates
00060   pose_estimate_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1);
00061 
00062   // Subscribe to the status message for the move_base action server
00063   sub_nav_status_ = nh_.subscribe<actionlib_msgs::GoalStatusArray>("move_base/status", 10, boost::bind(&BaseClient::moveBaseStatusCB, this, _1));
00064 
00065   // Subscribe to the result message for the move_base action server
00066   sub_nav_result_ = nh_.subscribe<move_base_msgs::MoveBaseActionResult>("move_base/result", 10, boost::bind(&BaseClient::moveBaseResultCB, this, _1));
00067 
00068   // Setup the timer callback for publishing updates
00069   publish_timer_ = nh_.createTimer(ros::Duration(0.05), boost::bind(&BaseClient::publishTwist, this, _1));
00070 }
00071 
00072 BaseClient::~BaseClient(){
00073   if(delete_tfl_)
00074     delete tfl_;
00075 }
00076 
00078 bool BaseClient::hasGoal()
00079 {
00080   return move_base_active_;
00081 }
00082 
00084 //void BaseClient::queryGoalState()
00085 //{
00086 //  actionlib::SimpleClientGoalState state = move_base_client_.getState();
00087 //  move_base_active_ = ( state == state.ACTIVE || state == state.PENDING );
00088 //}
00089 
00091 void BaseClient::cancelGoals()
00092 {
00093   ROS_INFO("Canceling all move_base goals!");
00094   move_base_client_.client().cancelAllGoals();
00095 }
00096 
00098 void BaseClient::moveBaseResultCB( const move_base_msgs::MoveBaseActionResultConstPtr &result)
00099 {
00100   if(result->status.status == result->status.SUCCEEDED)
00101   {
00102     ROS_INFO("Move base reports succeeded, text: %s", result->status.text.c_str());
00103   }
00104   else if(result->status.status == result->status.PREEMPTED)
00105   {
00106     ROS_INFO("Move base reports preempted, text: %s", result->status.text.c_str());
00107   }
00108   else if(result->status.status == result->status.ABORTED)
00109   {
00110     ROS_INFO("Move base reports aborted, text: %s", result->status.text.c_str());
00111   }
00112   else if(result->status.status == result->status.REJECTED)
00113   {
00114     ROS_INFO("Move base reports goal rejected, text: %s", result->status.text.c_str());
00115   }
00116   else
00117   {
00118     ROS_INFO("Move base result was %d, text: %s", result->status.status, result->status.text.c_str());
00119   }
00120 }
00121 
00123 void BaseClient::moveBaseStatusCB( const actionlib_msgs::GoalStatusArrayConstPtr &array)
00124 {
00125   size_t num = array->status_list.size();
00126   bool is_active = false;
00127   for(size_t i = 0; i < num; i++)
00128   {
00129     actionlib_msgs::GoalStatus goal_status = array->status_list[i];
00130     if(goal_status.status == goal_status.ACTIVE || goal_status.status == goal_status.PENDING)
00131     {
00132       ROS_DEBUG("Got a move_base goal status that is active or pending!");
00133       is_active = true;
00134     }
00135     else
00136     {
00137       ROS_DEBUG("Base client for goal %zd got status update: %d", i, goal_status.status);
00138     }
00139   }
00140   move_base_active_ = is_active;
00141 }
00142 
00144 void BaseClient::applyTwist(const geometry_msgs::TwistStamped &ts)
00145 {
00146   //store the base command
00147   base_cmd_ = ts;
00148 }
00149 
00151 void BaseClient::publishTwist(const ros::TimerEvent &e)
00152 {
00153   tf::Vector3 linear;
00154   tf::Vector3 angular;
00155 
00156   tf::vector3MsgToTF(base_cmd_.twist.linear, linear);
00157   tf::vector3MsgToTF(base_cmd_.twist.angular, angular);
00158 
00159   float nu = 0.4;
00160 
00161   last_linear_ = nu*linear + (1.0 -nu)*last_linear_;
00162   last_angular_ = nu*angular + (1.0 -nu)*last_angular_;
00163 
00164   geometry_msgs::Twist cmd;
00165   tf::vector3TFToMsg(last_linear_, cmd.linear);
00166   tf::vector3TFToMsg(last_angular_, cmd.angular);
00167 
00168   // send the command if it isn't too old
00169   if(     ( (e.current_real - base_cmd_.header.stamp) < timeout_)  )
00170   {
00171     //if( linear.length() > 0.01 || angular.length() > 0.01   )
00172     //  ROS_INFO("Publishing updated twist command to base.");
00173     cmd_vel_pub_.publish(cmd);
00174   }
00175 }
00176 
00177 void BaseClient::sendNavGoal( const geometry_msgs::PoseStamped &ps)
00178 {
00179   move_base_msgs::MoveBaseGoal goal;
00180   goal.target_pose = ps;
00181   ROS_DEBUG_STREAM("Sending move_base_goal \n" << ps);
00182   // aleeper: I think we can just send the goal here and not wait for a result, because we already have
00183   //          functionality above for monitoring the state of the nav stack.
00184   move_base_client_.client().sendGoal( goal ); //, boost::bind(&BaseClient::testGripperResultCallback, this, _1, _2));
00185 }
00186 
00187 void BaseClient::sendPoseEstimate(const geometry_msgs::PoseWithCovarianceStamped &ps)
00188 {
00189   ROS_DEBUG_STREAM("Sending pose estimate. \n" << ps);
00190   pose_estimate_pub_.publish(ps);
00191 }
00192 
00193 void BaseClient::clearLocalCostmap( )
00194 {
00195 
00196 
00197   std_srvs::Empty srv;
00198   if (!local_costmap_reset_srv_.client().call(srv)) ROS_ERROR("failed to call costmap reset client");
00199 
00200 }
00201 
00202 } // namespace


pr2_wrappers
Author(s): Adam Leeper
autogenerated on Fri Jan 3 2014 11:50:39