cart_local_planner: cart_local_planner.h 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 the 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 
00042 #ifndef CART_LOCAL_PLANNER_H_
00043 #define CART_LOCAL_PLANNER_H_
00044 
00045 #include <cart_local_planner/sbpl_subscriber.h>
00046 #include <ros/ros.h>
00047 #include <tf/tf.h>
00048 #include <tf/transform_datatypes.h>
00049 #include <tf/transform_listener.h>
00050 #include <tf/transform_broadcaster.h>
00051 #include <nav_core/base_local_planner.h>
00052 #include <costmap_2d/costmap_2d_ros.h>
00053 #include <geometry_msgs/PoseStamped.h>
00054 #include <geometry_msgs/PolygonStamped.h>
00055 #include <geometry_msgs/Twist.h>
00056 #include <geometry_msgs/Pose2D.h>
00057 #include <nav_msgs/Odometry.h>
00058 #include <cart_pushing_msgs/RobotCartPath.h>
00059 #include <cart_pushing_msgs/RobotCartConfiguration.h>
00060 #include <boost/scoped_ptr.hpp>
00061 #include <costmap_trajectory_checker/costmap_trajectory_checker.h>
00062 #include <manipulation_transforms/manipulation_transforms.h>
00063 
00064 namespace cart_local_planner {
00065 
00066 typedef struct {
00067   double x_min;
00068   double x_max;
00069   double y_min;
00070   double y_max;
00071   double t_min;
00072   double t_max;
00073 } pose_range_2D;
00074 
00076 const geometry_msgs::Twist operator+(const geometry_msgs::Twist& t1, const geometry_msgs::Twist& t2);
00077 const geometry_msgs::Twist operator-(const geometry_msgs::Twist& t1, const geometry_msgs::Twist& t2);
00078 double mag(geometry_msgs::Twist &t);
00079 double mag(geometry_msgs::Pose2D &p);
00080 
00081 class CartLocalPlanner: public nav_core::BaseLocalPlanner {
00082 public:
00083   CartLocalPlanner();
00084   void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);
00085   bool isGoalReached();
00086   bool setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan);
00087   bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
00088 
00089 protected:
00094   virtual void initialization_extras();
00095 
00096   /************
00097    * PLANNING *
00098    ************/
00103   virtual void setControlMode();
00104 
00111   virtual void controlModeAction();
00112 
00116   virtual void setGoalPoses();
00117 
00122   virtual void filterTwistsCombined(int filter_options);
00123 
00128   virtual bool checkTwists();
00129 
00134   virtual bool checkTwistsMonotonic();
00135 
00136   /************************
00137    * SUPPORTING UTILITIES *
00138    ************************/
00145   int setCartGoalFromWaypoint(double min_dist, double min_x_component);
00146 
00147   // Set the current goal of the cart/robot and update the error_ variables accordingly given current actual_ pose
00148   void setCartPoseGoal (const tf::Stamped<tf::Pose>& goal);
00149   void setRobotPoseGoal (const tf::Stamped<tf::Pose>& goal);
00150 
00151   
00153   double baseTwistFromError();
00154 
00156   double cartTwistFromError();
00157 
00159   void setYawFromVec(const tf::Pose& pose1, const tf::Pose& pose2,
00160                      tf::Pose& res);
00161 
00163   bool transformGlobalPlan(const tf::TransformListener& tf,
00164                            const std::vector<geometry_msgs::PoseStamped>& global_plan,
00165                            const costmap_2d::Costmap2DROS& costmap,
00166                            const std::string& global_frame, std::vector<
00167                            geometry_msgs::PoseStamped>& transformed_plan);
00168 
00170   std::vector<geometry_msgs::Point> transformFootprint(const geometry_msgs::PolygonStamped& poly) const;
00171 
00173   void freeze();
00174 
00176   static void scaleTwist2D(geometry_msgs::Twist& t, double scale);
00177 
00179   geometry_msgs::Twist mapBaseTwistToCart(
00180                                           const geometry_msgs::Twist &twist_base);
00181 
00183   void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
00184 
00186   void invalidPoseCallback (const std_msgs::Empty::ConstPtr& msg);
00187 
00188   // Publishers for pose and twist messages in 2D (mainly for debugging and plotting)
00189   void publishDebugPose(geometry_msgs::Pose &p);
00190   void publishDebugTwist(geometry_msgs::Twist &t);
00191   void publishDebugPose(tf::Pose &p);
00192 
00193   /****************
00194    * DATA MEMBERS *
00195    ****************/
00196   enum FILTER_OPTIONS {
00197     GLOBAL_SCALING = 0x1,
00198     CART_ERR_SCALING = GLOBAL_SCALING << 1,
00199     ALL = 0xffff
00200   };
00201 
00202   enum CONTROL_MODE {
00203     REGULAR
00204   };
00205 
00206   // The control mode for the computeVelocityCommands switch
00207   enum CONTROL_MODE control_mode_;
00208 
00209   tf::TransformListener* tf_;
00210   tf::TransformBroadcaster tb_;
00211   ros::NodeHandle nh_;
00212 
00213   costmap_2d::Costmap2DROS* costmap_ros_;
00214 
00215   CostmapTrajectoryChecker robot_collision_checker_;
00216   CostmapTrajectoryChecker cart_collision_checker_;
00217 
00218   ros::Publisher vel_pub_;
00219   ros::Subscriber odom_sub_, invalid_pose_sub_;
00220   nav_msgs::Odometry base_odom_;
00221   ros::Time goal_reached_time_;
00222   std::vector<geometry_msgs::PoseStamped> global_plan_;
00223   boost::mutex odom_lock_, invalid_pose_mutex_;
00224 
00225   // Transforms for mapping twists around
00226   tf::Stamped<tf::Pose> robot_pose_actual_;     
00227   tf::Stamped<tf::Pose> cart_pose_actual_;      
00229   // Transforms for holding goal poses
00230   tf::Stamped<tf::Pose> robot_pose_goal_;       
00231   tf::Stamped<tf::Pose> cart_pose_goal_;                
00233   // Containers for robot and cart error
00234   tf::Pose robot_pose_error_;   
00235   geometry_msgs::Pose2D cart_pose_error_;                       
00237   // Command twists
00238   geometry_msgs::Twist *twist_base_; 
00239   geometry_msgs::TwistStamped twist_cart_; 
00241   bool initialized_;
00242   double K_trans_base_, K_rot_base_; 
00243   double K_trans_cart_, K_rot_cart_; 
00244   int num_traj_steps_; 
00245   double dt_; 
00246   unsigned int current_waypoint_; 
00247   double tolerance_trans_, tolerance_rot_, tolerance_timeout_; 
00248   double trans_stopped_velocity_, rot_stopped_velocity_; 
00250   // Whether we directly subscribe to sbpl plan rather than getting it from move base
00251   bool subscribe_sbpl_plan_;
00252 
00253   // For controlling cart position w/ arms
00254   ros::Publisher cart_pose_pub_; 
00255   ros::Publisher cart_twist_pub_; 
00257   // For checking the relative pose between cart and robot
00258   pose_range_2D cart_range;
00259 
00260   ros::Publisher pose2D_pub_; // A publisher for 2d poses (just for debugging)
00261   geometry_msgs::Twist twist_cart_max_, twist_base_max_;
00262 
00263   // A flag that indicates whether we are printing debug messages on this iteration of computeVelocityCommand
00264   bool debug_print_;
00265 
00266   // A timestamp indicating the last time that we received an invalid cart pose message from the
00267   // articulate cart server
00268   ros::Time last_invalid_pose_time_;
00269 
00270   boost::shared_ptr<cart_local_planner::SBPLSubscriber<cart_pushing_msgs::RobotCartPath> > sbpl_subscriber_;
00271   std::vector<geometry_msgs::PoseStamped> original_global_plan_;
00272 
00273   bool getNextFewWaypointsIndices(const std::vector<geometry_msgs::PoseStamped> &current_plan, 
00274                                   const int &current_waypoint_index, 
00275                                   const int &max_num_waypoints, 
00276                                   const double &max_translation,
00277                                   const double &max_rotation,
00278                                   std::vector<unsigned int> &waypoint_indices);
00279 
00280   bool checkTrajectoryMonotonic(const std::vector<unsigned int> &indices);
00281 
00282 
00283 
00284 };
00285 
00286 } // namespace
00287 #endif // guard