70 lines
2.0 KiB
C++
Executable File
70 lines
2.0 KiB
C++
Executable File
#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 "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();
|
|
|
|
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;
|
|
};
|
|
}
|
|
|
|
#endif //TANGENT_PLANNER_TANGENT_PLANNER_NODE_H
|