RobotOperator.h

Go to the documentation of this file.

#ifndef OPERATOR_H
#define OPERATOR_H

#define NODE_NAME     "operator"
#define COMMAND_TOPIC "cmd"
#define CONTROL_TOPIC "cmd_vel"
#define ROUTE_TOPIC   "route"
#define PLAN_TOPIC    "desired"
#define LUT_RESOLUTION 100

#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <sensor_msgs/PointCloud.h>
#include <nav2d_operator/cmd.h>
#include <tf/transform_listener.h>
#include <tf2_ros/transform_listener.h>

#include <string>

class RobotOperator
{
public:
        // Default Constructor & Destructor
        RobotOperator();
        ~RobotOperator();
        
        // Public Methods
        void receiveCommand(const nav2d_operator::cmd::ConstPtr& msg);

        void executeCommand();

private:
        // Internal Methods
        int calculateFreeSpace(sensor_msgs::PointCloud* cloud);

        double evaluateAction(double direction, double velocity, bool debug = false);

        double findBestDirection();

        void initTrajTable();
        
        inline sensor_msgs::PointCloud* getPointCloud(double direction, double velocity);

        // Internal Storage
        costmap_2d::Costmap2DROS* mLocalMap;
        costmap_2d::Costmap2D* mCostmap;
        double mRasterSize;
        
        tf::TransformListener mTfListener;
        tf2_ros::Buffer mTf2Buffer;
        tf2_ros::TransformListener mTf2Listener;
        
        ros::Subscriber mCommandSubscriber;
        ros::Publisher mControlPublisher;
        ros::Publisher mTrajectoryPublisher;
        ros::Publisher mPlanPublisher;
        ros::Publisher mCostPublisher;
        
        double mDesiredVelocity;
        double mDesiredDirection;
        double mCurrentVelocity;
        double mCurrentDirection;
        int mDriveMode;
        
        sensor_msgs::PointCloud* mTrajTable[(LUT_RESOLUTION * 4) + 2];
        
        double mMaxVelocity;
        
        bool mPublishRoute;
        double mMaxFreeSpace;
        double mSafetyDecay;
        int mSafetyWeight;
        int mConformanceWeight;
        int mContinueWeight;
        int mEscapeWeight;

        std::string mOdometryFrame;
        std::string mRobotFrame;
        
        unsigned int mRecoverySteps;
};

#endif