/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Author: Kevin Lee
 *********************************************************************/
#ifndef NAV_MOVE_BASE_ACTION_H_
#define NAV_MOVE_BASE_ACTION_H_

#include <robot/robot.h>
#include <vector>
#include <string>
#include <tf3/buffer_core.h>
#include <move_base/simple_action_server.h>

// interfaces headers
#include <move_base_core/navigation.h>
#include <robot_nav_core/base_global_planner.h>
#include <robot_nav_core/base_local_planner.h>
#include <robot_nav_core/recovery_behavior.h>
#include <mission_adapters/mission_adapters.h>

// boost headers
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/recursive_mutex.hpp>

#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_nav_msgs/GetPlan.h>
#include <robot_protocol_msgs/Order.h>
#include <robot_move_base_msgs/MoveBaseAction.h>
#include <memory>
#include <functional>


namespace move_base
{
  template <class T>
  void move_parameter(robot::NodeHandle& old_h, robot::NodeHandle& new_h, std::string name,T& value)
  {
    if (!old_h.hasParam(name))
      return;
  
    old_h.getParam(name, value);
    new_h.setParam(name, value);
  }


  // typedefs to help us out with the action server so that we don't hace to type so much
  typedef move_base::SimpleActionServer<robot_move_base_msgs::MoveBaseAction> MoveBaseActionServer;

  enum MoveBaseState
  {
    PLANNING,
    CONTROLLING,
    CLEARING
  };

  enum RecoveryTrigger
  {
    PLANNING_R,
    CONTROLLING_R,
    OSCILLATION_R
  };

  struct MoveBaseConfig 
  {
    std::string base_global_planner;
  };

  /**
   * @class MoveBase
   * @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location.
   */
  class MoveBase : public robot::move_base_core::BaseNavigation
  {
  public:
    /**
     * @brief  Constructor for the actions
     * @param tf A reference to a TransformListener
     */
    MoveBase(robot::TFListenerPtr tf);
    
    /**
     * @brief Default constructor for the actions
     */
    MoveBase();

    /**
     * @brief Initialize the navigation system.
     * @param tf Shared pointer to the TF listener or buffer.
     */
    virtual void initialize(robot::TFListenerPtr tf) override;

    /**
     * @brief Initialize the cost-to-occupancy lookup table for costmap conversion
     */
    static void initializeCostToOccupancyLUT();

    /**
     * @brief Set the robot's footprint (outline shape) in the global frame.
     *        This can be used for planning or collision checking.
     *
     * @param fprt A vector of points representing the robot's footprint polygon.
     *             The points should be ordered counter-clockwise.
     */
    virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) override;

    /**
     * @brief Get the robot's footprint (outline shape) in the global frame.
     * @return A vector of points representing the robot's footprint polygon.
     */
    virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() override;

		/**
		 * @brief Add a static map to the navigation system.
		 * @param map_name The name of the map.
		 * @param map The map to add.
		 */
		 virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) override;

		 /**
			* @brief Add a laser scan to the navigation system.
			* @param laser_scan_name The name of the laser scan.
			* @param laser_scan The laser scan to add.
			*/
		 virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) override;

 
		 /**
			* @brief Add a point cloud to the navigation system.
			* @param point_cloud_name The name of the point cloud.
			* @param point_cloud The point cloud to add.
			*/
		 virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) override;
 
		 /**
			* @brief Add a point cloud2 to the navigation system.
			* @param point_cloud2_name The name of the point cloud2.
			* @param point_cloud2 The point cloud2 to add.
			*/
		 virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) override;

		 /**
			* @brief Add one coherent depth-camera sample to the navigation system.
			* @param topic The logical topic of the combined depth-camera stream.
			* @param data Depth image and matching camera intrinsics.
			*/
		 virtual void addDepthCameraData(const std::string &topic,
		                                 robot_sensor_msgs::DepthCameraData::ConstPtr data) override;
 
		 /**
			* @brief Get a static map from the navigation system.
			* @param map_name The name of the map.
			* @return The map.
			*/
		 virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) override;
 
		 /**
			* @brief Get a laser scan from the navigation system.
			* @param laser_scan_name The name of the laser scan.
			* @return The laser scan.
			*/
		 virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) override;
 
		 /**
			* @brief Get a point cloud from the navigation system.
			* @param point_cloud_name The name of the point cloud.
			* @return The point cloud.
			*/
		 virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) override;
 
		 /**
			* @brief Get a point cloud2 from the navigation system.
			* @param point_cloud2_name The name of the point cloud2.
			* @return The point cloud2.
			*/
		 virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) override;
 
		 /**
			* @brief Get all static maps from the navigation system.
			* @return The static maps.
			*/
		 virtual std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() override;
 
		 /**
			* @brief Get all laser scans from the navigation system.
			* @return The laser scans.
			*/
		 virtual std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() override;
 
		 /**
			* @brief Get all point clouds from the navigation system.
			* @return The point clouds.
			*/
		 virtual std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() override;
 
		 /**
			* @brief Get all point cloud2s from the navigation system.
			* @return The point cloud2s.
			*/
		 virtual std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() override;
 
		 /**
			* @brief Remove a static map from the navigation system.
			* @param map_name The name of the map.
			* @return True if the map was removed successfully.
			*/
		virtual bool removeStaticMap(const std::string &map_name) override;
 
		 /**
			* @brief Remove a laser scan from the navigation system.
			* @param laser_scan_name The name of the laser scan.
			* @return True if the laser scan was removed successfully.
			*/
		 virtual bool removeLaserScan(const std::string &laser_scan_name) override;
 
		 /**
			* @brief Remove a point cloud from the navigation system.
			* @param point_cloud_name The name of the point cloud.
			* @return True if the point cloud was removed successfully.
			*/
		 virtual bool removePointCloud(const std::string &point_cloud_name) override;
 
		 /**
			* @brief Remove a point cloud2 from the navigation system.
			* @param point_cloud2_name The name of the point cloud2.
			* @return True if the point cloud2 was removed successfully.
			*/
		 virtual bool removePointCloud2(const std::string &point_cloud2_name) override;
 
		 /**
			* @brief Remove all static maps from the navigation system.
			* @return True if the static maps were removed successfully.
			*/
		 virtual bool removeAllStaticMaps() override;

		 /**
			* @brief Remove all laser scans from the navigation system.
			* @return True if the laser scans were removed successfully.
			*/
		 virtual bool removeAllLaserScans() override;

		 /**
			* @brief Remove all point clouds from the navigation system.
			* @return True if the point clouds were removed successfully.
			*/
		 virtual bool removeAllPointClouds() override;

		 /**
			* @brief Remove all point cloud2s from the navigation system.
			* @return True if the point cloud2s were removed successfully.
			*/
		 virtual bool removeAllPointCloud2s() override;

		 /**
			* @brief Remove all data from the navigation system.
			* @return True if the data was removed successfully.
			*/
		 virtual bool removeAllData() override;

     /**
			* @brief Add an odometry to the navigation system.
			* @param odometry_name The name of the odometry.
			* @param odometry The odometry to add.
			*/
		 virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) override;

    /**
     * @brief Send a goal for the robot to navigate to.
     * @param goal              Target pose in the global frame.
     * @param xy_goal_tolerance Acceptable error in X/Y (meters).
     * @param yaw_goal_tolerance Acceptable angular error (radians).
     * @return True if goal was accepted and sent successfully.
     */
    virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
                        double xy_goal_tolerance = 0.0,
                        double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Send a goal for the robot to navigate to.
     * @param msg               Order message.
     * @param goal              Target pose in the global frame.
     * @param xy_goal_tolerance Acceptable error in X/Y (meters).
     * @param yaw_goal_tolerance Acceptable angular error (radians).
     * @return True if goal was accepted and sent successfully.
     */
    virtual bool moveTo(const robot_protocol_msgs::Order &msg,
                        const robot_geometry_msgs::PoseStamped &goal,
                        double xy_goal_tolerance = 0.0,
                        double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Send a docking goal to a predefined marker (e.g. charger).
     * @param maker             Marker name or ID.
     * @param goal              Target pose for docking.
     * @param xy_goal_tolerance Acceptable XY error (meters).
     * @param yaw_goal_tolerance Acceptable heading error (radians).
     * @return True if docking command succeeded.
     */
    virtual bool dockTo(const std::string &maker,
                        const robot_geometry_msgs::PoseStamped &goal,
                        double xy_goal_tolerance = 0.0,
                        double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Send a docking goal to a predefined marker (e.g. charger).
     * @param msg               Order message.
     * @param goal              Target pose for docking.
     * @param xy_goal_tolerance Acceptable XY error (meters).
     * @param yaw_goal_tolerance Acceptable heading error (radians).
     * @return True if docking command succeeded.
     */
    virtual bool dockTo(const robot_protocol_msgs::Order &msg,
                        const std::string &marker,
                        const robot_geometry_msgs::PoseStamped &goal,
                        double xy_goal_tolerance = 0.0,
                        double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Move straight toward the target position (X-axis).
     * @param msg               Order message.
     * @param goal              Target pose.
     * @param xy_goal_tolerance Acceptable positional error (meters).
     * @return True if command issued successfully.
     */
    virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
                                double xy_goal_tolerance = 0.0) override;

    /**
     * @brief Rotate in place to align with target orientation.
     * @param goal              Pose containing desired heading (only Z-axis used).
     * @param yaw_goal_tolerance Acceptable angular error (radians).
     * @return True if rotation command was sent successfully.
     */
    virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
                          double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Pause the robot's movement (e.g. during obstacle avoidance).
     */
    virtual void pause() override;

    /**
     * @brief Resume motion after a pause.
     */
    virtual void resume() override;

    /**
     * @brief Cancel the current goal and stop the robot.
     */
    virtual void cancel() override;

    /**
     * @brief Send direct linear velocity command.
     * @param linear Linear velocity vector (x, y, z).
     * @return True if the command was accepted.
     */
    virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) override;

    /**
     * @brief Send limited angular velocity command.
     * @param angular Angular velocity vector (x, y, z).
     * @return True if the command was accepted.
     */
    virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) override;

    /**
     * @brief Get the robot’s pose as a PoseStamped.
     * @param pose Output parameter with the robot’s current pose.
     * @return True if pose was successfully retrieved.
     */
    virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) override;

    /**
     * @brief Get the robot’s pose as a 2D pose.
     * @param pose Output parameter with the robot’s current 2D pose.
     * @return True if pose was successfully retrieved.
     */
    virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) override;


    /**
     * @brief Get the robot’s twist.
     * @return The robot’s current twist.
     */
    virtual robot_nav_2d_msgs::Twist2DStamped getTwist() override;

    /**
     * @brief Get the navigation feedback.
     * @return Pointer to the navigation feedback.
     */
    virtual robot::move_base_core::NavFeedback *getFeedback() override;

    /**
     * @brief Get the global data.
     * @return The global data.
     */
    virtual robot::move_base_core::PlannerDataOutput getGlobalData() override;

    /**
     * @brief Get the local data.
     * @return The local data.
     */
    virtual robot::move_base_core::PlannerDataOutput getLocalData() override;

    /**
     * @brief  Destructor - Cleans up
     */
    virtual ~MoveBase();

    /**
     * @brief Create a new MoveBase
     * @return A shared pointer to the new MoveBase
     */
    static robot::move_base_core::BaseNavigation::Ptr create();

  private:

    /**
    * @brief Update the global costmap.
    * @param value The value to update the costmap.
    * @param layer_type The type of the layer.
    * @param name The name of the costmap.
    */
    template<typename T>
    void updateGlobalCostmap(const T& value, robot_costmap_2d::LayerType layer_type, const std::string &name);

    /**
    * @brief Update the local costmap.
    * @param value The value to update the costmap.
    * @param layer_type The type of the layer.
    * @param name The name of the costmap.
    */
    template<typename T>
    void updateLocalCostmap(const T& value, robot_costmap_2d::LayerType layer_type, const std::string &name);
    
    /**
     * @brief  Performs a control cycle
     * @param goal A reference to the goal to pursue
     * @return True if processing of the goal is done, false otherwise
     */
     bool executeCycle(robot_geometry_msgs::PoseStamped &goal);

    /**
     * @brief Set velocity for yaw goal tolerance of the robot
     * @param yaw_goal_tolerance the value command
     */
    void setYawGoalTolerance(double yaw_goal_tolerance);

    /**
     * @brief Set the XY goal tolerance for the robot
     * @param xy_goal_tolerance The XY tolerance value in meters
     */
    void setXyGoalTolerance(double xy_goal_tolerance);

    /**
     * @brief Swaps the current global planner with a new one specified by name
     * @param base_global_planner The name of the new global planner to use
     */
    void swapPlanner(std::string base_global_planner);

    /**
     * @brief  Make a new global plan
     * @param  goal The goal to plan to
     * @param  plan Will be filled in with the plan made by the planner
     * @return  True if planning succeeds, false otherwise
     */
    bool makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);

    /**
     * @brief  Make a new global plan
     * @param  msg The order message
     * @param  goal The goal to plan to
     * @param  plan Will be filled in with the plan made by the planner
     * @return  True if planning succeeds, false otherwise
     */
    bool makePlan(const robot_protocol_msgs::Order &msg, const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);

    /**
     * @brief  Load the recovery behaviors for the navigation stack from the parameter server
     * @return True if the recovery behaviors were loaded successfully, false otherwise
     */
    bool loadRecoveryBehaviors(const robot::NodeHandle &node);

    /**
     * @brief  Loads the default recovery behaviors for the navigation stack
     */
    void loadDefaultRecoveryBehaviors();

    /**
     * @brief  Clears obstacles within a window around the robot
     * @param size_x The x size of the window
     * @param size_y The y size of the window
     */
    void clearCostmapWindows(double size_x, double size_y);

    /**
     * @brief  Publishes a velocity command of zero to the base
     */
    void publishZeroVelocity();

    /**
     * @brief  Reset the state of the move_base action and send a zero velocity command to the base
     */
    void resetState();


    /**
     * @brief Thread function that continuously plans paths to the goal
     */
    void planThread();

    /**
     * @brief Callback function to execute a move_base goal
     * @param move_base_goal The goal message to execute
     */
    void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);

    /**
     * @brief  Setup callbacks for the action server
     */
    void setupActionServerCallbacks();

    /**
     * @brief Check if a quaternion is valid (normalized)
     * @param q The quaternion to validate
     * @return True if the quaternion is valid, false otherwise
     */
    bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);

    /**
     * @brief Get the robot's pose in the global frame using a specific costmap
     * @param global_pose Output parameter with the robot's current pose
     * @param costmap The costmap to use for pose lookup
     * @return True if pose was successfully retrieved
     */
    bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap);

    /**
     * @brief Calculate the Euclidean distance between two poses
     * @param p1 First pose
     * @param p2 Second pose
     * @return The distance between the two poses in meters
     */
    double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);

    /**
     * @brief Transform a goal pose to the global frame
     * @param goal_pose_msg The goal pose to transform
     * @return The transformed pose in the global frame
     */
    robot_geometry_msgs::PoseStamped goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg);

    /**
     * @brief This is used to wake the planner at periodic intervals.
     */
    void wakePlanner(const robot::TimerEvent &event);

  private:
    /** @brief Flag indicating if the move_base has been initialized */
    bool initialized_;
    /** @brief Transform listener for coordinate frame transformations */
    robot::TFListenerPtr tf_;
    /** @brief Private node handle for ROS parameters and topics */
    robot::NodeHandle private_nh_;

    /** @brief Action server for move_base goals */
    MoveBaseActionServer *as_;

    /** @brief Function loader for global planner plugins */
    boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
    /** @brief Function loader for local planner (controller) plugins */
    boost::function<robot_nav_core::BaseLocalPlanner::Ptr()> controller_loader_;
    /** @brief Map of recovery behavior loaders by name */
    std::map<std::string, boost::function<robot_nav_core::RecoveryBehavior::Ptr()>> recovery_loaders_;

    /** @brief Local planner (trajectory controller) instance */
    robot_nav_core::BaseLocalPlanner::Ptr tc_;
    /** @brief Global planner instance */
    robot_nav_core::BaseGlobalPlanner::Ptr planner_;
    /** @brief Vector of recovery behavior instances */
    std::vector<robot_nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
    
    /** @brief Costmap for the local planner/controller */
    robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
    /** @brief Costmap for the global planner */
    robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_;    
    /** @brief Robot base frame name and global frame name */
    std::string robot_base_frame_, global_frame_;
    /** @brief Names of recovery behaviors to use */
    std::vector<std::string> recovery_behavior_names_;
    /** @brief Current index in the recovery behavior list */
    unsigned int recovery_index_;
    /** @brief Flag indicating if recovery behaviors are enabled */
    bool recovery_behavior_enabled_;

    /** @brief Current global pose of the robot */
    robot_geometry_msgs::PoseStamped global_pose_;
    /** @brief Frequency for planner execution (Hz) */
    double planner_frequency_;
    /** @brief Frequency for controller execution (Hz) */
    double controller_frequency_;
    /** @brief Inscribed radius of the robot footprint (meters) */
    double inscribed_radius_;
    /** @brief Circumscribed radius of the robot footprint (meters) */
    double circumscribed_radius_;
    /** @brief Maximum time to wait for a valid plan before triggering recovery (seconds) */
    double planner_patience_;
    /** @brief Maximum time to wait for valid control before triggering recovery (seconds) */
    double controller_patience_;
    /** @brief Maximum number of planning retries before giving up */
    int32_t max_planning_retries_;
    /** @brief Current number of planning retries */
    uint32_t planning_retries_;
    /** @brief Conservative distance threshold for resetting costmaps (meters) */
    double conservative_reset_dist_;
    /** @brief Radius around robot to clear obstacles during recovery (meters) */
    double clearing_radius_;
    /** @brief Flag to shutdown costmaps when not in use */
    bool shutdown_costmaps_;
    /** @brief Flag allowing rotation during costmap clearing */
    bool clearing_rotation_allowed_;
    /** @brief Flag to clear costmap when making a new plan */
    bool make_plan_clear_costmap_;
    /** @brief Flag to add unreachable goals to costmap */
    bool make_plan_add_unreachable_goal_;
    /** @brief Time threshold for detecting oscillation (seconds) */
    double oscillation_timeout_;
    /** @brief Distance threshold for detecting oscillation (meters) */
    double oscillation_distance_;

    /** @brief Current state of the move_base (PLANNING, CONTROLLING, CLEARING) */
    MoveBaseState state_;
    /** @brief Trigger that caused recovery behavior (PLANNING_R, CONTROLLING_R, OSCILLATION_R) */
    RecoveryTrigger recovery_trigger_;

    /** @brief Timestamp of the last valid plan */
    robot::Time last_valid_plan_;
    /** @brief Timestamp of the last valid control command */
    robot::Time last_valid_control_;
    /** @brief Timestamp of the last oscillation reset */
    robot::Time last_oscillation_reset_;
    /** @brief Pose used for oscillation detection */
    robot_geometry_msgs::PoseStamped oscillation_pose_;

    /** @brief Plan triple buffer: plan from the planner thread */
    std::vector<robot_geometry_msgs::PoseStamped> *planner_plan_;
    /** @brief Plan triple buffer: latest plan (intermediate buffer) */
    std::vector<robot_geometry_msgs::PoseStamped> *latest_plan_;
    /** @brief Plan triple buffer: plan used by the controller */
    std::vector<robot_geometry_msgs::PoseStamped> *controller_plan_;

    /** @brief Flag to control planner thread execution */
    bool runPlanner_;
    /** @brief Mutex for protecting planner thread data */
    boost::recursive_mutex planner_mutex_;
    /** @brief Condition variable to wake planner thread */
    boost::condition_variable_any planner_cond_;
    /** @brief Goal pose for the planner thread */
    robot_geometry_msgs::PoseStamped planner_goal_;
    /** @brief Order message for the planner thread (nullptr if not using Order-based planning) */
    boost::shared_ptr<robot_protocol_msgs::Order> planner_order_;
    /** @brief Thread handle for the planner */
    boost::thread *planner_thread_;

    /** @brief Mutex for protecting configuration changes */
    boost::recursive_mutex configuration_mutex_;
    /** @brief Last configuration used */
    move_base::MoveBaseConfig last_config_;
    /** @brief Default configuration */
    move_base::MoveBaseConfig default_config_;

    /** @brief Flag indicating if setup is complete */
    bool setup_;
    /** @brief Flag indicating planner frequency changed */
    bool p_freq_change_;
    /** @brief Flag indicating controller frequency changed */
    bool c_freq_change_;
    /** @brief Flag indicating a new global plan is available */
    bool new_global_plan_;
    /** @brief Flag indicating cancel control request */
    bool cancel_ctr_;
    /** @brief Flag indicating pause control request */
    bool pause_ctr_;
    /** @brief Flag indicating if robot is currently paused */
    bool paused_;
    /** @brief Minimum linear velocity when approaching goal (m/s) */
    double min_approach_linear_velocity_{0.1};
    /** @brief Previous forward linear velocity command */
    robot_geometry_msgs::Vector3 old_linear_fwd_;
    /** @brief Previous backward linear velocity command */
    robot_geometry_msgs::Vector3 old_linear_bwd_;
    /** @brief Previous forward angular velocity command */
    robot_geometry_msgs::Vector3 old_angular_fwd_;
    /** @brief Previous reverse angular velocity command */
    robot_geometry_msgs::Vector3 old_angular_rev_;
    /** @brief Original XY goal tolerance value (meters) */
    double original_xy_goal_tolerance_;
    /** @brief Original yaw goal tolerance value (radians) */
    double original_yaw_goal_tolerance_;

    /** @brief Name of the position/local planner plugin */
    std::string position_planner_name_{"MKTLocalPlanner"};
    /** @brief Name of the docking planner plugin */
    std::string docking_planner_name_{"NAVDockingLocalPlanner"};
    /** @brief Name of the go-straight planner plugin */
    std::string go_straight_planner_name_{"NAVGoStraightLocalPlanner"};
    /** @brief Name of the rotate planner plugin */
    std::string rotate_planner_name_{"NAVRotateLocalPlanner"};
  
  };

} // namespace move_base

#endif // NAV_MOVE_BASE_ACTION_H_
