NavigationState.h

Go to the documentation of this file.

#ifndef NAVIGATIONSTATE_H
#define NAVIGATIONSTATE_H

#include <pluginlib/class_list_macros.h>
#include <rsm_core/BaseState.h>
#include <rsm_core/IdleState.h>
#include <rsm_core/EmergencyStopState.h>
#include <rsm_core/StateInterface.h>
#include <geometry_msgs/PoseArray.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <rsm_msgs/GetNavigationGoal.h>
#include <rsm_msgs/GetRobotPose.h>
#include <rsm_msgs/OperationMode.h>
#include <rsm_msgs/GoalCompleted.h>
#include <std_srvs/Trigger.h>
#include <tf/transform_datatypes.h>
#include <std_msgs/Bool.h>

namespace rsm {

class NavigationState: public BaseState {

public:

        NavigationState();

        ~NavigationState();

        void onSetup();

        void onEntry();

        void onActive();

        void onExit();

        void onExplorationStart(bool &success, std::string &message);

        void onExplorationStop(bool &success, std::string &message);

        void onWaypointFollowingStart(bool &success, std::string &message);

        void onWaypointFollowingStop(bool &success, std::string &message);

        void onInterrupt(int interrupt);

private:

        typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
        boost::shared_ptr<MoveBaseClient> _move_base_client;

        ros::NodeHandle _nh;
        ros::ServiceClient _get_navigation_goal_service;
        ros::ServiceClient _navigation_goal_completed_service;
        ros::ServiceClient _get_robot_pose_service;
        ros::ServiceClient _get_exploration_mode_service;
        ros::ServiceClient _get_reverse_mode_service;
        ros::Subscriber _get_goal_obsolete_subscriber;
        ros::Subscriber _reverse_mode_subscriber;
        ros::Subscriber _operation_mode_subscriber;
        ros::Timer _idle_timer;
        ros::Timer _unstuck_timer;

        geometry_msgs::Pose _nav_goal;
        bool _goal_active;
        int _navigation_mode;
        std::string _routine;
        tf::Vector3 _last_position;
        double _last_yaw;
        bool _exploration_mode;
        bool _reverse_mode;
        int _operation_mode;
        int _navigation_completed_status;
        bool _robot_did_move;
        double _pose_tolerance;
        double _idle_timer_duration;
        double _unstuck_timer_duration;
        bool _unstucking_robot;
        bool _unstucking_executed;
        bool _idle_timer_behavior;

        void idleTimerCallback(const ros::TimerEvent& event);
        void unstuckTimerCallback(const ros::TimerEvent& event);
        void goalObsoleteCallback(const std_msgs::Bool::ConstPtr& msg);
        void reverseModeCallback(const std_msgs::Bool::ConstPtr& reverse_mode);
        void operationModeCallback(
                        const rsm_msgs::OperationMode::ConstPtr& operation_mode);
        void abortNavigation();
        bool comparePose();
};

}

#endif