cart_local_planner.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#ifndef CART_LOCAL_PLANNER_H_
#define CART_LOCAL_PLANNER_H_

#include <cart_local_planner/sbpl_subscriber.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <nav_core/base_local_planner.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <nav_msgs/Odometry.h>
#include <cart_pushing_msgs/RobotCartPath.h>
#include <cart_pushing_msgs/RobotCartConfiguration.h>
#include <boost/scoped_ptr.hpp>
#include <costmap_trajectory_checker/costmap_trajectory_checker.h>
#include <manipulation_transforms/manipulation_transforms.h>

namespace cart_local_planner {

typedef struct {
  double x_min;
  double x_max;
  double y_min;
  double y_max;
  double t_min;
  double t_max;
} pose_range_2D;

const geometry_msgs::Twist operator+(const geometry_msgs::Twist& t1, const geometry_msgs::Twist& t2);
const geometry_msgs::Twist operator-(const geometry_msgs::Twist& t1, const geometry_msgs::Twist& t2);
double mag(geometry_msgs::Twist &t);
double mag(geometry_msgs::Pose2D &p);

class CartLocalPlanner: public nav_core::BaseLocalPlanner {
public:
  CartLocalPlanner();
  void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);
  bool isGoalReached();
  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan);
  bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);

protected:
  virtual void initialization_extras();

  /************
   * PLANNING *
   ************/
  virtual void setControlMode();

  virtual void controlModeAction();

  virtual void setGoalPoses();

  virtual void filterTwistsCombined(int filter_options);

  virtual bool checkTwists();

  virtual bool checkTwistsMonotonic();

  /************************
   * SUPPORTING UTILITIES *
   ************************/
  int setCartGoalFromWaypoint(double min_dist, double min_x_component);

  // Set the current goal of the cart/robot and update the error_ variables accordingly given current actual_ pose
  void setCartPoseGoal (const tf::Stamped<tf::Pose>& goal);
  void setRobotPoseGoal (const tf::Stamped<tf::Pose>& goal);

  
  double baseTwistFromError();

  double cartTwistFromError();

  void setYawFromVec(const tf::Pose& pose1, const tf::Pose& pose2,
                     tf::Pose& res);

  bool transformGlobalPlan(const tf::TransformListener& tf,
                           const std::vector<geometry_msgs::PoseStamped>& global_plan,
                           const costmap_2d::Costmap2DROS& costmap,
                           const std::string& global_frame, std::vector<
                           geometry_msgs::PoseStamped>& transformed_plan);

  std::vector<geometry_msgs::Point> transformFootprint(const geometry_msgs::PolygonStamped& poly) const;

  void freeze();

  static void scaleTwist2D(geometry_msgs::Twist& t, double scale);

  geometry_msgs::Twist mapBaseTwistToCart(
                                          const geometry_msgs::Twist &twist_base);

  void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);

  void invalidPoseCallback (const std_msgs::Empty::ConstPtr& msg);

  // Publishers for pose and twist messages in 2D (mainly for debugging and plotting)
  void publishDebugPose(geometry_msgs::Pose &p);
  void publishDebugTwist(geometry_msgs::Twist &t);
  void publishDebugPose(tf::Pose &p);

  /****************
   * DATA MEMBERS *
   ****************/
  enum FILTER_OPTIONS {
    GLOBAL_SCALING = 0x1,
    CART_ERR_SCALING = GLOBAL_SCALING << 1,
    ALL = 0xffff
  };

  enum CONTROL_MODE {
    REGULAR
  };

  // The control mode for the computeVelocityCommands switch
  enum CONTROL_MODE control_mode_;

  tf::TransformListener* tf_;
  tf::TransformBroadcaster tb_;
  ros::NodeHandle nh_;

  costmap_2d::Costmap2DROS* costmap_ros_;

  CostmapTrajectoryChecker robot_collision_checker_;
  CostmapTrajectoryChecker cart_collision_checker_;

  ros::Publisher vel_pub_;
  ros::Subscriber odom_sub_, invalid_pose_sub_;
  nav_msgs::Odometry base_odom_;
  ros::Time goal_reached_time_;
  std::vector<geometry_msgs::PoseStamped> global_plan_;
  boost::mutex odom_lock_, invalid_pose_mutex_;

  // Transforms for mapping twists around
  tf::Stamped<tf::Pose> robot_pose_actual_;     
  tf::Stamped<tf::Pose> cart_pose_actual_;      
  // Transforms for holding goal poses
  tf::Stamped<tf::Pose> robot_pose_goal_;       
  tf::Stamped<tf::Pose> cart_pose_goal_;                
  // Containers for robot and cart error
  tf::Pose robot_pose_error_;   
  geometry_msgs::Pose2D cart_pose_error_;                       
  // Command twists
  geometry_msgs::Twist *twist_base_; 
  geometry_msgs::TwistStamped twist_cart_; 
  bool initialized_;
  double K_trans_base_, K_rot_base_; 
  double K_trans_cart_, K_rot_cart_; 
  int num_traj_steps_; 
  double dt_; 
  unsigned int current_waypoint_; 
  double tolerance_trans_, tolerance_rot_, tolerance_timeout_; 
  double trans_stopped_velocity_, rot_stopped_velocity_; 
  // Whether we directly subscribe to sbpl plan rather than getting it from move base
  bool subscribe_sbpl_plan_;

  // For controlling cart position w/ arms
  ros::Publisher cart_pose_pub_; 
  ros::Publisher cart_twist_pub_; 
  // For checking the relative pose between cart and robot
  pose_range_2D cart_range;

  ros::Publisher pose2D_pub_; // A publisher for 2d poses (just for debugging)
  geometry_msgs::Twist twist_cart_max_, twist_base_max_;

  // A flag that indicates whether we are printing debug messages on this iteration of computeVelocityCommand
  bool debug_print_;

  // A timestamp indicating the last time that we received an invalid cart pose message from the
  // articulate cart server
  ros::Time last_invalid_pose_time_;

  boost::shared_ptr<cart_local_planner::SBPLSubscriber<cart_pushing_msgs::RobotCartPath> > sbpl_subscriber_;
  std::vector<geometry_msgs::PoseStamped> original_global_plan_;

  bool getNextFewWaypointsIndices(const std::vector<geometry_msgs::PoseStamped> &current_plan, 
                                  const int &current_waypoint_index, 
                                  const int &max_num_waypoints, 
                                  const double &max_translation,
                                  const double &max_rotation,
                                  std::vector<unsigned int> &waypoint_indices);

  bool checkTrajectoryMonotonic(const std::vector<unsigned int> &indices);



};

} // namespace
#endif // guard