ftc_planner.h

Go to the documentation of this file.

#ifndef FTC_LOCAL_PLANNER_FTC_PLANNER_H_
#define FTC_LOCAL_PLANNER_FTC_PLANNER_H_

#include <nav_msgs/Odometry.h>

#include <costmap_2d/costmap_2d_ros.h>

#include <tf/transform_listener.h>

#include <Eigen/Core>

#include <dynamic_reconfigure/server.h>

#include <asr_ftc_local_planner/FTCPlannerConfig.h>

#include <nav_core/base_local_planner.h>

#include <ftc_local_planner/transform_global_plan.h>

#include <ftc_local_planner/join_costmap.h>

namespace ftc_local_planner
{

    class FTCPlanner : public nav_core::BaseLocalPlanner
    {

    public:
        FTCPlanner();
        bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);

        bool isGoalReached();

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

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

        ~FTCPlanner();

    private:

        void reconfigureCB(FTCPlannerConfig &config, uint32_t level);

        int checkMaxDistance(tf::Stamped<tf::Pose> current_pose);

        int checkMaxAngle(int points, tf::Stamped<tf::Pose> current_pose);

        bool rotateToOrientation(double angle, geometry_msgs::Twist& cmd_vel, double accuracy);

        void publishPlan(int max_point);

        int driveToward(tf::Stamped<tf::Pose> current_pose, geometry_msgs::Twist& cmd_vel);

        double calculateGlobalPlanAngle(tf::Stamped<tf::Pose> current_pose, const std::vector<geometry_msgs::PoseStamped>& plan, int points);

        bool checkCollision(int max_points);

        //used for transformation
        tf::TransformListener* tf_;
        //costmap to get the current position
        costmap_2d::Costmap2DROS* costmap_ros_;
        //global plan which we run along
        std::vector<geometry_msgs::PoseStamped> global_plan_;
        //transformed global plan in global frame with only the points with are needed for calculation (max_points)
        std::vector<geometry_msgs::PoseStamped> transformed_global_plan_;
        //check if plan first at first time
        bool first_setPlan_;
        //last point of the global plan in global frame
        tf::Stamped<tf::Pose> goal_pose_;
        // true if the robot should rotate to gobal plan if new global goal set
        tf::Stamped<tf::Pose> old_goal_pose_;
        // true if the robot should rotate to gobal plan if new global goal set
        bool rotate_to_global_plan_;
        //for dynamic reconfigure
        dynamic_reconfigure::Server<FTCPlannerConfig> *dsrv_;
        //start config
        ftc_local_planner::FTCPlannerConfig default_config_;
        //reconfigure config
        ftc_local_planner::FTCPlannerConfig config_;
        //true if the goal point is reache and orientation of goal is reached
        bool goal_reached_;
        //true if the goal point is reache and orientation of goal isn't reached
        bool stand_at_goal_;
        //opublisher where the local plan for visulatation is published
        ros::Publisher local_plan_publisher_;

        //rotation velocity of previous round for the rotateToOrientation methode
        double cmd_vel_angular_z_rotate_;
        //x velocity of the previous round
        double cmd_vel_linear_x_;
        //rotation velocity of previous round for the dirveToward methode
        double cmd_vel_angular_z_;

        JoinCostmap *joinCostmap_;

    };
};
#endif