init

include/tangent_planner.h

@@ -0,0 +1,140 @@
+#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