CalculateGoalState.h

Go to the documentation of this file.

#ifndef CALCULATEGOALSTATE_H
#define CALCULATEGOALSTATE_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/TeleoperationState.h>
#include <rsm_core/StateInterface.h>
#include <geometry_msgs/PoseArray.h>
#include <rsm_msgs/SetNavigationGoal.h>
#include <rsm_msgs/GetRobotPose.h>
#include <tf/transform_listener.h>

namespace rsm {

class CalculateGoalState: public BaseState {

public:

        CalculateGoalState();

        ~CalculateGoalState();

        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:

        ros::NodeHandle _nh;
        ros::Subscriber _frontiers_sub;
        ros::Subscriber _failed_goals_sub;
        ros::ServiceClient _set_navigation_goal_service;
        ros::ServiceClient _get_robot_pose_service;
        ros::Timer _idle_timer;

        geometry_msgs::PoseArray _failed_goals;
        geometry_msgs::Pose _goal;
        geometry_msgs::PoseArray _frontier_points;
        bool _frontiers_received;
        bool _failed_goals_received;

        void timerCallback(const ros::TimerEvent& event);
        void abortCalculateGoal();

        void frontiersCallback(const geometry_msgs::PoseArray::ConstPtr& frontiers);

        void failedGoalsCallback(
                        const geometry_msgs::PoseArray::ConstPtr& failed_goals);

        bool differentFromFailedGoals(geometry_msgs::Point point);
};

}

#endif