AdditionsServiceProvider.h

Go to the documentation of this file.

#include <ros/ros.h>

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <std_srvs/SetBool.h>
#include <geometry_msgs/Twist.h>

#include <std_srvs/Trigger.h>
#include <std_msgs/Float64.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/server/simple_action_server.h>

#include <std_msgs/Bool.h>
#include <rsm_msgs/GetNavigationGoal.h>
#include <rsm_msgs/GoalCompleted.h>

typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

namespace rsm {

class AdditionsServiceProvider {

public:
        AdditionsServiceProvider();
        ~AdditionsServiceProvider();
        void publishTopics();

private:
        ros::NodeHandle _nh;

        ros::ServiceServer _set_navigation_to_reverse_service;
        ros::Subscriber _reverse_mode_cmd_vel_subscriber;
        ros::Publisher _reverse_mode_cmd_vel_publisher;

        ros::Publisher _failed_goals_publisher;

        ros::Subscriber frontiers_marker_array_subscriber;
        ros::Publisher _exploration_goals_publisher;
        ros::ServiceClient _get_navigation_goal_service;
        ros::Publisher _goal_obsolete_publisher;
        ros::Subscriber _exploration_goal_subscriber;
        ros::Subscriber _exploration_mode_subscriber;

        ros::ServiceServer _reset_kinect_position_serivce;
        ros::Publisher _kinect_joint_controller;

        MoveBaseActionServer* as;
        std::string _autonomy_cmd_vel_topic;
        geometry_msgs::PoseArray _exploration_goals;
        geometry_msgs::PoseArray _failed_goals;
        double _exploration_goal_tolerance;
        bool _exploration_mode;
        bool _calculate_goal_plugin_used;
        bool _goal_obsolete;

        void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg);

        bool setNavigationToReverse(std_srvs::SetBool::Request &req,
                        std_srvs::SetBool::Response &res);
        void reverseModeCmdVelCallback(
                        const geometry_msgs::Twist::ConstPtr& cmd_vel);

        void navigationGoalCallback(
                        const move_base_msgs::MoveBaseGoalConstPtr& frontier_goal);
        void frontierCallback(
                        const visualization_msgs::MarkerArray::ConstPtr& frontiers);
        void publishExplorationGoals();
        void publishFailedGoals();
        void publishGoalObsolete();

        void explorationGoalCallback(
                        const rsm_msgs::GoalStatus::ConstPtr& goal_status);
        void explorationModeCallback(
                        const std_msgs::Bool::ConstPtr& exploration_mode);
        bool navGoalIncludedInFrontiers();

        bool resetKinectPosition(std_srvs::Trigger::Request &req,
                        std_srvs::Trigger::Response &res);
};

}