segment_controller.h

Go to the documentation of this file.

#ifndef SEGMENT_CONTROLLER_H
#define SEGMENT_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 PathPrecondition_t
{
    int robot;
    int stepCondition;
} PathPrecondition;

typedef struct PathPoint_t
{
    float x;
    float y;
    float theta;
    std::vector<PathPrecondition> precondition;
} PathPoint;

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

class SegmentController
{

  public:
    SegmentController();
    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);
    void updatePrecondition(PathPrecondition pc);
    int getCount();

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

    bool getPlanActive() { return plan_active; }

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

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

    std::vector<int> actualPreconditions;

    state actual_cmd_ = run;
};

} // namespace velocity_controller

#endif // CONTROLLER_NODE_H