140 lines
4.3 KiB
C++
Executable File
140 lines
4.3 KiB
C++
Executable File
#ifndef TANGENT_PLANNER_RANGE_MSG_TANGENT_PLANNER_H
|
|
#define TANGENT_PLANNER_RANGE_MSG_TANGENT_PLANNER_H
|
|
|
|
#include <vector>
|
|
#include <iterator>
|
|
#include <list>
|
|
#include <memory>
|
|
#include <chrono>
|
|
#include "geometry_msgs/Pose2D.h"
|
|
#include "sensor_msgs/Range.h"
|
|
#include "geometry_msgs/Twist.h"
|
|
#define square(x) pow(x, 2)
|
|
|
|
namespace Planner {
|
|
struct Map{
|
|
std::shared_ptr<geometry_msgs::Pose2D> obstacle;
|
|
std::shared_ptr<geometry_msgs::Pose2D> robot;
|
|
std::shared_ptr<geometry_msgs::Pose2D> goal;
|
|
Map(std::shared_ptr<geometry_msgs::Pose2D> &ob,
|
|
std::shared_ptr<geometry_msgs::Pose2D> &rb,
|
|
std::shared_ptr<geometry_msgs::Pose2D> &go){
|
|
obstacle = ob;
|
|
robot = rb;
|
|
goal = go;
|
|
}
|
|
Map(std::shared_ptr<geometry_msgs::Pose2D> &rb,
|
|
std::shared_ptr<geometry_msgs::Pose2D> &go){
|
|
robot = rb;
|
|
goal = go;
|
|
}
|
|
};
|
|
class TangentPlanner {
|
|
public:
|
|
TangentPlanner();
|
|
~TangentPlanner();
|
|
int sign(float number);
|
|
void normalizeAngle(float &angle);
|
|
float normalizeAngle(float &&angle);
|
|
bool computeVelocityCommands();
|
|
bool isAtGoal();
|
|
bool isAtGoalOrientation();
|
|
void resetVariables();
|
|
bool foundObstacle();
|
|
void calculateErrors();
|
|
bool setGoal();
|
|
void updateVelocity();
|
|
bool foundLocalMinima();
|
|
float calculateDistance(std::shared_ptr<geometry_msgs::Pose2D> point, std::shared_ptr<geometry_msgs::Pose2D> target);
|
|
float convertAtan2(float&& angle);
|
|
float average(float* array, std::size_t index=2, float av=0.0);
|
|
void shift(float* array,std::size_t index=2);
|
|
|
|
float goal_orientation_=0.f;
|
|
float goal_distance_=0.f;
|
|
float prev_angle_error_=0.f;
|
|
float valid_dis_to_goal_=0.f;
|
|
float xy_goal_tolerance_=0.05, yaw_goal_tolerance_=0.05;
|
|
float heuristic_distance_=0.f, prev_heuristic_distance_=0.f;
|
|
float dfollowed_=0.f;
|
|
float robot_front_, robot_back_, robot_width_min_, robot_width_max_;
|
|
bool turned_around_obs_ = false;
|
|
|
|
// control values for turn controller
|
|
float kp_=0.f, kp_i_=0.f;
|
|
|
|
float sensor_msg_scale_=1.f;
|
|
float factor_scale_ = 2.0;
|
|
float max_trans_vel_=0.f, max_rot_vel_=0.f;
|
|
|
|
// control values for motion controller
|
|
float c_ = 0.f , b_ = 0.f;
|
|
|
|
// the point to be followed
|
|
std::vector<std::shared_ptr<geometry_msgs::Pose2D>> global_plan_;
|
|
std::shared_ptr<geometry_msgs::Pose2D> current_pose_;
|
|
std::shared_ptr<sensor_msgs::Range> range_msg_;
|
|
std::shared_ptr<geometry_msgs::Twist> cmd_ptr_;
|
|
|
|
|
|
private:
|
|
enum BugState
|
|
{
|
|
MotionToGoal = 0,
|
|
FollowBoundary = 1,
|
|
TurnToGoal = 2,
|
|
OrientToGoal = 3,
|
|
StopAtGoal = 4
|
|
} state_;
|
|
enum Controller
|
|
{
|
|
Motion = 0,
|
|
Turn = 1
|
|
} controller_;
|
|
|
|
Controller prev_controller_;
|
|
int cycle_counter_ = 0;
|
|
std::shared_ptr<geometry_msgs::Pose2D> start_follow_point_;
|
|
std::vector<std::shared_ptr<Map>> map_;
|
|
|
|
//will be calculated for motion controller
|
|
float k1_ = 0.f;
|
|
float k2_ = 0.f;
|
|
float k3_ = 0.f;
|
|
|
|
float error_angle_=0.f,i_error_angle_=0.f;
|
|
float error_x_=0.f,i_error_x_=0.f;
|
|
float error_y_=0.f,i_error_y_=0.f;
|
|
|
|
std::tuple<std::shared_ptr<geometry_msgs::Pose2D>,std::shared_ptr<geometry_msgs::Pose2D>>calculateCenter();
|
|
float calculateFactor();
|
|
void followBoundaries();
|
|
void selectDiscontinuityPoint();
|
|
void updateController();
|
|
void updateState();
|
|
void resetError(){
|
|
angle_error_t_ = 0;
|
|
error_angle_ = 0;
|
|
error_x_ = 0;
|
|
error_y_ = 0;
|
|
i_error_y_ = 0;
|
|
i_error_x_ = 0;
|
|
}
|
|
|
|
|
|
protected:
|
|
// log files
|
|
FILE * tof_logfile;
|
|
FILE * goal_logfile;
|
|
FILE * odometry_logfile;
|
|
FILE * twist_logfile;
|
|
FILE * state_logfile;
|
|
FILE * error_logfile;
|
|
FILE * terminal_logfile;
|
|
FILE * param_logfile;
|
|
FILE * center_logfile;
|
|
FILE * custom_logfile;
|
|
};
|
|
}
|
|
|
|
#endif //TANGENT_PLANNER_RANGE_MSG_TANGENT_PLANNER_H
|