eband_local_planner.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, 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 Willow Garage, Inc. 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.
 *
 * Author: Christian Connette
 *********************************************************************/

#ifndef EBAND_LOCAL_PLANNER_H_
#define EBAND_LOCAL_PLANNER_H_

//#define DEBUG_EBAND_ // uncomment to enable additional publishing, visualization and logouts for debug

#include <ros/ros.h>
#include <ros/assert.h>

// classes which are part of this package
#include <eband_local_planner/conversions_and_types.h>
#include <eband_local_planner/eband_visualization.h>
#include <eband_local_planner/EBandPlannerConfig.h>

// local planner specific classes
#include <base_local_planner/costmap_model.h>

// msgs
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/WrenchStamped.h>

// transforms
#include <angles/angles.h>
#include <tf/tf.h>

// costmap & geometry
#include <costmap_2d/costmap_2d_ros.h>

// boost classes
#include <boost/shared_ptr.hpp>

namespace eband_local_planner{

  class EBandPlanner{

    public:
      EBandPlanner();

      EBandPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

      ~EBandPlanner();

      void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

      void reconfigure(EBandPlannerConfig& config);

      void setVisualization(boost::shared_ptr<EBandVisualization> eband_visual);

      bool setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan);

      bool getPlan(std::vector<geometry_msgs::PoseStamped>& global_plan);

      bool getBand(std::vector<Bubble>& elastic_band);

      bool addFrames(const std::vector<geometry_msgs::PoseStamped>& robot_pose, const AddAtPosition& add_frames_at);

      bool optimizeBand();

      bool optimizeBand(std::vector<Bubble>& band);

    private:
      // pointer to external objects (do NOT delete object)
      costmap_2d::Costmap2DROS* costmap_ros_; 

      // parameters
      std::vector<double> acc_lim_; 
      int num_optim_iterations_; 
      double internal_force_gain_; 
      double external_force_gain_; 
      double tiny_bubble_distance_; 
      double tiny_bubble_expansion_; 
      double min_bubble_overlap_; 
      int max_recursion_depth_approx_equi_; 
      double equilibrium_relative_overshoot_; 
      double significant_force_; 
      double costmap_weight_; // the costmap weight or scaling factor

      // pointer to locally created objects (delete - except for smart-ptrs:)
      base_local_planner::CostmapModel* world_model_; // local world model
      boost::shared_ptr<EBandVisualization> eband_visual_; // pointer to visualization object

      // flags
      bool initialized_, visualization_;

      // data
      std::vector<geometry_msgs::Point> footprint_spec_; // specification of robot footprint as vector of corner points
      costmap_2d::Costmap2D* costmap_; // pointer to underlying costmap
      std::vector<geometry_msgs::PoseStamped> global_plan_; // copy of the plan that shall be optimized
      std::vector<Bubble> elastic_band_;


      // methods

      // band refinement (filling of gaps & removing redundant bubbles)

      bool refineBand(std::vector<Bubble>& band);


      bool removeAndFill(std::vector<Bubble>& band, std::vector<Bubble>::iterator& start_iter,std::vector<Bubble>::iterator& end_iter);


      bool fillGap(std::vector<Bubble>& band, std::vector<Bubble>::iterator& start_iter,std::vector<Bubble>::iterator& end_iter);


      // optimization

      bool modifyBandArtificialForce(std::vector<Bubble>& band);


      bool applyForces(int bubble_num, std::vector<Bubble>& band, std::vector<geometry_msgs::WrenchStamped> forces);


      bool moveApproximateEquilibrium(const int& bubble_num, const std::vector<Bubble>& band, Bubble& curr_bubble,
          const geometry_msgs::WrenchStamped& curr_bubble_force, geometry_msgs::Twist& curr_step_width,
          const int& curr_recursion_depth);


      // problem (geometry) dependant functions

      bool interpolateBubbles(geometry_msgs::PoseStamped start_center, geometry_msgs::PoseStamped end_center, geometry_msgs::PoseStamped& interpolated_center);


      bool checkOverlap(Bubble bubble1, Bubble bubble2);


      bool calcBubbleDistance(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, double& distance);


      bool calcBubbleDifference(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, geometry_msgs::Twist& difference);


      bool calcObstacleKinematicDistance(geometry_msgs::Pose center_pose, double& distance);


      bool getForcesAt(int bubble_num, std::vector<Bubble> band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces);


      bool calcInternalForces(int bubble_num, std::vector<Bubble> band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces);


      bool calcExternalForces(int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces);


      bool suppressTangentialForces(int bubble_num, std::vector<Bubble> band, geometry_msgs::WrenchStamped& forces);


      // type conversion

      bool convertPlanToBand(std::vector<geometry_msgs::PoseStamped> plan, std::vector<Bubble>& band);


      bool convertBandToPlan(std::vector<geometry_msgs::PoseStamped>& plan, std::vector<Bubble> band);

  };
};
#endif