public/Tangent_Planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init

Browse Source
This commit is contained in:
vpn
2023-01-21 11:33:15 +01:00
parent 924825aa57
commit e9f436ddd7
37 changed files with 3748 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gtest.cpp
Executable
+698
View File
@@ -0,0 +1,698 @@
#include <gtest/gtest.h>
#include "tangent_planner.h"
#include <memory>
#include "ros/ros.h"
#include <cstdio>
#include "cstring"
#include "vector"
#include "math.h"
#include "read.hpp"
#include <tf/transform_datatypes.h>
#include <functional>
#include <iostream>
#include <random>
#include "time.h"
bool simulation(std::string path){
// file to save data for visualizing
FILE *fptr;
fptr = fopen("/home/p_46_ws/src/tangent_planner/data/visual.txt","w");
Planner::TangentPlanner tangent_planner = Planner::TangentPlanner();
// Define random generator with Gaussian distribution to add noise to odometry
const double mean = 0.001;
const double stddev = 0.0001;
auto dist = std::bind(std::normal_distribution<double>{mean, stddev},
std::mt19937(std::random_device{}()));
// load path file and set points to global_plan_
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];
std::cerr << "x is : " << in[1] << "and y is : " << in[0] << std::endl;
pose->theta = yaw;
tangent_planner.global_plan_.insert(tangent_planner.global_plan_.begin(), pose);
}
tangent_planner.setGoal();
file.reset();
// initial tangent_planner values
tangent_planner.kp_ = 1;
tangent_planner.kp_i_ = 0.01;
tangent_planner.xy_goal_tolerance_ = 0.2;
tangent_planner.yaw_goal_tolerance_ = 0.1;
tangent_planner.max_trans_vel_ =1;
tangent_planner.max_rot_vel_ = 0.5;
tangent_planner.robot_front_ = 0.2;
tangent_planner.robot_back_ = 0.05;
tangent_planner.robot_width_min_ = 0.06;
tangent_planner.robot_width_max_ = 0.08;
tangent_planner.valid_dis_to_goal_ = 0.5;
tangent_planner.sensor_msg_scale_ = 1;
tangent_planner.range_msg_->field_of_view = 0.4;
tangent_planner.range_msg_->min_range = 0.02;
tangent_planner.range_msg_->max_range = 2.0;
tangent_planner.c = 0.15;
tangent_planner.b = 20;
tangent_planner.current_pose_->x = 0;
tangent_planner.current_pose_->y = 0;
tangent_planner.current_pose_->theta = 0.f;
// setting points of obstacle
std::vector<std::shared_ptr<geometry_msgs::Pose2D>> obstacles;
for(double i=0;i<0.4;i+=0.0001){
std::shared_ptr<geometry_msgs::Pose2D> point = std::make_shared<geometry_msgs::Pose2D>();
point->x = -3;
point->y = 0.2 -i;
obstacles.push_back(point);
}
bool cycle = true;
bool goal_found = false;
for(int i = 0; i<2000; i++) {
float temp = std::numeric_limits<float>::max();
float temp2 = std::numeric_limits<float>::max();
tangent_planner.range_msg_->range = std::numeric_limits<float>::max();
for (auto obs:obstacles){
float obs_theta = atan2((obs->y-tangent_planner.current_pose_->y),(obs->x-tangent_planner.current_pose_->x));
if (obs_theta < tangent_planner.current_pose_->theta + 0.2 &&
obs_theta > tangent_planner.current_pose_->theta - 0.2 &&
tangent_planner.calculateDistance(tangent_planner.current_pose_, obs) < 2){
tangent_planner.range_msg_->range = tangent_planner.calculateDistance(tangent_planner.current_pose_, obs);
temp = obs->x;
temp2 = obs->y;
}
}
fprintf(fptr, "%f,%f,%f,%f,%f,%f\n",
tangent_planner.current_pose_->x,
tangent_planner.current_pose_->y,
tangent_planner.current_pose_->theta,
tangent_planner.range_msg_->range,
temp,
temp2
);
cycle = tangent_planner.computeVelocityCommands();
tangent_planner.cmd_ptr_->linear.x,
tangent_planner.cmd_ptr_->linear.y,
tangent_planner.cmd_ptr_->linear.z,
tangent_planner.cmd_ptr_->angular.x,
tangent_planner.cmd_ptr_->angular.y,
tangent_planner.cmd_ptr_->angular.z;
float vel = sqrt(pow(tangent_planner.cmd_ptr_->linear.x, 2) + pow(tangent_planner.cmd_ptr_->linear.y, 2));
tangent_planner.current_pose_->theta += tangent_planner.cmd_ptr_->angular.z*0.1;
tangent_planner.current_pose_->x += vel*0.1 * cos(tangent_planner.current_pose_->theta);
tangent_planner.current_pose_->y += vel*0.1 * sin(tangent_planner.current_pose_->theta);
//with noise --- uncomment to add noise
// tangent_planner.current_pose_->x += dist();
// tangent_planner.current_pose_->y += dist();
// tangent_planner.current_pose_->theta += dist();
if(tangent_planner.global_plan_.empty()){
goal_found = true;
break;
}
}
fclose(fptr);
return goal_found;
}
TEST(TangentPlanner, rosFreeCycle){
bool status = simulation( "/home/p_46_ws/src/tangent_planner/data/straight-front&back-path.txt");
std::cerr << status;
}
TEST(TangentPlanner, average){
float a[50];
for(int i=0;i<50;++i)a[i]=i;
Planner::TangentPlanner tangent_planner = Planner::TangentPlanner();
auto c=tangent_planner.average(a);
std::cerr<<c;
for (int i=0;i<50;i++){
std::cerr << a[i] << "\t";
}
}
TEST(TangentPlanner, normalizeAngle) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.angle_error_t_ = (-3 * M_PI - M_PI_2) * 0.99;
double angle = -3 * M_PI - M_PI_2;
angle = rftp.normalizeAngle(angle);
std::cerr << angle;
// EXPECT_NEAR(angle, 0.1, 0.01);
// angle = M_PI_2;
// angle = rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, M_PI_2, 0.00001);
// angle = M_PI * 3;
// angle = rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, 0.1, 0.01);
// angle = M_PI_2; // angle = pi/2; fn should return pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, 1.5708, 0.00001);
// angle = M_PI;
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, 3.14159, 0.00001);
// angle = M_PI + M_PI_2;
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, 4.71239, 0.00001);
// angle = M_PI * 2 + 1;
// std::cerr << angle << std::endl;
// rftp.normalizeAngle(angle);
// std::cerr << angle << std::endl;
// EXPECT_NEAR(angle, 0, 0.00001);
// angle = M_PI + M_PI_2; // angle = 3pi/2; fn should return -pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, -m_pi/2, 0.001);
//
// angle = 2*M_PI + M_PI_2; //angle = 2pi+(pi/2); fn should return pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, m_pi/2, 0.001);
//
// angle = -M_PI - M_PI_2; //angle = -3pi/2; fn should return pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, m_pi/2, 0.001);
//
// angle = -2*M_PI - M_PI_2; // angle = -2pi-(pi/2); fn should return -pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, -m_pi/2, 0.001);
//
// angle = M_PI+0.1;
// // note angle > M_PI this time returns true
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, m_pi+0.1-2*m_pi, 0.001);
//
// angle = -M_PI;
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, m_pi, 0.001);
// std::cerr << rftp.normalizeAngle(0.022850);
//}
//TEST(TangentPlanner, normalizeAngle2){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// float angle = -6.389263;
// rftp.angle_error_t_ = -0.2;
// rftp.normalizeAngle(angle);
// std::cerr << angle;
//}
TEST(TangentPlanner, setGoal) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
EXPECT_FALSE(rftp.setGoal());
std::shared_ptr<geometry_msgs::Pose2D> point = std::make_shared<geometry_msgs::Pose2D>();
point->x = 0;
point->y = 0;
point->theta = 0;
rftp.global_plan_.push_back(point);
EXPECT_TRUE(rftp.setGoal());
rftp.valid_dis_to_goal_ = 0.1;
EXPECT_FALSE(rftp.setGoal());
}
//
//TEST(TangentPlanner, isInGoalDirection) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.yaw_goal_tolerance_ = 2;
// rftp.goal_angle_ = 1.5;
// EXPECT_EQ(rftp.isInGoalDirection(), true);
// rftp.goal_angle_ = -1.5;
// EXPECT_EQ(rftp.isInGoalDirection(), true);
// rftp.goal_angle_ = 2;
// EXPECT_EQ(rftp.isInGoalDirection(), false);
// rftp.goal_angle_ = 3;
// EXPECT_EQ(rftp.isInGoalDirection(), false);
// rftp.goal_angle_ = -3;
// EXPECT_EQ(rftp.isInGoalDirection(), false);
//}
//
TEST(TangentPlanner, foundLocalMinima) {
// it seems that true anf false should be exchange
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.prev_heuristic_distance_ = 3;
rftp.heuristic_distance_ = 2;
rftp.xy_goal_tolerance_ = 1;
EXPECT_FALSE(rftp.foundLocalMinima());
rftp.xy_goal_tolerance_ = 0.5;
EXPECT_FALSE(rftp.foundLocalMinima());
rftp.xy_goal_tolerance_ = 0.1;
EXPECT_TRUE(rftp.foundLocalMinima());
rftp.prev_heuristic_distance_ = 2;
rftp.heuristic_distance_ = 3;
rftp.xy_goal_tolerance_ = 1;
EXPECT_FALSE(rftp.foundLocalMinima());
rftp.xy_goal_tolerance_ = 0.1;
EXPECT_FALSE(rftp.foundLocalMinima());
}
TEST(TangentPlanner, isAtGoal) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.xy_goal_tolerance_ = 2;
rftp.goal_distance_ = 1;
EXPECT_TRUE(rftp.isAtGoal());
rftp.goal_distance_ = 2;
EXPECT_FALSE(rftp.isAtGoal());
rftp.goal_distance_ = 3;
EXPECT_FALSE(rftp.isAtGoal());
}
TEST(TangentPlanner, isAtGoalOrientation) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.yaw_goal_tolerance_ = 2;
rftp.goal_orientation_ = 1;
EXPECT_EQ(rftp.isAtGoalOrientation(), true);
rftp.goal_orientation_ = 2;
EXPECT_EQ(rftp.isAtGoalOrientation(), false);
rftp.goal_orientation_ = -2;
EXPECT_EQ(rftp.isAtGoalOrientation(), false);
rftp.goal_orientation_ = 0.3;
EXPECT_EQ(rftp.isAtGoalOrientation(), true);
rftp.goal_orientation_ = -0.3;
EXPECT_EQ(rftp.isAtGoalOrientation(), true);
}
TEST(TangentPlanner, resetVariables) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.dfollowed_ = 3;
rftp.resetVariables();
EXPECT_EQ(rftp.dfollowed_, 0);
}
//TEST(TangentPlanner, foundObstacle){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.range_msg_->range = 0.5;
// rftp.range_msg_->min_range = 0.001;
// rftp.range_msg_->max_range = 2.0;
// rftp.goal_distance_ = 1.5;
// EXPECT_EQ(rftp.foundObstacle(), true);
// rftp.range_msg_->range = 0.0005;
// EXPECT_EQ(rftp.foundObstacle(), false);
// rftp.range_msg_->range = 2.1;
// EXPECT_EQ(rftp.foundObstacle(), false);
// rftp.range_msg_->range = rftp.range_msg_->min_range;
// EXPECT_EQ(rftp.foundObstacle(), false);
// rftp.range_msg_->range = rftp.range_msg_->max_range;
// EXPECT_EQ(rftp.foundObstacle(), false);
//
// rftp.range_msg_->range = 1.8;
// EXPECT_EQ(rftp.foundObstacle(), false);
// rftp.range_msg_->range = 1.2;
// EXPECT_EQ(rftp.foundObstacle(), true);
// rftp.goal_distance_ = rftp.range_msg_->max_range;
// EXPECT_EQ(rftp.foundObstacle(), true);
//}
//TEST(TangentPlanner, followBoundaries){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// gp->x = 4.0;
// gp->y = 5.0;
// gp->theta = M_PI_2;
// rftp.global_plan_.push_back(gp);
// rftp.goal_distance_ = 10.0;
// rftp.goal_angle_ = M_PI_2;
// rftp.dfollowed_ = std::numeric_limits<float>::max();
// rftp.state_ = Planner::TangentPlanner::BugState::FollowBoundary;
// rftp.range_msg_->range = 1.0;
// rftp.current_pose_->x = 2.0;
// rftp.current_pose_->y = 3.0;
// rftp.current_pose_->theta = M_PI_2;
// rftp.followBoundaries();
// EXPECT_EQ(rftp.goal_angle_, 0.0);
// EXPECT_EQ(rftp.dreached_, 10.0);
// EXPECT_NEAR(rftp.dfollowed_, sqrt(5), 0.001);
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::FollowBoundary);
//}
//
//TEST(TangentPlanner, selectDiscontinuityPoint){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.range_msg_->field_of_view = M_PI;
// rftp.range_msg_->range = 1.0;
// rftp.range_msg_->max_range = 2.0;
// rftp.range_msg_->min_range = 0.001;
// rftp.goal_distance_ = 10;
// rftp.goal_angle_ = 0.0;
// rftp.selectDiscontinuityPoint();
// EXPECT_NEAR(rftp.heuristic_distance_, sqrt(101)+1.0, 0.001);
// EXPECT_NEAR(rftp.goal_angle_, -M_PI_2, 0.001);
//}
//
//TEST(TangentPlanner, updateVelocity){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.angular_velocity_ = 5;
// rftp.max_rot_vel_ = 3;
// rftp.max_trans_vel_ = 10;
// rftp.kp_ = 2;
// rftp.updateVelocity();
// EXPECT_EQ(rftp.cmd_ptr_->angular.z, 3);
// rftp.angular_velocity_ = 3;
// rftp.max_rot_vel_ = 5;
// rftp.updateVelocity();
// EXPECT_EQ(rftp.cmd_ptr_->angular.z, 3);
// EXPECT_EQ(rftp.cmd_ptr_->linear.x, 8);
//
//}
//
//TEST(TangentPlanner, updateControllerStateStopAtGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::StopAtGoal;
// rftp.kp_ = 1;
// rftp.angular_velocity_ = 1;
// rftp.goal_reached_ = false;
// rftp.prev_heuristic_distance_ = -1;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 0);
// EXPECT_EQ(rftp.angular_velocity_, 0);
// EXPECT_EQ(rftp.goal_reached_, true);
// EXPECT_EQ(rftp.prev_heuristic_distance_, 0);
//}
//
//TEST(TangentPlanner, updateControllerStateTurnToGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.kp_ = 1;
// rftp.goal_angle_ = 4;
// rftp.angular_velocity_ = 1;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 0);
// rftp.yaw_goal_tolerance_ = 1;
// EXPECT_NEAR(rftp.goal_angle_, -2.28319, 0.001);
// rftp.goal_angle_ = M_PI_2;
// rftp.updateController();
// EXPECT_NEAR(rftp.angular_velocity_, 0.5*M_PI_2, 0.0001);
// rftp.goal_angle_ = 1;
// rftp.yaw_goal_tolerance_ = 5;
// rftp.updateController();
// EXPECT_EQ(rftp.goal_angle_, 0);
// EXPECT_EQ(rftp.angular_velocity_, 0);
//}
//
//TEST(TangentPlanner, updateControllerStateOrientToGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::OrientToGoal;
// rftp.kp_ = 1;
// rftp.goal_orientation_ = 2;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 0);
// EXPECT_EQ(rftp.angular_velocity_, 1);
//}
//
//TEST(TangentPlanner, updateControllerStateMotionToGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::MotionToGoal;
// rftp.kp_ = 1.5;
// rftp.kp_i_ = 0.8;
// rftp.range_msg_->range = 0.0001;
// rftp.goal_angle_ = 1.5;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 1);
// EXPECT_EQ(rftp.angular_velocity_, 1.5);
// rftp.kp_ = 0.1;
// rftp.updateController();
// EXPECT_NEAR(rftp.kp_, 0.9, 0.1);
//}
//
//TEST(TangentPlanner, updateControllerStateMotionToGoal2){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::MotionToGoal;
// rftp.kp_ = 1.5;
// rftp.kp_i_ = 0.8;
// rftp.goal_distance_ = 10;
// rftp.goal_angle_ = 0.0;
// rftp.range_msg_->range = 1.0;
// rftp.range_msg_->min_range =0.001;
// rftp.range_msg_->max_range =2.0;
// rftp.range_msg_->field_of_view = M_PI;
// rftp.updateController();
// EXPECT_NEAR(rftp.heuristic_distance_, sqrt(101)+1.0, 0.1);
// EXPECT_NEAR(rftp.goal_angle_, -M_PI_2, 0.0001);
// EXPECT_NEAR(rftp.angular_velocity_, -M_PI_2, 0.0001);
//}
//
//TEST(TangentPlanner, updateControllerStateFollowBoundary){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::FollowBoundary;
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// rftp.kp_ = 1.5;
// rftp.kp_i_ = 0.8;
// gp->x = 4.0;
// gp->y = 5.0;
// gp->theta = M_PI_2;
// rftp.global_plan_.push_back(*gp);
// rftp.goal_distance_ = 10;
// rftp.goal_angle_ = M_PI_2;
// rftp.range_msg_->range = 1.0;
// rftp.current_pose_->x = 2.0;
// rftp.current_pose_->y = 3.0;
// rftp.current_pose_->theta = M_PI_2;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 1);
// EXPECT_EQ(rftp.angular_velocity_, 0.0);
// rftp.kp_ = 0.1;
// rftp.updateController();
// EXPECT_NEAR(rftp.kp_, 0.9, 0.1);
//}
//
//TEST(TangentPlanner, updateControllerStateCloseToGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::CloseToGoal;
// rftp.kp_ = 0.3;
// rftp.kp_i_ = 0.1;
// rftp.goal_angle_ = 1.5;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 0.25);
// EXPECT_EQ(rftp.angular_velocity_, 1.5);
// rftp.kp_ = 0.3;
// rftp.kp_i_ = 0.01;
// rftp.updateController();
// EXPECT_NEAR(rftp.kp_, 0.29, 0.01);
// EXPECT_EQ(rftp.angular_velocity_, 1.5);
//}
//
//
//TEST(TangentPlanner, updateStateToFollowBoundary) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::FollowBoundary;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::FollowBoundary);
//}
//
//TEST(TangentPlanner, updateStateToTurnToGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::OrientToGoal;
// rftp.goal_angle_ = 2;
// rftp.yaw_goal_tolerance_ = 1.5;
// rftp.goal_distance_ = 2;
// rftp.xy_goal_tolerance_ = 1.5;
// rftp.is_new_goal_ = true;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::TurnToGoal);
//}
//
//TEST(TangentPlanner, updateStateSetIsNewGoalFalse) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.goal_angle_ = 1.5;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.updateState();
// EXPECT_EQ(rftp.is_new_goal_, false);
//}
//TEST(TangentPlanner, updateStateToOrientToGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.is_new_goal_ = false;
// rftp.goal_angle_ = 2;
// rftp.yaw_goal_tolerance_ = 1.5;
// rftp.goal_distance_ = 1.5;
// rftp.xy_goal_tolerance_ = 2;
// rftp.goal_orientation_ = 2;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::OrientToGoal);
//}
//
//TEST(TangentPlanner, updateStateToCloseToGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.is_new_goal_ = false;
// //rftp.goal_angle_ = 1.5;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.kp_ = 7;
// rftp.max_trans_vel_ = 1;
// rftp.goal_distance_ = 4;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// //EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::CloseToGoal);
//}
//
//TEST(TangentPlanner, updateStateToCloseToGoal2) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.is_new_goal_ = false;
// //rftp.goal_angle_ = 2;
// rftp.yaw_goal_tolerance_ = 1.5;
// rftp.kp_ = 7;
// rftp.max_trans_vel_ = 1;
// rftp.range_msg_->range = 2.4;
// rftp.range_msg_->min_range = 0.001;
// rftp.range_msg_->max_range = 2.0;
// //rftp.goal_angle_ = 3.0;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.goal_distance_ = 4;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// //EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::CloseToGoal);
//}
//
////printf("is in goal orientation: %d and is close to goal: %d and is at goal: %d and is in goal direction: %d",
////rftp.isInGoalDirection(),
//// rftp.isCloseToGoal(),
//// rftp.isAtGoal(),
//// rftp.isInGoalDirection());
//
//TEST(TangentPlanner, updateStateToStopAtGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.xy_goal_tolerance_ = 2;
// rftp.goal_distance_ = 1;
// rftp.goal_orientation_ = -1;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.is_new_goal_ = false;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::StopAtGoal);
//}
//
//TEST(TangentPlanner, updateStateToMotionToGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.xy_goal_tolerance_ = 8;
// rftp.goal_distance_ = 10;
// rftp.kp_ = 1;
// rftp.max_trans_vel_ = 1;
// rftp.goal_orientation_ = -3;
// rftp.yaw_goal_tolerance_ = 4;
// rftp.is_new_goal_ = false;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::MotionToGoal);
//}
//
//TEST(TangentPlanner, updateStateToMotionToGoal2) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.xy_goal_tolerance_ = 8;
// rftp.goal_distance_ = 10;
// rftp.kp_ = 1;
// rftp.max_trans_vel_ = 1;
// rftp.goal_orientation_ = -3;
// rftp.is_new_goal_ = false;
// rftp.range_msg_->range = 0.5;
// rftp.range_msg_->min_range = 0.001;
// rftp.range_msg_->max_range = 2.0;
// //rftp.goal_angle_ = 3.0;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.prev_heuristic_distance_ = 26.0;
// rftp.heuristic_distance_ = 9.0;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::FollowBoundary);
//}
//
//TEST(TangentPlanner, updateGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.current_pose_->x = 1.0;
// rftp.current_pose_->y = 5.0;
// rftp.current_pose_->theta = M_PI;
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// gp->x = 4.0;
// gp->y = 9.0;
// gp->theta = M_PI_2;
// float gp_atan2 = atan2(gp->y - rftp.current_pose_->y, gp->x - rftp.current_pose_->x) ;
// rftp.global_plan_.push_back(gp);
//
// //rftp.updateGoal();
// std::cout<<"step 000"<<std::endl;
// EXPECT_EQ(rftp.goal_distance_, 5.0);
// //EXPECT_NEAR(rftp.goal_angle_, gp_atan2 - rftp.current_pose_->theta, 0.0001);
// EXPECT_NEAR(rftp.goal_orientation_, -M_PI_2, 0.0001);
//}
//
//TEST(TangentPlanner, computeVelocityCommands){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::MotionToGoal;
// rftp.initialized_ = false;
// EXPECT_EQ(rftp.computeVelocityCommands(), false);
// rftp.initialized_ = true;
// // goal position:
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// gp->x = 4.0;
// gp->y = 8.0;
// gp->theta = M_PI_2;
// rftp.global_plan_.push_back(gp);
// // current position:
// rftp.current_pose_->x = 1.0;
// rftp.current_pose_->y = 5.0;
// rftp.current_pose_->theta = M_PI;
// rftp.range_msg_->range = 0.7;
// rftp.range_msg_->max_range = 2.0;
// rftp.range_msg_->min_range = 0.001;
// EXPECT_EQ(rftp.computeVelocityCommands(), true);
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::TurnToGoal);
// EXPECT_NEAR(rftp.cmd_ptr_->angular.z, 0.5*(M_PI_2/2-M_PI), 0.001);
//}
//
//TEST(TangentPlanner, whileLoop){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::MotionToGoal;
// rftp.initialized_ = true;
// // number of goal points:
// int goalNumbers = 5;
// rftp.plan_size_ = goalNumbers;
// // goal positions:
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// gp->x = 4.0;
// gp->y = 9.0;
// gp->theta = M_PI_2;
// for (int i=0; i<goalNumbers; i++){
// rftp.global_plan_.push_back(*gp);
// gp->x += 2.0;
// gp->y += 1.0;
// gp->theta = M_PI_2;
// }
// // while loop for multiple goals:
// uint counter = 0;
// rftp.range_msg_->max_range = 2.0;
// rftp.range_msg_->min_range = 0.001;
// for (int j=0; j<goalNumbers; j++){
// while (!rftp.isGoalReached() && counter!=100){
// counter++;
// // current position and range sensor:
// rftp.current_pose_->x = rand()%10;
// rftp.current_pose_->y = rand()%10;
// rftp.current_pose_->theta = rand()%3;
// rftp.range_msg_->range = rand()%2;
//
// rftp.computeVelocityCommands();
// EXPECT_EQ(rftp.computeVelocityCommands(), true);
// }
// std::cout<<"counter is:"<<counter<<std::endl;
// rftp.resetVariables();
// rftp.plan_size_--;
// counter = 0;
// rftp.goal_reached_ = false;
// }
//}
int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "tangent_planner_test");
ros::NodeHandle nh;
return RUN_ALL_TESTS();
}
src/main.cpp
Executable
+35
View File
@@ -0,0 +1,35 @@
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <ros/callback_queue.h>
#include "tangent_planner_node.h"
int main(int argc, char **argv) {
ROS_INFO("Main started");
ros::init(argc, argv, "tangent_planner_remote", ros::init_options::NoRosout);
Planner::TangentPlannerNode tangent_planner_node;
tangent_planner_node.start_thread();
//// integration into the portal control has been left for client
// ros::NodeHandle node_handler;
// ros::CallbackQueue Q;
// node_handler.setCallbackQueue(&Q);
// tangent_planner_node.start_patrolling = node_handler.advertiseService("nav_patrol",
// &Planner::TangentPlannerNode::start_patrolling_callback, &tangent_planner_node);
// tangent_planner_node.stop_patrolling = node_handler.advertiseService("nav_patrol_stop",
// &Planner::TangentPlannerNode::stop_patrolling_callback, &tangent_planner_node);
// tangent_planner_node.get_patrol_path_name = node_handler.advertiseService("nav_get_patrol_name",
// &Planner::TangentPlannerNode::get_patrol_path_name_callback, &tangent_planner_node);
// tangent_planner_node.get_current_status = node_handler.advertiseService("nav_get_status",
// &Planner::TangentPlannerNode::get_current_status_callback, &tangent_planner_node);
// tangent_planner_node.low_battery = node_handler.advertiseService("nav_low_bat", &Planner::TangentPlannerNode::low_battery_callback, &tangent_planner_node);
// while (ros::ok()) {
// Q.callOne(ros::WallDuration(1, 0.0));
// }
// ROS_INFO("shutdown");
return 0;
}
src/tangent_planner.cpp
Executable
+650
View File
@@ -0,0 +1,650 @@
#include "tangent_planner.h"
#include <cmath>
#include <limits>
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "parameters_helper.h"
#define M_PI 3.141592
#define M_PI_2 1.570796
using namespace Planner;
/* This algorithm has two main parts: "MotionToGoal" and "FollowBoundary".
*
* heuristic_distance = robot_to_obstacle + obstacle_to_goal distance
* d_reach = distance from robot to goal
* d_followed = minimum distance of obstacle to goal during the FollowBoundary part
*
* 1. Keep MotionToGoal in direction of the goal until sensing an obstacle,
* If heuristic_distance is decreasing continues step 1 else switch to FollowBoundary part:
* 2. Keep FollowBoundary while d_followed < d_reach
* Go to step 1 when d_reach > d_followed
* 3. Continues until the goal is reached.
*/
// NOTE: code snippets inside M_DEBUG blocks are for debugging purpose
TangentPlanner::TangentPlanner() {
#if M_DEBUG
// open log files
tof_logfile = fopen(
Planner::getParam<std::string>("tof_log", "/home/p_46_ws/src/tangent_planner/data/tof_log.txt").c_str(),
"w");
odometry_logfile = fopen(Planner::getParam<std::string>("odometry_log",
"/home/p_46_ws/src/tangent_planner/data/odometry_log.txt").c_str(),
"w");
state_logfile = fopen(
Planner::getParam<std::string>("state_log", "/home/p_46_ws/src/tangent_planner/data/state_log.txt").c_str(),
"w");
goal_logfile = fopen(
Planner::getParam<std::string>("goal_log", "/home/p_46_ws/src/tangent_planner/data/goal_log.txt").c_str(),
"w");
twist_logfile = fopen(
Planner::getParam<std::string>("twist_log", "/home/p_46_ws/src/tangent_planner/data/twist_log.txt").c_str(),
"w");
error_logfile = fopen(
Planner::getParam<std::string>("error_log", "/home/p_46_ws/src/tangent_planner/data/error_log.txt").c_str(),
"w");
terminal_logfile = fopen(Planner::getParam<std::string>("terminal_log",
"/home/p_46_ws/src/tangent_planner/data/terminal_log.txt").c_str(),
"w");
param_logfile = fopen(
Planner::getParam<std::string>("param_log", "/home/p_46_ws/src/tangent_planner/data/param_log.txt").c_str(),
"w");
center_logfile = fopen(Planner::getParam<std::string>("center_log",
"/home/p_46_ws/src/tangent_planner/data/center_log.txt").c_str(),
"w");
custom_logfile = fopen(Planner::getParam<std::string>("param_log",
"/home/p_46_ws/src/tangent_planner/data/custom_log.txt").c_str(),
"w");
#endif
dfollowed_ = std::numeric_limits<float>::max();
heuristic_distance_ = 0.f;
prev_heuristic_distance_ = 0.f;
range_msg_ = std::make_shared<sensor_msgs::Range>();
state_ = MotionToGoal;
cmd_ptr_ = std::make_shared<geometry_msgs::Twist>();
current_pose_ = std::make_shared<geometry_msgs::Pose2D>();
start_follow_point_ = std::make_shared<geometry_msgs::Pose2D>();
range_msg_->min_range = 0.02;
range_msg_->max_range = 2;
range_msg_->field_of_view = 0.4;
for (int i = 0; i < 50; ++i)this->frq_[i] = 0.01;
}
TangentPlanner::~TangentPlanner() {
#if M_DEBUG
// close log files
fclose(tof_logfile);
fclose(error_logfile);
fclose(odometry_logfile);
fclose(twist_logfile);
fclose(state_logfile);
fclose(goal_logfile);
fclose(terminal_logfile);
fclose(center_logfile);
fclose(custom_logfile);
#endif
}
// return the sign of the given number
int TangentPlanner::sign(float number) {
if (number > 0)
return 1;
else if (number < 0)
return -1;
else
return 0;
}
// Keeps the angle interval in: [-M_PI, 0] or [0, M_PI]
void TangentPlanner::normalizeAngle(float &angle) {
if (abs(angle - angle_error_t_) > (0.9 * M_PI)) {
angle = angle_error_t_;
}
if (abs(angle) > 2.0 * M_PI) {
angle = angle - (2.0 * M_PI);
}
if (angle < 0){
angle = (2*M_PI) + angle;
}
}
float TangentPlanner::normalizeAngle(float &&angle) {
if (abs(angle - angle_error_t_) > (0.9 * M_PI)) {
angle = angle_error_t_;
}
if (abs(angle) > 2.0 * M_PI) {
angle = abs(angle) - (2.0 * M_PI);
}
if (angle < 0){
angle = (2*M_PI) + angle;
}
return angle;
}
float TangentPlanner::convertAtan2(float&& angle){
if (angle < 0){
angle = (2*M_PI) + angle;
}
return angle;
}
float TangentPlanner::average(float* array, std::size_t index, float av){
if(index>50){
return av/50.0;
}
av+=array[0];
return average(&array[1], ++index, av);
}
void TangentPlanner::shift(float* array,std::size_t index){
if(index>50)return;
array[0]=array[1];
shift(&array[1],++index);
}
bool TangentPlanner::setGoal() {
while (!global_plan_.empty()) {
float dis_x = global_plan_.front()->x - current_pose_->x;
float dis_y = global_plan_.front()->y - current_pose_->y;
float dis = hypot(dis_x, dis_y);
if (dis < valid_dis_to_goal_) {
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\tpoint x=%f, y = %f, theta=%f is invalid\n", cycle_counter_,
global_plan_.front()->x, global_plan_.front()->y,
global_plan_.front()->theta);
#endif
global_plan_.erase(global_plan_.begin());
} else {
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\tgoal set to x=%f, y=%f, theta=%f\n", cycle_counter_,
global_plan_.front()->x, global_plan_.front()->y,
global_plan_.front()->theta);
fprintf(goal_logfile, "%d, %f, %f, %f\n", cycle_counter_, global_plan_.front()->x, global_plan_.front()->y,
global_plan_.front()->theta);
#endif
error_angle_ = 0;
error_y_ = 0;
error_x_ = 0;
i_error_angle_ = 0;
i_error_x_ = 0;
i_error_y_ = 0;
break;
}
}
if (global_plan_.empty()) {
std::shared_ptr<geometry_msgs::Pose2D> t_goal = std::make_shared<geometry_msgs::Pose2D>();
t_goal->x = 0.f;
t_goal->y = 0.f;
global_plan_.push_back(t_goal);
return true;
}
return true;
}
// When heuristic_distance is increasing it means that we are in local minima and there is an obstacle that we should follow its boundary
bool TangentPlanner::foundLocalMinima() {
return prev_heuristic_distance_ - heuristic_distance_ < 2 * xy_goal_tolerance_;
}
bool TangentPlanner::isAtGoal() {
return goal_distance_ < xy_goal_tolerance_;
}
bool TangentPlanner::isAtGoalOrientation() {
return std::abs(goal_orientation_) < yaw_goal_tolerance_;
}
void TangentPlanner::resetVariables() {
dfollowed_ = std::numeric_limits<float>::max();
map_.clear();
}
// check the range msg to see if there is any obstacle
bool TangentPlanner::foundObstacle() {
if (range_msg_->range > range_msg_->min_range * sensor_msg_scale_ &&
range_msg_->range <= robot_front_ * sensor_msg_scale_) {
return true;
}
return false;
}
float TangentPlanner::calculateDistance(std::shared_ptr<geometry_msgs::Pose2D> point,
std::shared_ptr<geometry_msgs::Pose2D> target) {
float dis_x = point->x - target->x;
float dis_y = point->y - target->y;
return hypot(dis_x, dis_y);
}
std::tuple<std::shared_ptr<geometry_msgs::Pose2D>, std::shared_ptr<geometry_msgs::Pose2D>>
TangentPlanner::calculateCenter() {
std::shared_ptr<geometry_msgs::Pose2D> center = std::make_shared<geometry_msgs::Pose2D>();
std::shared_ptr<geometry_msgs::Pose2D> center_path = std::make_shared<geometry_msgs::Pose2D>();
center->x = 0.0;
center->y = 0.0;
center->theta = 0.0;
center_path->x = 0.0;
center_path->y = 0.0;
center_path->theta = 0.0;
std::shared_ptr<geometry_msgs::Pose2D> max_point = std::make_shared<geometry_msgs::Pose2D>();
max_point->x = 0.f;
max_point->y = 0.f;
int count = 0;
for (auto &&point: map_) {
if (point->obstacle) {
center->x += point->obstacle->x;
center->y += point->obstacle->y;
count++;
}
if (point->robot->x > max_point->x) {
max_point->x = point->robot->x;
}
if (point->robot->y > max_point->y) {
max_point->y = point->robot->y;
}
center_path->x += point->robot->x;
center_path->y += point->robot->y;
}
center->x /= count;
center->y /= count;
center_path->x /= map_.size();
center_path->y /= map_.size();
float radios = calculateDistance(max_point, center_path);
bool point_90 = false;
bool point_180 = false;
bool point_270 = false;
turned_around_obs_ = false;
for (auto &&point: map_) {
float theta = convertAtan2(atan2((point->robot->x - center_path->x), (point->robot->y - center_path->y)));
float l = theta * radios;
if (l > 0.3 * M_PI * radios && l < 0.6 * M_PI * radios) {
point_90 = true;
}
if (l > 0.8 * M_PI * radios && l < 1.2 * M_PI * radios) {
point_180 = true;
}
if (l > 1.3 * M_PI * radios && l < 1.7 * M_PI * radios) {
point_270 = true;
}
}
if (point_90 && point_180 && point_270) {
turned_around_obs_ = true;
}
return std::make_tuple(center, center_path);
}
float TangentPlanner::calculateFactor() {
float latest_ob = 0.f;
for (size_t i = 0; i < map_.size(); ++i) {
if (map_.at(i)->obstacle) {
latest_ob = static_cast<float >(i);
}
}
return latest_ob / (factor_scale_ * static_cast<int>(map_.size()));
}
// FollowBoundary part of the algorithm: it turns right or left against obstacle and tries to follow its boundary
void TangentPlanner::followBoundaries() {
if (foundObstacle()) {
// obstacle is ahead try to turn left or right
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\tfollowing boundary.... still seeing obstacle", cycle_counter_);
#endif
std::shared_ptr<geometry_msgs::Pose2D> obs_point = std::make_shared<geometry_msgs::Pose2D>();
float obs_distance;
// position of sensed point on obstacle:
obs_point->x = range_msg_->range * cos(current_pose_->theta) + current_pose_->x;
obs_point->y = range_msg_->range * sin(current_pose_->theta) + current_pose_->y;
// distance from obstacle to goal:
obs_distance = calculateDistance(global_plan_.front(), obs_point);
if (obs_distance < dfollowed_) // finds the d_followed here
{
dfollowed_ = obs_distance;
}
if (map_.empty()) {
float rotate_i = M_PI_2 - (range_msg_->range / (robot_front_ - 0.1) * sensor_msg_scale_) * M_PI_2;
std::shared_ptr<geometry_msgs::Pose2D> right = std::make_shared<geometry_msgs::Pose2D>();
std::shared_ptr<geometry_msgs::Pose2D> left = std::make_shared<geometry_msgs::Pose2D>();
right->theta = current_pose_->theta - rotate_i;
right->x = current_pose_->x + (1 / range_msg_->range) * cos(right->theta);
right->y = current_pose_->y + (1 / range_msg_->range) * sin(right->theta); // 0
left->theta = current_pose_->theta + rotate_i;
left->x = current_pose_->x + (1 / range_msg_->range) * cos(left->theta);
left->y = current_pose_->y + (1 / range_msg_->range) * sin(left->theta);; // 0
if (calculateDistance(global_plan_.front(), left) > calculateDistance(global_plan_.front(), right)) {
error_angle_ = right->theta;
error_y_ = right->y - current_pose_->y; // 0
} else {
error_angle_ = left->theta;
error_y_ = left->y - current_pose_->y; // 0
}
} else {
auto centers = calculateCenter();
std::shared_ptr<geometry_msgs::Pose2D> ob_center2current = std::make_shared<geometry_msgs::Pose2D>();
std::shared_ptr<geometry_msgs::Pose2D> map_center2current = std::make_shared<geometry_msgs::Pose2D>();
ob_center2current->x = current_pose_->x - std::get<0>(centers)->x + 0.0001;
ob_center2current->y = current_pose_->y - std::get<0>(centers)->y + 0.0001;
map_center2current->x = current_pose_->x - std::get<1>(centers)->x + 0.0001;
map_center2current->y = current_pose_->y - std::get<1>(centers)->y + 0.0001;
float dot =
(ob_center2current->x * (map_center2current->x)) + (ob_center2current->y * map_center2current->y);
float cross =
(ob_center2current->x * map_center2current->y) - (ob_center2current->y * map_center2current->x);
error_angle_ = convertAtan2(atan2(cross, dot));
error_y_ = (1 / range_msg_->range) * sin(error_angle_); // 0
}
resetError();
error_x_ = 0.f;
// stop if distance to obstacle is less than 0.1 (error_x_ and error_y_ became zero, only error_angle_ exist)
controller_ = Controller::Motion;
if (range_msg_->range > range_msg_->min_range * sensor_msg_scale_ &&
range_msg_->range < 0.1 * sensor_msg_scale_) {
resetError();
float rotate_i = M_PI_2 - (0.01/ (robot_front_ - 0.1)) * M_PI_2;
error_angle_ = current_pose_->theta - rotate_i;
error_y_ = 0.f;
controller_ = Controller::Turn;
}
normalizeAngle(error_angle_);
// FollowBoundary part terminates here:
state_ = BugState::FollowBoundary;
map_.push_back(std::make_shared<Map>(obs_point, current_pose_, global_plan_.front()));
} else {
// no obstacle, go forward and try to turn back
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\tno obstacle anymore", cycle_counter_);
#endif
float factor = calculateFactor();
auto centers = calculateCenter();
std::shared_ptr<geometry_msgs::Pose2D> home_point = std::make_shared<geometry_msgs::Pose2D>();
float temp_distance = calculateDistance(global_plan_.front(), home_point);
if (calculateDistance(std::get<1>(centers), home_point) > temp_distance && temp_distance > 0.00001) {
global_plan_.clear();
global_plan_.push_back(home_point);
resetVariables();
return;
}
std::shared_ptr<geometry_msgs::Pose2D> ob_center2current = std::make_shared<geometry_msgs::Pose2D>();
std::shared_ptr<geometry_msgs::Pose2D> map_center2current = std::make_shared<geometry_msgs::Pose2D>();
ob_center2current->x = current_pose_->x - std::get<0>(centers)->x;
ob_center2current->y = current_pose_->y - std::get<0>(centers)->y;
map_center2current->x = current_pose_->x - std::get<1>(centers)->x;
map_center2current->y = current_pose_->y - std::get<1>(centers)->y;
#if M_DEBUG
fprintf(center_logfile, "%d, %f, %f, %f, %f, %f\n", cycle_counter_, std::get<0>(centers)->x,
std::get<0>(centers)->y, std::get<1>(centers)->x, std::get<1>(centers)->y, factor);
#endif
resetError();
float dot = (ob_center2current->x * map_center2current->x) + (ob_center2current->y * map_center2current->y);
float cross = (ob_center2current->x * map_center2current->y) - (ob_center2current->y * map_center2current->x);
error_angle_ = (1 - factor) * (error_angle_) - factor * convertAtan2(atan2(cross, dot));
error_x_ = ((1 - factor) * (goal_distance_) -
factor * calculateDistance(std::get<0>(centers), std::get<1>(centers))) * cos(error_angle_);
error_y_ = 0; // 0
normalizeAngle(error_angle_);
// FollowBoundary part terminates here:
goal_distance_ < dfollowed_ ? state_ = BugState::TurnToGoal : BugState::FollowBoundary;
map_.push_back(std::make_shared<Map>(current_pose_, global_plan_.front()));
controller_ = Controller::Motion;
}
// reset variables if followBoundary part has been finished:
if (state_ == BugState::TurnToGoal) {
resetError();
controller_ = Controller::Turn;
resetVariables();
} else {
float dis = calculateDistance(start_follow_point, current_pose_);
float check_point = abs(
atan2(start_follow_point->y, start_follow_point->x) - atan2(current_pose_->y, current_pose_->x));
if (dis < 0.001 && check_point > M_PI_2 * 0.8
&& global_plan_.front()->x != 0 && global_plan_.front()->y != 0.f) {
resetError();
std::shared_ptr<geometry_msgs::Pose2D> t_goal = std::make_shared<geometry_msgs::Pose2D>();
t_goal->x = 0.f;
t_goal->y = 0.f;
global_plan_.clear();
global_plan_.push_back(t_goal);
controller_ = Controller::Turn;
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\t can't reach goal going back home %d\n", cycle_counter_, state_);
#endif
}
}
}
// This function selects the range sensor's min_angle/max_angle that has min dist from robot to obs and obs to goal
void TangentPlanner::selectDiscontinuityPoint() {
start_follow_point_ = current_pose_;
prev_heuristic_distance_ = heuristic_distance_;
float angle = 0.f;
float obstacle_to_goal_distance = 0.f;
float range_sensor_minAngle = -range_msg_->field_of_view / 2;
float range_sensor_maxAngle = range_msg_->field_of_view / 2;
// From range sensor's min_angle/max angle selects the one that has minimum distance from robot to obs and obs to goal:
obstacle_to_goal_distance = sqrt(square(range_msg_->range) + square(goal_distance_) -
2 * range_msg_->range * goal_distance_ * cos(abs(range_sensor_minAngle)));
heuristic_distance_ = obstacle_to_goal_distance + range_msg_->range;
followBoundaries();
}
// This function sets cmd_ptr_ (from geometry_msgs::Twist type) for publishing and sending these velocity commands to robot's motor to move
void TangentPlanner::updateVelocity() {
shift(this->frq_);
this->frq_[49]=std::chrono::duration<float>(std::chrono::system_clock::now() - this->time_).count();
float dt= average(this->frq_);
this->time_= std::chrono::system_clock::now();
if (controller_ != prev_controller_) {
error_angle_ = 0.f;
error_x_ = 0.f;
error_y_ = 0.f;
i_error_angle_ = 0.f;
i_error_x_ = 0.f;
i_error_y_ = 0.f;
}
if (error_angle_ > M_PI || -M_PI > error_angle_) {
error_angle_ = (2 * M_PI) - sign(error_angle_) * error_angle_;
}
prev_controller_ = controller_;
if (controller_ == Controller::Motion) {
float e1, e2, e3;
e1 = error_x_ * cos(current_pose_->theta) + error_y_ * sin(current_pose_->theta);
e2 = -error_x_ * sin(current_pose_->theta) + error_y_ * cos(current_pose_->theta);
e3 = error_angle_;
float v_d = 0.01;
float w_d = 0.01;
k1_ = 2 * c_ * sqrt((pow(w_d, 2)) + (b_ * pow(v_d, 2)));
k3_ = k1_;
k2_ = b_ * abs(v_d);
float u1 = -k1_ * e1;
float u2 = (-k2_ * sign(v_d) * e2) - (k3_ * e3);
float linear_velocity = v_d * cos(error_angle_) - u1;
float angular_velocity = w_d - u2;
cmd_ptr_->angular.x = 0.f;
cmd_ptr_->angular.y = 0.f;
cmd_ptr_->angular.z = (abs(angular_velocity) > max_rot_vel_) ? sign(angular_velocity) * max_rot_vel_
: angular_velocity;
cmd_ptr_->linear.x = (abs(linear_velocity) > max_trans_vel_) ? sign(linear_velocity) * max_trans_vel_
: linear_velocity;
cmd_ptr_->linear.z = 0.f;
cmd_ptr_->linear.y = 0.f;
} else {
// TODO: remove next line or check why this returns positive for negative angle
error_angle_ = std::abs(error_angle_) < yaw_goal_tolerance_ ? 0.f : error_angle_;
float angular_vel = kp_ * error_angle_ + kp_i_ * i_error_angle_;
cmd_ptr_->angular.x = 0.f;
cmd_ptr_->angular.y = 0.f;
cmd_ptr_->angular.z = (abs(angular_vel) > max_rot_vel_) ? sign(angular_vel) * max_rot_vel_
: angular_vel;
cmd_ptr_->linear.x = 0.f;
cmd_ptr_->linear.y = 0.f;
cmd_ptr_->linear.z = 0.f;
float temp = error_angle_ * dt;
//normalizeAngle(temp);
i_error_angle_ += temp;
prev_angle_error_ = error_angle_;
}
#if M_DEBUG
fprintf(error_logfile, "[%d],%f,%f,%f\n", cycle_counter_, error_x_, error_y_,
error_angle_);
fprintf(twist_logfile, "[%d], %f, %f, %f, %f, %f, %f\n", cycle_counter_, cmd_ptr_->linear.x, cmd_ptr_->linear.y,
cmd_ptr_->linear.z, cmd_ptr_->angular.x, cmd_ptr_->angular.y, cmd_ptr_->angular.z);
#endif
}
/* This function computes the necessary velocity commands for moving the robot in every state
especially finds the direction of moving in different states by calling different functions.*/
void TangentPlanner::updateController() {
if (state_ == BugState::StopAtGoal) {
error_angle_ = 0.f;
error_x_ = 0.f;
error_y_ = 0.f;
resetVariables();
global_plan_.erase(global_plan_.begin());
if (!setGoal()) {
ROS_WARN("Path ended");
state_ = BugState::StopAtGoal;
}
state_ = BugState::TurnToGoal;
resetError();
controller_ = Controller::Turn;
} else if (state_ == BugState::TurnToGoal) {
error_x_ = 0.f;
error_y_ = 0.f;
controller_ = Controller::Turn;
} else if (state_ == BugState::OrientToGoal) {
resetError();
error_x_ = 0.f;
error_y_ = 0.f;
i_error_x_ = 0.f;
i_error_y_ = 0.f;
controller_ = Controller::Turn;
} else if (state_ == BugState::MotionToGoal) {
if (foundObstacle()) {
selectDiscontinuityPoint();
} else {
resetError();
controller_ = Controller::Motion;
}
} else if (state_ == BugState::FollowBoundary) {
followBoundaries();
}
}
/* This function controls the robot states and decides when to switch from MotionToGoal to FollowBoundary state.
Switching from FollowBoundary to MotionToGoal state occurs in followBoundaries() function.*/
void TangentPlanner::updateState() {
if (state_ != BugState::FollowBoundary) {
if (error_angle_ > M_PI_2 * 0.25 && !isAtGoal()) {
if(state_ != BugState::TurnToGoal){
i_error_angle_ = 0;
}
state_ = BugState::TurnToGoal;
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\t state updated to TurnToGoal %d\n", cycle_counter_, state_);
#endif
} else if (isAtGoal() && !isAtGoalOrientation()) {
if (state_ != BugState::OrientToGoal) {
i_error_angle_ = 0;
}
state_ = BugState::OrientToGoal;
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\t state updated to OrientToGoal %d\n", cycle_counter_, state_);
#endif
} else if (isAtGoal() && isAtGoalOrientation()) {
state_ = BugState::StopAtGoal;
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\t state updated to StopAtGoal %d\n", cycle_counter_, state_);
#endif
} else {
state_ = BugState::MotionToGoal;
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\t state updated to MotionToGoal %d\n", cycle_counter_, state_);
#endif
if (foundObstacle() && foundLocalMinima()) {
state_ = BugState::FollowBoundary;
#if M_DEBUG
fprintf(terminal_logfile, "CYCLE[%d]\t state updated to FollowBoundary %d\n", cycle_counter_, state_);
#endif
}
}
}
#if M_DEBUG
fprintf(state_logfile, "%d, %d\n", cycle_counter_, state_);
#endif
}
// This function finds the direction to goal and the distance of robot to goal at start of the algorithm:
// global_plan_ has position of the goal.
void TangentPlanner::calculateErrors() {
#if M_DEBUG
fprintf(odometry_logfile, "%d, %f, %f, %f\n", cycle_counter_, current_pose_->x, current_pose_->y,
current_pose_->theta);
fprintf(tof_logfile, "%d, %f, %f, %f, %f\n", cycle_counter_, range_msg_->field_of_view, range_msg_->range,
range_msg_->max_range, range_msg_->min_range);
#endif
#if M_DEBUG
fprintf(terminal_logfile, "\nCYCLE[%d]\tcurrent position x=%f, y=%f, theta=%f\n", cycle_counter_, current_pose_->x,
current_pose_->y,
current_pose_->theta);
#endif
error_x_ = global_plan_.front()->x - current_pose_->x;
error_y_ = global_plan_.front()->y - current_pose_->y;
goal_distance_ = calculateDistance(global_plan_.front(), current_pose_);
goal_orientation_ = global_plan_.front()->theta - current_pose_->theta;
error_angle_ = state_ != BugState::OrientToGoal ? normalizeAngle(atan2(error_y_, error_x_)) - current_pose_->theta : goal_orientation_;
normalizeAngle(error_angle_);
#if M_DEBUG
fprintf(error_logfile, "%d, %f, %f, %f, %f\n", cycle_counter, error_x_, error_y_, error_angle_, atan2(error_y_, error_x_));
#endif
#if M_DEBUG
ROS_ERROR("CYCLE[%d]\terror x=%f, y=%f, theta=%f", cycle_counter, error_x_, error_y_, error_angle_);
fprintf(terminal_logfile, "CYCLE[%d]\t error x=%f, y=%f, theta=%f, angle_error_t=%f\n", cycle_counter, error_x_, error_y_,
error_angle_, angle_error_t_);
#endif
}
// Call this function to start running of the algorithm
// the input argument is previous velocity command of the robot.
bool TangentPlanner::computeVelocityCommands() {
if (global_plan_.empty()) {
ROS_WARN("Planner is not initialized or end of path");
return false;
}
// step 1
calculateErrors();
// step 2
updateState();
// step 3
updateController();
// step 4
updateVelocity();
cycle_counter_++;
return true;
}
src/tangent_planner_node.cpp
Executable
+125
View File
@@ -0,0 +1,125 @@
#include "tangent_planner_node.h"
#include <cstdio>
#include "parameters_helper.h"
#include <ros/callback_queue.h>
Planner::TangentPlannerNode::TangentPlannerNode() {
sensor_msg_scale_ = Planner::getParam<float>("range_sensor_scale", 1);
factor_scale_ = Planner::getParam<float>("factor_scale_", 2);
kp_ = Planner::getParam<float>("kp", 0.05);
kp_i_ = Planner::getParam<float>("kp_i", 0.3);
xy_goal_tolerance_ = Planner::getParam<float>("xy_goal_tolerance", 0.05);
yaw_goal_tolerance_ = Planner::getParam<float>("yaw_goal_tolerance", 0.05);
max_trans_vel_ = Planner::getParam<float>("max_trans_vel", 1);
max_rot_vel_ = Planner::getParam<float>("max_rot_vel", 0.5);
robot_front_ = Planner::getParam<float>("robot_front", 0.05);
robot_back_ = Planner::getParam<float>("robot_back", 0.05);
robot_width_min_ = Planner::getParam<float>("robot_width_min", 0.06);
robot_width_max_ = Planner::getParam<float>("robot_width_max", 0.08);
valid_dis_to_goal_ = Planner::getParam<float>("valid_dis_to_goal", 0.2);
read_path_file(Planner::getParam<std::string>("goal_path", "/home/p_46_ws/src/tangent_planner/data/straight-front&back-path.txt").c_str());
c_ = Planner::getParam<float>("c", 0.2);
b_ = Planner::getParam<float>("b", 0.2);
#if M_DEBUG
fprintf(param_logfile,
"sensor_msg_scale_ is: %f\nkp_ is: %f\nkp_i_ is: %f\nxy_goal_tolerance_ is: %f\nyaw_goal_tolerance_ is: %f\nmax_trans_vel_ is: %f\nmax_rot_vel_ is: %f\nrobot_front_ is: %f\nrobot_back_ is: %f\nrobot_width_min_ is: %f\nrobot_width_max_ is: %f\nvalid_dis_to_goal_ is: %f\n",
sensor_msg_scale_, kp_, kp_i_, xy_goal_tolerance_, yaw_goal_tolerance_, max_trans_vel_, max_rot_vel_,
robot_front_, robot_back_, robot_width_min_, robot_width_max_, valid_dis_to_goal_);
fclose(param_logfile);
#endif
}
Planner::TangentPlannerNode::~TangentPlannerNode() {
}
// get path name, load path file form storage and set points of path to global_plan_
// TODO: add comment for correctly reading path file because of deference between path file and robot frame
void Planner::TangentPlannerNode::read_path_file(std::string 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 = convertAtan2(yaw);
global_plan_.insert(global_plan_.begin(), pose);
}
setGoal();
file.reset();
}
void Planner::TangentPlannerNode::set_publisher(const ros::Publisher &publisher) {
twist_publisher = publisher;
}
void Planner::TangentPlannerNode::odometry_subscriber_callback(const nav_msgs::Odometry &msg) {
current_pose_->y = msg.pose.pose.position.x;
current_pose_->x = msg.pose.pose.position.y;
tf::Quaternion q(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
current_pose_->theta = yaw;
computeVelocity();
}
void Planner::TangentPlannerNode::tof_subscriber_callback(const sensor_msgs::Range &msg) {
range_msg_->field_of_view = msg.field_of_view;
range_msg_->min_range = msg.min_range * sensor_msg_scale_;
range_msg_->max_range = msg.max_range * sensor_msg_scale_;
range_msg_->range = msg.range * sensor_msg_scale_;
}
void Planner::TangentPlannerNode::twist_publisher_callback() {
geometry_msgs::Twist msg;
msg.linear.x = cmd_ptr_->linear.y;
msg.linear.y = cmd_ptr_->linear.x;
msg.linear.z = cmd_ptr_->linear.z;
msg.angular.x = cmd_ptr_->angular.x;
msg.angular.y = cmd_ptr_->angular.y;
msg.angular.z = cmd_ptr_->angular.z;
twist_publisher.publish(msg);
}
void Planner::TangentPlannerNode::computeVelocity() {
bool status = computeVelocityCommands();
if (!status) {
ROS_WARN("End of path or not initialized");
}
twist_publisher_callback();
}
// is planner algorithm thread started or waiting for start request?
bool Planner::TangentPlannerNode::is_started() {
return started_;
}
// starting the cycle of subscribing to odometry and tof and publishing velocity to motor node as a thread until is_started gets false
void Planner::TangentPlannerNode::start_thread() {
ROS_WARN("Navigation started");
ros::NodeHandle node_handler;
ros::CallbackQueue Q;
node_handler.setCallbackQueue(&Q);
ros::Subscriber odometry_subscriber = node_handler.subscribe("/odometry",
1,
&Planner::TangentPlannerNode::odometry_subscriber_callback,
this);
ros::Subscriber tof_subscriber = node_handler.subscribe("/SensorNode/tof",
1,
&Planner::TangentPlannerNode::tof_subscriber_callback,
this);
set_publisher(node_handler.advertise<geometry_msgs::Twist>("/cmd_vel", 1));
while (ros::ok()) {
Q.callOne(ros::WallDuration(1, 0.0));
}
ROS_WARN("Navigation stopped");
}