#ifndef PLC_AGV__HH__
#define PLC_AGV__HH__

#include <ros/ros.h>
#include "robot_base.h"
#include "tf2_msgs/TFMessage.h"
#include "PLCModbus.h"

class PlcAgvRobot : public RobotBase
{
public:
    PlcAgvRobot();
    ~PlcAgvRobot();
    virtual bool get_odom();
    virtual bool post_tf();
    bool down_speed();
protected:
    
  
    
    static void location_tf_callback(const tf2_msgs::TFMessage::ConstPtr &msg, PlcAgvRobot* par);
    bool point2Wheels(const double linear_speed, const double angular_speed);
    bool wheelsToPoint(double &linear_speed, double& angular_speed);
    bool readSpeed();
    bool writeSpeed(const double &linear_speed, const double &angular_speed);
    struct wheelStatus{
        double left_linear_speed;
        double right_linear_speed;
        // double left_angular_speed;
        // double right_angular_speed;
    };
public:
    ros::Subscriber tf_sub_;
    tf::Pose        laser_pose_;
    tf::Transform   laser_base_link_;
    wheelStatus ws_load;
    wheelStatus ws_down;
    plc_modbus::CPLCModbus plc_;
    bool exit_;
};


#endif // !PLC_AGV__HH__
