init

2023-01-21 11:33:15 +01:00
parent 924825aa57
commit e9f436ddd7
37 changed files with 3748 additions and 92 deletions
@@ -0,0 +1,50 @@
+#ifndef TANGENT_PLANNER_PARAMETERS_HELPER_H
+#define TANGENT_PLANNER_PARAMETERS_HELPER_H
+
+#include <string>
+#include <ros/ros.h>
+#include <chrono>
+#include <ctime>
+#include <iomanip>
+#include <sstream>
+#include <string>
+
+namespace Planner {
+
+    inline
+    bool hasParam(const std::string& name)
+    {
+        return ros::param::has(name);
+    }
+
+    template<typename T>
+    T getParam(const std::string& name, const T& defaultValue)
+    {
+        T v;
+        if(ros::param::get(name, v))
+        {
+            ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v);
+            return v;
+        }
+        else
+            ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue);
+        return defaultValue;
+    }
+
+    template<typename T>
+    T getParam(const std::string& name)
+    {
+        T v;
+        if(ros::param::get(name, v))
+        {
+            ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v);
+            return v;
+        }
+        else
+            ROS_ERROR_STREAM("Cannot find value for parameter: " << name);
+        return T();
+    }
+
+} // namespace planner
+
+#endif //TANGENT_PLANNER_PARAMETERS_HELPER_H
@@ -0,0 +1,57 @@
+//
+// Created by gfuse on 04/06/22.
+//
+
+#ifndef P_46_TANGUNT_BUG_OBSTACLE_AVOIDANCE_READ_HPP
+#define P_46_TANGUNT_BUG_OBSTACLE_AVOIDANCE_READ_HPP
+#include <vector>
+#include <sys/types.h>
+#include <sys/stat.h>
+template<typename T,int N>
+class DATA_FILE
+{
+public:
+    DATA_FILE(std::string path){
+        in_ =fopen(path.c_str(),"r+");
+        assert(in_ && "path is empty in DATA_FILE. Please notice you need to specify the path from project directory like /data/data.txt");
+/*        out_=fopen("/result.txt").c_str(),"w+");
+        assert(out_ && "path is empty in DATA_FILE");
+        chmod((std::string(getenv("HOME"))+"/result.txt").c_str(), ACCESSPERMS);*/
+    };
+    ~DATA_FILE(){
+        fclose(in_);
+//        fclose(out_);
+    };
+    bool read(std::array<T,N>& in){
+        double count=0;
+        if(fscanf(in_,"%le",&count)<1)return false;
+        for(int i=0;i<N;++i){
+            T value;
+            fscanf(in_,",%f",&value);
+            in.at(i)=value;
+        }
+        fscanf(in_,"\n");
+        return true;
+    };
+    void write(std::array<T,N>& out){
+        fprintf(out_,"%ld,",count);
+        for(int i=0;i<N;++i)fprintf(out_,"%f,",out[i]);
+        fprintf(out_,"\n");
+        ++count;
+    };
+    void write(std::array<T,N>& out,T& error){
+        fprintf(out_,"%ld,",count);
+        for(int i=0;i<N;++i)fprintf(out_,"%f,",out[i]);
+        fprintf(out_,", ,");
+        fprintf(out_,"%f,",error);
+        fprintf(out_,"\n");
+        ++count;
+    };
+
+private:
+    FILE* in_;
+    FILE* out_;
+    size_t count=0;
+};
+
+#endif //P_46_TANGUNT_BUG_OBSTACLE_AVOIDANCE_READ_HPP
@@ -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
@@ -0,0 +1,86 @@
+#ifndef TANGENT_PLANNER_TANGENT_PLANNER_NODE_H
+#define TANGENT_PLANNER_TANGENT_PLANNER_NODE_H
+
+#include "tangent_planner.h"
+#include "memory.h"
+#include "nav_msgs/Odometry.h"
+#include "geometry_msgs/Pose2D.h"
+#include "ros/ros.h"
+#include "sensor_msgs/Range.h"
+#include "read.hpp"
+#include <tf/transform_datatypes.h>
+//#include "tangent_planner/nav_path_start.h"
+//#include "tangent_planner/nav_path_stop.h"
+//#include "tangent_planner/nav_get_patrol_name.h"
+//#include "tangent_planner/nav_get_status.h"
+//#include "tangent_planner/nav_low_bat.h"
+#include "thread"
+
+
+
+namespace Planner {
+    class TangentPlannerNode : public TangentPlanner {
+    public:
+        TangentPlannerNode();
+
+        ~TangentPlannerNode();
+
+        void set_publisher(const ros::Publisher &publisher);
+
+        void odometry_subscriber_callback(const nav_msgs::Odometry &msg);
+
+        void tof_subscriber_callback(const sensor_msgs::Range &msg);
+
+        void twist_publisher_callback();
+
+//        bool start_patrolling_callback(tangent_planner::nav_path_startRequest &req, tangent_planner::nav_path_startResponse &res);
+//
+//        bool stop_patrolling_callback(tangent_planner::nav_path_stopRequest &req, tangent_planner::nav_path_stopResponse &res);
+//
+//        bool get_patrol_path_name_callback(tangent_planner::nav_get_patrol_nameRequest &req,
+//                                           tangent_planner::nav_get_patrol_nameResponse &res);
+//
+//        bool get_current_status_callback(tangent_planner::nav_get_statusRequest &req, tangent_planner::nav_get_statusResponse &res);
+//
+//        bool low_battery_callback(tangent_planner::nav_low_batRequest &req, tangent_planner::nav_low_batResponse &res);
+
+        void computeVelocity();
+
+        bool is_started();
+
+        bool going_home = false;
+        ros::ServiceServer start_patrolling;
+        ros::ServiceServer stop_patrolling;
+        ros::ServiceServer get_patrol_path_name;
+        ros::ServiceServer get_current_status;
+        ros::ServiceServer low_battery;
+        std::string path_file_name;
+        void start_thread();
+    private:
+        void read_path_file(const char* path){
+            std::shared_ptr<DATA_FILE<float,10>> file = std::make_shared<DATA_FILE<float, 10>>(path);
+            std::array<float,10> in={0.f};
+            while(file->read(in)){
+                tf::Quaternion q(in[4],in[5],in[6], in[3]);
+                tf::Matrix3x3 m(q);
+                double roll, pitch, yaw;
+                m.getRPY(roll, pitch, yaw);
+                std::shared_ptr<geometry_msgs::Pose2D> pose = std::make_shared<geometry_msgs::Pose2D>();
+                pose->x = in[1];
+                pose->y = in[0];
+                pose->theta = yaw;
+                global_plan_.insert(global_plan_.begin(), pose);
+            }
+            setGoal();
+            file.reset();
+        }
+
+        void read_path_file(std::string path);
+
+        ros::Publisher twist_publisher;
+        bool started_ = false;
+//        std::mutex mtx_;
+    };
+}
+
+#endif //TANGENT_PLANNER_TANGENT_PLANNER_NODE_H