RobotControlMux.h

Go to the documentation of this file.

#ifndef ROBOTCONTROLMUX_H_
#define ROBOTCONTROLMUX_H_

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
#include <rsm_msgs/OperationMode.h>
#include <rsm_msgs/SetOperationMode.h>

#define MOVE_THRESH 0.05

namespace rsm {

class RobotControlMux {
public:
        RobotControlMux();
        ~RobotControlMux();
        void publishTopics();

private:
        ros::NodeHandle _nh;
        ros::ServiceServer _set_operation_mode_service;
        ros::Subscriber _teleoperation_cmd_vel_sub;
        ros::Subscriber _autonomy_cmd_vel_sub;
        ros::Subscriber _joystick_sub;
        ros::Publisher _cmd_vel_pub;
        ros::Publisher _operation_mode_pub;
        ros::Timer _teleoperation_idle_timer;

        std::string _teleoperation_cmd_vel_topic;
        std::string _autonomy_operation_cmd_vel_topic;
        std::string _cmd_vel_topic;
        std::string _joystick_topic;

        double _teleoperation_idle_timer_duration;
        int _operation_mode;
        bool _emergency_stop_active;
        bool _joystick_used;
        geometry_msgs::Twist _autonomy_cmd_vel;
        geometry_msgs::Twist _teleoperation_cmd_vel;
        sensor_msgs::Joy _joystick_cmd;

        void publishCmdVel();
        void publishOperationMode();
        void autonomyCmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_vel);
        void teleoperationCmdVelCallback(
                        const geometry_msgs::Twist::ConstPtr& cmd_vel);
        void joystickCallback(const sensor_msgs::Joy::ConstPtr& joy);
        void teleoperationIdleTimerCallback(const ros::TimerEvent& event);
        bool checkJoystickCommand(const sensor_msgs::Joy::ConstPtr& joy);
        bool setOperationMode(rsm_msgs::SetOperationMode::Request &req,
                        rsm_msgs::SetOperationMode::Response &res);

};

} /* namespace rsm */

#endif /* ROBOTCONTROLMUX_H_ */