controller.h

Go to the documentation of this file.

#ifndef CONTROLLER_H
#define CONTROLLER_H

// ROS
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tuw_multi_robot_msgs/RobotInfo.h>

namespace velocity_controller
{
typedef struct Point_t
{
  float x = 0;
  float y = 0;
  float theta = 0;
} PathPoint;



typedef enum state_t
{
  run,
  stop,
  step,
  wait_step
} state;

class Controller
{

public:
  Controller();
  void setPath(std::shared_ptr<std::vector<PathPoint>> _path);
  void update(PathPoint _odom, float _delta_t);
  void setSpeedParams(float _max_v, float _max_w);
  void getSpeed(float *_v, float *_w);
  void setGoalRadius(float _r);
  void setPID(float _Kp, float _Ki, float _Kd);
  void setState(state s);
  size_t getProgress();
  bool isActive();

  int getStatus();
  void setGoodId(int);
  int getGoodId();
  void setOrderId(int);
  int getOrderId();
  void setOrderStatus(int);
  int getOrderStatus();

private:
  float normalizeAngle(float _angle);
  float absolute(float _val);
  bool checkGoal(PathPoint _odom);
  std::vector<PathPoint> path_;
  size_t idx_path_target_point_;
  float max_v_, max_w_;
  float v_, w_;
  bool plan_active = false;
  float e_last_ = 0;
  float e_dot_ = 0;
  float Kp_ = 5;
  float Kd_ = 1;
  float Ki_ = 0.0;

  int robot_status = tuw_multi_robot_msgs::RobotInfo::STATUS_STOPPED;
  int goodId;
  int orderId;
  int orderStatus;

  float goal_radius_ = 0.25;

  state actual_cmd_ = run;
  
protected:   
  PathPoint current_pose_;
};

} // namespace velocity_controller

#endif // CONTROLLER_NODE_H