RealsenseMappingState.h

Go to the documentation of this file.

#ifndef MAPPINGSTATE_H
#define MAPPINGSTATE_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 <rsm_msgs/GoalCompleted.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
#include <std_srvs/Trigger.h>

#define MOVE_LEFT 0
#define MOVE_RIGHT 1
#define MOVE_TO_CENTER 2

#define REALSENSE_LEFT_LIMIT 1.30
#define REALSENSE_RIGHT_LIMIT -1.30
#define REALSENSE_CENTER_POSITION 0.0

#define POS_TOLERANCE 0.05

namespace rsm {

class RealsenseMappingState: public BaseState {

public:

        RealsenseMappingState();

        ~RealsenseMappingState();

        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 _joint_states_subscriber;
        ros::Publisher _realsense_joint_controller;
        ros::ServiceClient _reset_realsense_position_client;
        ros::ServiceClient _navigation_goal_completed_service;

        int _swivel_state;
        bool _position_reached;
        bool _message_send;
        int _navigation_completed_status;

        void jointStateCallback(sensor_msgs::JointState::ConstPtr joint_state);
};

}

#endif