mpc_local_planner_ros.h

Go to the documentation of this file.

/*********************************************************************
 *
 *  Software License Agreement
 *
 *  Copyright (c) 2020, Christoph Rösmann, All rights reserved.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 *  Authors: Christoph Rösmann
 *********************************************************************/

#ifndef MPC_LOCAL_PLANNER_ROS_H_
#define MPC_LOCAL_PLANNER_ROS_H_

#include <ros/ros.h>

// base local planner base class and utilities
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/goal_functions.h>
#include <base_local_planner/odometry_helper_ros.h>
#include <mbf_costmap_core/costmap_controller.h>
#include <nav_core/base_local_planner.h>

// mpc_local_planner related classes
#include <mpc_local_planner/controller.h>
#include <mpc_local_planner/utils/publisher.h>

// teb_local_planner related classes
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/pose_se2.h>
#include <teb_local_planner/robot_footprint_model.h>

// message types
#include <costmap_converter/ObstacleMsg.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// transforms
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>

// costmap
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_converter/costmap_converter_interface.h>

// dynamic reconfigure
// #include <dynamic_reconfigure/server.h>
// #include <mpc_local_planner/MpcLocalPlannerReconfigureConfig.h>

#include <boost/shared_ptr.hpp>

#include <memory>
#include <mutex>

namespace mpc_local_planner {

class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController
{
    using PoseSE2                = teb_local_planner::PoseSE2;
    using RobotFootprintModelPtr = teb_local_planner::RobotFootprintModelPtr;
    using Point2dContainer       = teb_local_planner::Point2dContainer;
    using ObstContainer          = teb_local_planner::ObstContainer;
    using ViaPointContainer      = std::vector<PoseSE2>;

    using ObstaclePtr      = teb_local_planner::ObstaclePtr;
    using PointObstacle    = teb_local_planner::PointObstacle;
    using CircularObstacle = teb_local_planner::CircularObstacle;
    using LineObstacle     = teb_local_planner::LineObstacle;
    using PolygonObstacle  = teb_local_planner::PolygonObstacle;

 public:
    MpcLocalPlannerROS();
    ~MpcLocalPlannerROS();

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

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

    bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);

    uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity,
                                     geometry_msgs::TwistStamped& cmd_vel, std::string& message);

    bool isGoalReached();

    bool isGoalReached(double xy_tolerance, double yaw_tolerance) { return isGoalReached(); };

    bool cancel() { return false; };


    static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);

    static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros = nullptr);

    static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros);

    static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);

    static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);


 protected:
    void updateObstacleContainerWithCostmap();

    void updateObstacleContainerWithCostmapConverter();

    void updateObstacleContainerWithCustomObstacles();

    void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation);

    // void reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level);

    void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);

    void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg);

    bool pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose,
                         std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot = 1);

    bool transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
                             const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame,
                             double max_plan_length, std::vector<geometry_msgs::PoseStamped>& transformed_plan, int* current_goal_idx = NULL,
                             geometry_msgs::TransformStamped* tf_plan_to_global = NULL) const;

    double estimateLocalGoalOrientation(const std::vector<geometry_msgs::PoseStamped>& global_plan, const geometry_msgs::PoseStamped& local_goal,
                                        int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global,
                                        int moving_average_length = 3) const;

    void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);

 private:
    // Definition of member variables

    // external objects (store weak pointers)
    costmap_2d::Costmap2DROS* _costmap_ros;  
    costmap_2d::Costmap2D* _costmap;         
    tf2_ros::Buffer* _tf;                    

    // internal objects
    Controller _controller;
    ObstContainer _obstacles;  
    Publisher _publisher;
    std::shared_ptr<base_local_planner::CostmapModel> _costmap_model;

    corbo::TimeSeries::Ptr _x_seq = std::make_shared<corbo::TimeSeries>();
    corbo::TimeSeries::Ptr _u_seq = std::make_shared<corbo::TimeSeries>();

    std::vector<geometry_msgs::PoseStamped> _global_plan;  

    base_local_planner::OdometryHelperRos _odom_helper;  

    pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> _costmap_converter_loader;  
    boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> _costmap_converter;              

    // std::shared_ptr<dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>>
    //    dynamic_recfg_;                                        //!< Dynamic reconfigure server to allow config modifications at runtime
    ros::Subscriber _custom_obst_sub;                          
    std::mutex _custom_obst_mutex;                             
    costmap_converter::ObstacleArrayMsg _custom_obstacle_msg;  

    ViaPointContainer _via_points;
    ros::Subscriber _via_points_sub;         
    bool _custom_via_points_active = false;  
    std::mutex _via_point_mutex;             

    PoseSE2 _robot_pose;                   
    PoseSE2 _robot_goal;                   
    geometry_msgs::Twist _robot_vel;       
    bool _goal_reached = false;            
    ros::Time _time_last_infeasible_plan;  
    int _no_infeasible_plans = 0;          
    geometry_msgs::Twist _last_cmd;        
    ros::Time _time_last_cmd;

    RobotFootprintModelPtr _robot_model;

    std::vector<geometry_msgs::Point> _footprint_spec;  
    double _robot_inscribed_radius;                     
    double _robot_circumscribed_radius;                 

    std::string _global_frame;      
    std::string _robot_base_frame;  

    // flags
    bool _initialized;  

    struct Parameters
    {
        double xy_goal_tolerance                      = 0.2;
        double yaw_goal_tolerance                     = 0.1;
        bool global_plan_overwrite_orientation        = true;
        double global_plan_prune_distance             = 1.0;
        double max_global_plan_lookahead_dist         = 1.5;
        bool is_footprint_dynamic                     = false;
        bool include_costmap_obstacles                = true;
        double costmap_obstacles_behind_robot_dist    = 1.5;
        double global_plan_viapoint_sep               = -1;
        double collision_check_min_resolution_angular = M_PI;
        int collision_check_no_poses                  = -1;
        std::string odom_topic                        = "odom";
        double controller_frequency                   = 10;

    } _params;

    struct CostmapConverterPlugin
    {
        std::string costmap_converter_plugin;
        double costmap_converter_rate      = 5;
        bool costmap_converter_spin_thread = true;

    } _costmap_conv_params;

 public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

};  // end namespace mpc_local_planner

#endif  // MPC_LOCAL_PLANNER_ROS_H_