cart_local_planner: cart_local_planner: cart_local

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   virtual bool clearCartFootprint();
00097 
00098   /************
00099    * PLANNING *
00100    ************/
00105   virtual void setControlMode();
00106 
00113   virtual void controlModeAction();
00114 
00118   virtual bool setGoalPoses();
00119 
00124   virtual void filterTwistsCombined(int filter_options);
00125 
00130   virtual bool checkTwists();
00131 
00136   virtual bool checkTwistsMonotonic();
00137 
00138   /************************
00139    * SUPPORTING UTILITIES *
00140    ************************/
00147   int setCartGoalFromWaypoint(double min_dist, double min_x_component);
00148 
00149   // Set the current goal of the cart/robot and update the error_ variables accordingly given current actual_ pose
00150   void setCartPoseGoal (const tf::Stamped<tf::Pose>& goal);
00151   void setRobotPoseGoal (const tf::Stamped<tf::Pose>& goal);
00152 
00153   
00155   double baseTwistFromError();
00156 
00158   double cartTwistFromError();
00159 
00161   void setYawFromVec(const tf::Pose& pose1, const tf::Pose& pose2,
00162                      tf::Pose& res);
00163 
00165   bool transformGlobalPlan(const tf::TransformListener& tf,
00166                            const std::vector<geometry_msgs::PoseStamped>& global_plan,
00167                            const costmap_2d::Costmap2DROS& costmap,
00168                            const std::string& global_frame, std::vector<
00169                            geometry_msgs::PoseStamped>& transformed_plan);
00170 
00172   std::vector<geometry_msgs::Point> transformFootprint(const geometry_msgs::PolygonStamped& poly) const;
00173 
00175   void freeze();
00176 
00178   static void scaleTwist2D(geometry_msgs::Twist& t, double scale);
00179 
00181   geometry_msgs::Twist mapBaseTwistToCart(
00182                                           const geometry_msgs::Twist &twist_base);
00183 
00185   void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
00186 
00188   void invalidPoseCallback (const std_msgs::Empty::ConstPtr& msg);
00189 
00190   // Publishers for pose and twist messages in 2D (mainly for debugging and plotting)
00191   void publishDebugPose(geometry_msgs::Pose &p);
00192   void publishDebugTwist(geometry_msgs::Twist &t);
00193   void publishDebugPose(tf::Pose &p);
00194 
00195   /****************
00196    * DATA MEMBERS *
00197    ****************/
00198   enum FILTER_OPTIONS {
00199     GLOBAL_SCALING = 0x1,
00200     CART_ERR_SCALING = GLOBAL_SCALING << 1,
00201     ALL = 0xffff
00202   };
00203 
00204   enum CONTROL_MODE {
00205     REGULAR
00206   };
00207 
00208   // The control mode for the computeVelocityCommands switch
00209   enum CONTROL_MODE control_mode_;
00210 
00211   tf::TransformListener* tf_;
00212   tf::TransformBroadcaster tb_;
00213   ros::NodeHandle nh_;
00214 
00215   costmap_2d::Costmap2DROS* costmap_ros_;
00216 
00217   CostmapTrajectoryChecker robot_collision_checker_;
00218   CostmapTrajectoryChecker cart_collision_checker_;
00219 
00220   ros::Publisher vel_pub_;
00221   ros::Subscriber odom_sub_, invalid_pose_sub_;
00222   nav_msgs::Odometry base_odom_;
00223   ros::Time goal_reached_time_;
00224   std::vector<geometry_msgs::PoseStamped> global_plan_;
00225   boost::mutex odom_lock_, invalid_pose_mutex_;
00226 
00227   // Transforms for mapping twists around
00228   tf::Stamped<tf::Pose> robot_pose_actual_;     
00229   tf::Stamped<tf::Pose> cart_pose_actual_;      
00231   // Transforms for holding goal poses
00232   tf::Stamped<tf::Pose> robot_pose_goal_;       
00233   tf::Stamped<tf::Pose> cart_pose_goal_;                
00235   // Containers for robot and cart error
00236   tf::Pose robot_pose_error_;   
00237   geometry_msgs::Pose2D cart_pose_error_;                       
00239   // Command twists
00240   geometry_msgs::Twist *twist_base_; 
00241   geometry_msgs::TwistStamped twist_cart_; 
00243   bool initialized_;
00244   double K_trans_base_, K_rot_base_; 
00245   double K_trans_cart_, K_rot_cart_; 
00246   int num_traj_steps_; 
00247   double dt_; 
00248   unsigned int current_waypoint_; 
00249   double tolerance_trans_, tolerance_rot_, tolerance_timeout_; 
00250   double trans_stopped_velocity_, rot_stopped_velocity_; 
00252   // Whether we directly subscribe to sbpl plan rather than getting it from move base
00253   bool subscribe_sbpl_plan_;
00254 
00255   double cart_length_, cart_width_, cart_x_offset_, cart_y_offset_;
00256   bool cleared_cart_footprint_;
00257 
00258   ros::Publisher cart_clear_pub_, waypoint_pub_; // debug
00259 
00260   // For controlling cart position w/ arms
00261   ros::Publisher cart_pose_pub_; 
00262   ros::Publisher cart_twist_pub_; 
00264   // For checking the relative pose between cart and robot
00265   pose_range_2D cart_range;
00266 
00267   ros::Publisher pose2D_pub_; // A publisher for 2d poses (just for debugging)
00268   geometry_msgs::Twist twist_cart_max_, twist_base_max_;
00269 
00270   // A flag that indicates whether we are printing debug messages on this iteration of computeVelocityCommand
00271   bool debug_print_;
00272 
00273   // A timestamp indicating the last time that we received an invalid cart pose message from the
00274   // articulate cart server
00275   ros::Time last_invalid_pose_time_;
00276 
00277   boost::shared_ptr<cart_local_planner::SBPLSubscriber<cart_pushing_msgs::RobotCartPath> > sbpl_subscriber_;
00278   std::vector<geometry_msgs::PoseStamped> original_global_plan_;
00279 
00280   bool getNextFewWaypointsIndices(const std::vector<geometry_msgs::PoseStamped> &current_plan, 
00281                                   const int &current_waypoint_index, 
00282                                   const int &max_num_waypoints, 
00283                                   const double &max_translation,
00284                                   const double &max_rotation,
00285                                   std::vector<unsigned int> &waypoint_indices);
00286 
00287   bool checkTrajectoryMonotonic(const std::vector<unsigned int> &indices);
00288 
00289 
00290 
00291 };
00292 
00293 } // namespace
00294 #endif // guard
cart_local_planner.h
