From c114f3ba96551464e2909e3ed8c89e8183c76067 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 2 Nov 2017 17:49:32 +0100 Subject: [PATCH 001/200] Start path planner interface. --- include/control_fsm/path_planner.hpp | 37 +++++++++++++++++++++++----- 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/include/control_fsm/path_planner.hpp b/include/control_fsm/path_planner.hpp index 0ff9795..87c15b8 100644 --- a/include/control_fsm/path_planner.hpp +++ b/include/control_fsm/path_planner.hpp @@ -5,18 +5,43 @@ #include "include/control_fsm/go_to_state.hpp" #include #include "include/control_fsm/tools/control_pose.hpp" +#include + + +#define FIELD_LENGTH 20 + + + +class Node{ +private: + float32 x; + float32 y; + + // heuristic, use Euclidian distance + float32 h; + float32 g; + // Sum of g and h + float32 f; + +}; class PathPlanner{ private: - -ascend_msgs::PathPlannerPlan current_plan_; + Node graph[FIELD_LENGTH][FIELD_LENGTH]; + + std::list open_list; + std::list closed_list; + + std::list plan; + + Node end_node; + Node start_node; + + ascend_msgs::PathPlannerPlan current_plan_; public: - PathPlanner(); - - - + PathPlanner(float32 current_x, float32 current_y, float32 target_x, float32 target_y); }; #endif // PATH_PLANNER_HPP From 17f31eb0fbb3a173f9d2d4fa6998c23e141b9382 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 5 Nov 2017 12:03:20 +0100 Subject: [PATCH 002/200] Add some variables for path planner --- include/control_fsm/path_planner.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/control_fsm/path_planner.hpp b/include/control_fsm/path_planner.hpp index 87c15b8..11a1d0c 100644 --- a/include/control_fsm/path_planner.hpp +++ b/include/control_fsm/path_planner.hpp @@ -22,7 +22,8 @@ class Node{ float32 g; // Sum of g and h float32 f; - +public: + Node(float32 x, float32 y):x(x), y(y){}4 }; From 048d378f71eb181a523dfd63c4299f5aafc73eef Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 6 Nov 2017 21:00:08 +0100 Subject: [PATCH 003/200] Add basic path planner functions. --- include/control/planner/path_planner.hpp | 13 ++++++++++-- src/nodes/path_planner.cpp | 27 ++++++++++++++++++++++-- 2 files changed, 36 insertions(+), 4 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 7d042b6..184b43c 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -5,6 +5,8 @@ #include #include "control/tools/control_pose.hpp" #include +#include +#include #define FIELD_LENGTH 20 @@ -22,14 +24,18 @@ class Node{ // Sum of g and h float32 f; public: - Node(float32 x, float32 y):x(x), y(y){}4 + Node(float32 x, float32 y, float32 g, float32 h):x(x), y(y), g(g), h(h){f = g+h;} + // Implemented only for the closed list priority queue + friend bool operator< (const Node &lhs, const Node &rhs); + float32 getX() const {return x;} + float32 getY() const {return y;} }; class PathPlanner{ private: Node graph[FIELD_LENGTH][FIELD_LENGTH]; - std::list open_list; + std::priority_queue open_list; std::list closed_list; std::list plan; @@ -41,6 +47,9 @@ class PathPlanner{ public: PathPlanner(float32 current_x, float32 current_y, float32 target_x, float32 target_y); + void initializeGraph(); + float32 calculateHeuristic(float32 x, float32 y); + float32 calculateG(float32 x, float32 y); }; #endif // PATH_PLANNER_HPP diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 883e2f6..9079b16 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -1,7 +1,30 @@ #include "control/planner/path_planner.hpp" -PathPlanner::PathPlanner() -{ +friend bool operator< (const Node &lhs, const Node &rhs){ + return lhs.f > rhs.f; +} + + +PathPlanner::PathPlanner(float32 current_x, float32 current_y, float32 target_x, float32 target_y){ + start_node = Node(current_x, current_y, 0, 0); + end_node = Node(target_x, target_y, 0, 0); + +} + +void PathPlanner::initializeGraph(){ + for (x = 0; x < FIELD_LENGTH; x++){ + for (y = 0; y < FIELD_LENGTH; y++){ + graph[x][y] = Node(x, y, computeHeuristic(x,y), calculateG(x,y)); + open_list.push(graph[x][y]); + } + } +} +float32 PathPlanner::calculateHeuristic(float32 x, float32 y){ + return (sqrt((x - end_node.getX())*(x - end_node.getX()) + (y - end_node.getY())*(y - end_node.getY()))); } + +float32 PathPlanner::calculateG(float32 x, float32 y){ + return(sqrt((start_node.getX() - x)*(start_node.getX() - x) + (start_node.getY() - y)*(start_node.getY() - y))); +} \ No newline at end of file From 1ad93854e6f6895c98f0fb8352ca96e430c4dc09 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 6 Nov 2017 21:10:52 +0100 Subject: [PATCH 004/200] Fix bug in for loop. --- include/control/planner/path_planner.hpp | 2 +- src/nodes/path_planner.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 184b43c..7957041 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -12,7 +12,6 @@ #define FIELD_LENGTH 20 - class Node{ private: float32 x; @@ -31,6 +30,7 @@ class Node{ float32 getY() const {return y;} }; + class PathPlanner{ private: Node graph[FIELD_LENGTH][FIELD_LENGTH]; diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 9079b16..b3c10c2 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -13,8 +13,8 @@ PathPlanner::PathPlanner(float32 current_x, float32 current_y, float32 target_x, } void PathPlanner::initializeGraph(){ - for (x = 0; x < FIELD_LENGTH; x++){ - for (y = 0; y < FIELD_LENGTH; y++){ + for (int x = 0; x < FIELD_LENGTH; x++){ + for (int y = 0; y < FIELD_LENGTH; y++){ graph[x][y] = Node(x, y, computeHeuristic(x,y), calculateG(x,y)); open_list.push(graph[x][y]); } From 3f9b18e1937e29508a1d31ce296c466413fe1330 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 6 Nov 2017 21:19:17 +0100 Subject: [PATCH 005/200] Add default constructor for node class --- include/control/planner/path_planner.hpp | 1 + src/nodes/path_planner.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 7957041..b4cb02b 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -23,6 +23,7 @@ class Node{ // Sum of g and h float32 f; public: + Node(); Node(float32 x, float32 y, float32 g, float32 h):x(x), y(y), g(g), h(h){f = g+h;} // Implemented only for the closed list priority queue friend bool operator< (const Node &lhs, const Node &rhs); diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index b3c10c2..73df9ab 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -1,7 +1,7 @@ #include "control/planner/path_planner.hpp" -friend bool operator< (const Node &lhs, const Node &rhs){ +bool operator< (const Node &lhs, const Node &rhs){ return lhs.f > rhs.f; } From a0fa6ff7ec2f5be7585a06eed1bf8df30f1c6134 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 6 Nov 2017 22:13:57 +0100 Subject: [PATCH 006/200] Change Node objects to pointers in PathPlanner and fix bugs. --- include/control/planner/path_planner.hpp | 54 +++++++++++++----------- src/nodes/path_planner.cpp | 39 +++++++++++------ 2 files changed, 54 insertions(+), 39 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index b4cb02b..b2030b9 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -7,6 +7,8 @@ #include #include #include +#include +#include #define FIELD_LENGTH 20 @@ -14,43 +16,45 @@ class Node{ private: - float32 x; - float32 y; - - // heuristic, use Euclidian distance - float32 h; - float32 g; - // Sum of g and h - float32 f; + float x; + float y; + + // heuristic, use Euclidian distance + float h; + float g; + // Sum of g and h + float f; public: - Node(); - Node(float32 x, float32 y, float32 g, float32 h):x(x), y(y), g(g), h(h){f = g+h;} - // Implemented only for the closed list priority queue - friend bool operator< (const Node &lhs, const Node &rhs); - float32 getX() const {return x;} - float32 getY() const {return y;} + Node(); + Node(float x, float y, float g, float h):x(x), y(y), g(g), h(h){f = g+h;} + + float getX() const {return x;} + float getY() const {return y;} + float getF() const {return f;} + + // Implemented only for the closed list priority queue + friend bool operator< (const Node &lhs, const Node &rhs); }; class PathPlanner{ private: - Node graph[FIELD_LENGTH][FIELD_LENGTH]; + Node* graph[FIELD_LENGTH][FIELD_LENGTH]; - std::priority_queue open_list; - std::list closed_list; + std::priority_queue open_list; + std::list closed_list; - std::list plan; + std::list plan; - Node end_node; - Node start_node; - - ascend_msgs::PathPlannerPlan current_plan_; + Node* end_node; + Node* start_node; public: - PathPlanner(float32 current_x, float32 current_y, float32 target_x, float32 target_y); + PathPlanner(float current_x, float current_y, float target_x, float target_y); void initializeGraph(); - float32 calculateHeuristic(float32 x, float32 y); - float32 calculateG(float32 x, float32 y); + float calculateHeuristic(float x, float y); + float calculateG(float x, float y); + void printGraph(); }; #endif // PATH_PLANNER_HPP diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 73df9ab..0520dc6 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -6,25 +6,36 @@ bool operator< (const Node &lhs, const Node &rhs){ } -PathPlanner::PathPlanner(float32 current_x, float32 current_y, float32 target_x, float32 target_y){ - start_node = Node(current_x, current_y, 0, 0); - end_node = Node(target_x, target_y, 0, 0); - +PathPlanner::PathPlanner(float current_x, float current_y, float target_x, float target_y){ + start_node = new Node(current_x, current_y, 0, 0); + end_node = new Node(target_x, target_y, 0, 0); + initializeGraph(); } void PathPlanner::initializeGraph(){ - for (int x = 0; x < FIELD_LENGTH; x++){ - for (int y = 0; y < FIELD_LENGTH; y++){ - graph[x][y] = Node(x, y, computeHeuristic(x,y), calculateG(x,y)); - open_list.push(graph[x][y]); - } - } + for (int x = 0; x < FIELD_LENGTH; x++){ + for (int y = 0; y < FIELD_LENGTH; y++){ + graph[x][y] = new Node(x, y, calculateHeuristic(x,y), calculateG(x,y)); + open_list.push(*(graph[x][y])); + } + } +} + +float PathPlanner::calculateHeuristic(float x, float y){ + return (sqrt((x - end_node->getX())*(x - end_node->getX()) + (y - end_node->getY())*(y - end_node->getY()))); } -float32 PathPlanner::calculateHeuristic(float32 x, float32 y){ - return (sqrt((x - end_node.getX())*(x - end_node.getX()) + (y - end_node.getY())*(y - end_node.getY()))); +float PathPlanner::calculateG(float x, float y){ + return (sqrt((start_node->getX() - x)*(start_node->getX() - x) + (start_node->getY() - y)*(start_node->getY() - y))); } -float32 PathPlanner::calculateG(float32 x, float32 y){ - return(sqrt((start_node.getX() - x)*(start_node.getX() - x) + (start_node.getY() - y)*(start_node.getY() - y))); +void PathPlanner::printGraph(){ + for(int x = 0; x < FIELD_LENGTH; x++){ + for(int y = 0; y < FIELD_LENGTH; y++){ + std::cout << std::fixed; + std::cout << std::setprecision(2); + std::cout << graph[x][y]->getF() << " "; + } + std::cout << std::endl; + } } \ No newline at end of file From 79833dbaabb3f046021f510641681c6f082c8639 Mon Sep 17 00:00:00 2001 From: atussa Date: Tue, 7 Nov 2017 11:00:39 +0100 Subject: [PATCH 007/200] Change to std::array. --- include/control/planner/path_planner.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index b2030b9..de13f92 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #define FIELD_LENGTH 20 @@ -39,7 +40,7 @@ class Node{ class PathPlanner{ private: - Node* graph[FIELD_LENGTH][FIELD_LENGTH]; + std::array, FIELD_LENGTH> graph; std::priority_queue open_list; std::list closed_list; From 88e162592a755d206af6e75edfa7c2d3504bce3b Mon Sep 17 00:00:00 2001 From: atussa Date: Tue, 7 Nov 2017 11:14:22 +0100 Subject: [PATCH 008/200] Change from pointer to std::unique_ptr --- include/control/planner/path_planner.hpp | 7 ++++--- src/nodes/path_planner.cpp | 6 +++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index de13f92..d6dac5f 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #define FIELD_LENGTH 20 @@ -40,15 +41,15 @@ class Node{ class PathPlanner{ private: - std::array, FIELD_LENGTH> graph; + std::array, FIELD_LENGTH>, FIELD_LENGTH> graph; std::priority_queue open_list; std::list closed_list; std::list plan; - Node* end_node; - Node* start_node; + std::unique_ptr end_node; + std::unique_ptr start_node; public: PathPlanner(float current_x, float current_y, float target_x, float target_y); diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 0520dc6..94445c3 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -7,15 +7,15 @@ bool operator< (const Node &lhs, const Node &rhs){ PathPlanner::PathPlanner(float current_x, float current_y, float target_x, float target_y){ - start_node = new Node(current_x, current_y, 0, 0); - end_node = new Node(target_x, target_y, 0, 0); + start_node = std::unique_ptr(new Node (current_x, current_y, 0, 0)); + end_node = std::unique_ptr(new Node (target_x, target_y, 0, 0)); initializeGraph(); } void PathPlanner::initializeGraph(){ for (int x = 0; x < FIELD_LENGTH; x++){ for (int y = 0; y < FIELD_LENGTH; y++){ - graph[x][y] = new Node(x, y, calculateHeuristic(x,y), calculateG(x,y)); + graph[x][y] = std::unique_ptr (new Node(x, y, calculateHeuristic(x,y), calculateG(x,y))); open_list.push(*(graph[x][y])); } } From 6d95807beef62d69fbf41ec2de8d43ddd620aef9 Mon Sep 17 00:00:00 2001 From: atussa Date: Fri, 10 Nov 2017 20:15:36 +0100 Subject: [PATCH 009/200] Alot of changes at once.. Create super simple working A* --- include/control/planner/path_planner.hpp | 47 +++++++-- src/nodes/path_planner.cpp | 129 +++++++++++++++++++++-- 2 files changed, 158 insertions(+), 18 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index d6dac5f..3e725e2 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -11,6 +11,8 @@ #include #include #include +#include +#include #define FIELD_LENGTH 20 @@ -21,18 +23,31 @@ class Node{ float x; float y; - // heuristic, use Euclidian distance - float h; float g; + // heuristic, use diagonal distance + float h; // Sum of g and h float f; + + float parent_x = -1; + float parent_y = -1; public: - Node(); + Node(){} Node(float x, float y, float g, float h):x(x), y(y), g(g), h(h){f = g+h;} float getX() const {return x;} float getY() const {return y;} float getF() const {return f;} + float getG() const {return g;} + float getParentX() const {return parent_x;} + float getParentY() const {return parent_y;} + + void setG(float g) {this->g = g;} + void setParentX(float parent_x){this->parent_x = parent_x;} + void setParentY(float parent_y){this->parent_y = parent_y;} + void updateF(float new_g); + + bool isValid(); // Implemented only for the closed list priority queue friend bool operator< (const Node &lhs, const Node &rhs); @@ -41,22 +56,36 @@ class Node{ class PathPlanner{ private: - std::array, FIELD_LENGTH>, FIELD_LENGTH> graph; + std::array, FIELD_LENGTH> graph; std::priority_queue open_list; - std::list closed_list; + std::array, FIELD_LENGTH> closed_list; std::list plan; - std::unique_ptr end_node; - std::unique_ptr start_node; + Node end_node; + Node start_node; + + bool destination_reached = false; public: PathPlanner(float current_x, float current_y, float target_x, float target_y); + void initializeGraph(); - float calculateHeuristic(float x, float y); - float calculateG(float x, float y); + void initializeClosedList(); + + float calculateEuclidianHeuristic(float x, float y); + float calculateDiagonalHeuristic(float x, float y); + void printGraph(); + void relaxGraph(); + + void handleSuccessor(float x, float y, float parent_x, float parent_y, + float distance_to_parent); + void handleAllSuccessors(float x, float y); + bool isDestination(float x, float y); + + void makePlan(); }; #endif // PATH_PLANNER_HPP diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 94445c3..a3bd1e6 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -5,28 +5,61 @@ bool operator< (const Node &lhs, const Node &rhs){ return lhs.f > rhs.f; } +bool Node::isValid(){ + return (x>0 && x0 && y(new Node (current_x, current_y, 0, 0)); - end_node = std::unique_ptr(new Node (target_x, target_y, 0, 0)); + start_node = Node(current_x, current_y, 0, 0); + start_node.setParentX(0); + start_node.setParentY(0); + end_node = Node(target_x,target_y, std::numeric_limits::infinity(), 0); initializeGraph(); + initializeClosedList(); } void PathPlanner::initializeGraph(){ for (int x = 0; x < FIELD_LENGTH; x++){ for (int y = 0; y < FIELD_LENGTH; y++){ - graph[x][y] = std::unique_ptr (new Node(x, y, calculateHeuristic(x,y), calculateG(x,y))); - open_list.push(*(graph[x][y])); + if(x == start_node.getX() && y == start_node.getY()){ + graph[x][y] = start_node; + open_list.push(graph[x][y]); + } + /*else if(x == end_node.getX() && y == end_node.getY()){ + graph[x][y] = end_node; + }*/ + else { + graph[x][y] = Node(x,y, std::numeric_limits::infinity(), calculateDiagonalHeuristic(x,y)); + } } } } -float PathPlanner::calculateHeuristic(float x, float y){ - return (sqrt((x - end_node->getX())*(x - end_node->getX()) + (y - end_node->getY())*(y - end_node->getY()))); +void PathPlanner::initializeClosedList(){ + for (int x = 0; x < FIELD_LENGTH; x++) { + for (int y = 0; y < FIELD_LENGTH; y++){ + closed_list[x][y] = false; + } + } +} + +float PathPlanner::calculateEuclidianHeuristic(float x, float y){ + return static_cast(sqrt((x - end_node.getX()) * (x - end_node.getX()) + + (y - end_node.getY()) * (y - end_node.getY()))); } -float PathPlanner::calculateG(float x, float y){ - return (sqrt((start_node->getX() - x)*(start_node->getX() - x) + (start_node->getY() - y)*(start_node->getY() - y))); +float PathPlanner::calculateDiagonalHeuristic(float x, float y) { + return (std::max(abs(x-end_node.getX()), abs(y-end_node.getY()))); } void PathPlanner::printGraph(){ @@ -34,8 +67,86 @@ void PathPlanner::printGraph(){ for(int y = 0; y < FIELD_LENGTH; y++){ std::cout << std::fixed; std::cout << std::setprecision(2); - std::cout << graph[x][y]->getF() << " "; + std::cout << graph[x][y].getF() << " "; } std::cout << std::endl; } +} + +void PathPlanner::relaxGraph(){ + Node current_node; + while(!open_list.empty()){ + current_node = open_list.top(); + open_list.pop(); + float x = current_node.getX(); + float y = current_node.getY(); + closed_list[x][y] = true; + handleAllSuccessors(x,y); + if(destination_reached) {return;} + } +} + +void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent_y, float distance_to_parent) { + if(x>= 0 && x < FIELD_LENGTH && y >= 0 && y < FIELD_LENGTH){ + if(isDestination(x,y)){ + graph[x][y].setParentX(parent_x); + graph[x][y].setParentY(parent_y); + destination_reached = true; + std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; + } + else if(closed_list[x][y] == false){ + float new_g = graph[parent_x][parent_y].getG() + distance_to_parent; + if(graph[x][y].getG() > new_g) { + std::cout << "New node: " << x << ", " << y << " Parent: " + << parent_x << ", " << parent_y << std::endl; + graph[x][y].updateF(new_g); + graph[x][y].setParentX(parent_x); + graph[x][y].setParentY(parent_y); + open_list.push(graph[x][y]); + } + } + } +} + +void PathPlanner::handleAllSuccessors(float x, float y) { + // LOVER Å LØSE DETTE PÅ EN BEDRE MÅTE!!!!!!!!!!!!!!!!!!!! + handleSuccessor(x+1,y, x, y, 1); + if(destination_reached){return;} + handleSuccessor(x+1, y+1, x, y, sqrt(2)); + if(destination_reached){return;} + handleSuccessor(x+1, y-1, x, y, sqrt(2)); + if(destination_reached){return;} + handleSuccessor(x, y+1, x, y, 1); + if(destination_reached){return;} + handleSuccessor(x, y-1, x, y, 1); + if(destination_reached){return;} + handleSuccessor(x-1, y, x, y, 1); + if(destination_reached){return;} + handleSuccessor(x-1, y+1, x, y, sqrt(2)); + if(destination_reached){return;} + handleSuccessor(x-1, y-1, x, y, sqrt(2)); +} + +bool PathPlanner::isDestination(float x, float y) { + return ((x == end_node.getX()) && (y == end_node.getY())); +} + +void PathPlanner::makePlan() { + relaxGraph(); + + float x = end_node.getX(); + float y = end_node.getY(); + + while(!(x == start_node.getX() && y == start_node.getY())){ + plan.push_front(graph[x][y]); + float x_temp = graph[x][y].getParentX(); + float y_temp = graph[x][y].getParentY(); + x = x_temp; + y = y_temp; + } + plan.push_front(graph[x][y]); + + for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ + std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; + } } \ No newline at end of file From 3fccb8bd4f2dce325814cbca8c5be9f37fc2c114 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 16 Nov 2017 09:32:01 +0100 Subject: [PATCH 010/200] Make separate file for Node class --- include/control/planner/node.hpp | 42 ++++++++++++++++++++++++ include/control/planner/path_planner.hpp | 40 +--------------------- src/nodes/node.cpp | 17 ++++++++++ src/nodes/path_planner.cpp | 18 ---------- 4 files changed, 60 insertions(+), 57 deletions(-) create mode 100644 include/control/planner/node.hpp create mode 100644 src/nodes/node.cpp diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp new file mode 100644 index 0000000..9f75b5a --- /dev/null +++ b/include/control/planner/node.hpp @@ -0,0 +1,42 @@ +#ifndef NODE_HPP +#define NODE_HPP + +#define FIELD_LENGTH 20 + + +class Node{ +private: + float x; + float y; + + float g; + // heuristic, use diagonal distance + float h; + // Sum of g and h + float f; + + float parent_x = -1; + float parent_y = -1; +public: + Node(){} + Node(float x, float y, float g, float h):x(x), y(y), g(g), h(h){f = g+h;} + + float getX() const {return x;} + float getY() const {return y;} + float getF() const {return f;} + float getG() const {return g;} + float getParentX() const {return parent_x;} + float getParentY() const {return parent_y;} + + void setG(float g) {this->g = g;} + void setParentX(float parent_x){this->parent_x = parent_x;} + void setParentY(float parent_y){this->parent_y = parent_y;} + void updateF(float new_g); + + bool isValid(); + + // Implemented only for the closed list priority queue + friend bool operator< (const Node &lhs, const Node &rhs); +}; + +#endif // NODE_HPP \ No newline at end of file diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 3e725e2..b78c256 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -3,6 +3,7 @@ #include #include +#include "control/planner/node.hpp" #include "control/tools/control_pose.hpp" #include #include @@ -15,45 +16,6 @@ #include -#define FIELD_LENGTH 20 - - -class Node{ -private: - float x; - float y; - - float g; - // heuristic, use diagonal distance - float h; - // Sum of g and h - float f; - - float parent_x = -1; - float parent_y = -1; -public: - Node(){} - Node(float x, float y, float g, float h):x(x), y(y), g(g), h(h){f = g+h;} - - float getX() const {return x;} - float getY() const {return y;} - float getF() const {return f;} - float getG() const {return g;} - float getParentX() const {return parent_x;} - float getParentY() const {return parent_y;} - - void setG(float g) {this->g = g;} - void setParentX(float parent_x){this->parent_x = parent_x;} - void setParentY(float parent_y){this->parent_y = parent_y;} - void updateF(float new_g); - - bool isValid(); - - // Implemented only for the closed list priority queue - friend bool operator< (const Node &lhs, const Node &rhs); -}; - - class PathPlanner{ private: std::array, FIELD_LENGTH> graph; diff --git a/src/nodes/node.cpp b/src/nodes/node.cpp new file mode 100644 index 0000000..917a685 --- /dev/null +++ b/src/nodes/node.cpp @@ -0,0 +1,17 @@ +#include "control/planner/node.hpp" + + +bool operator< (const Node &lhs, const Node &rhs){ + return lhs.f > rhs.f; +} + +bool Node::isValid(){ + return (x>0 && x0 && y rhs.f; -} - -bool Node::isValid(){ - return (x>0 && x0 && y Date: Fri, 17 Nov 2017 12:54:34 +0100 Subject: [PATCH 011/200] Change graph to have higher resolution. Add functionality for adding obstacles to the graph. --- include/control/planner/node.hpp | 5 +- include/control/planner/path_planner.hpp | 28 +++- src/nodes/node.cpp | 10 +- src/nodes/path_planner.cpp | 159 +++++++++++++++-------- 4 files changed, 136 insertions(+), 66 deletions(-) diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp index 9f75b5a..cb6e17a 100644 --- a/include/control/planner/node.hpp +++ b/include/control/planner/node.hpp @@ -3,14 +3,13 @@ #define FIELD_LENGTH 20 - class Node{ private: float x; float y; - float g; // heuristic, use diagonal distance + float g; float h; // Sum of g and h float f; @@ -33,8 +32,6 @@ class Node{ void setParentY(float parent_y){this->parent_y = parent_y;} void updateF(float new_g); - bool isValid(); - // Implemented only for the closed list priority queue friend bool operator< (const Node &lhs, const Node &rhs); }; diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index b78c256..722d55b 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -15,13 +15,15 @@ #include #include +#define GRAPH_SIZE 200 // number of nodes in one direction +#define OBSTACLE_RADIUS 0.8 // meters class PathPlanner{ private: - std::array, FIELD_LENGTH> graph; + std::array, GRAPH_SIZE> graph; std::priority_queue open_list; - std::array, FIELD_LENGTH> closed_list; + std::array, GRAPH_SIZE> closed_list; std::list plan; @@ -36,18 +38,38 @@ class PathPlanner{ void initializeGraph(); void initializeClosedList(); - float calculateEuclidianHeuristic(float x, float y); + // Add a circular obstacle + void addObstacle(float center_x, float center_y); + + // Diagonal heuristic - can move in 8 directions from current point float calculateDiagonalHeuristic(float x, float y); + // Print the f-value of each node void printGraph(); void relaxGraph(); + // void handleSuccessor(float x, float y, float parent_x, float parent_y, float distance_to_parent); + // Every point has 8 succsessors void handleAllSuccessors(float x, float y); + bool isDestination(float x, float y); + bool isStart(float x, float y); + bool isValidCoordinate(float x, float y); + // Use A* to calculate the path void makePlan(); + + // These functions are mainly for the visualization + std::array, FIELD_LENGTH*10> getGraph(){return graph;} + std::list getPlan(){return plan;} + // For the colors in the visualization + float max_f = 0; }; +// Convert between coordinate x or y in metres and index in graph +int coordToIndex(float k); + + #endif // PATH_PLANNER_HPP diff --git a/src/nodes/node.cpp b/src/nodes/node.cpp index 917a685..cc4a690 100644 --- a/src/nodes/node.cpp +++ b/src/nodes/node.cpp @@ -2,16 +2,10 @@ bool operator< (const Node &lhs, const Node &rhs){ - return lhs.f > rhs.f; -} - -bool Node::isValid(){ - return (x>0 && x0 && y rhs.f; } void Node::updateF(float new_g){ - if(new_g < g){ - g = new_g; - } + g = new_g; f = g+h; } \ No newline at end of file diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 0f29da4..8405f07 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -1,6 +1,13 @@ #include "control/planner/path_planner.hpp" +int coordToIndex(float k) { + int index = static_cast(k*10.0+0.5); + return (index); +} + + + PathPlanner::PathPlanner(float current_x, float current_y, float target_x, float target_y){ start_node = Node(current_x, current_y, 0, 0); start_node.setParentX(0); @@ -11,45 +18,63 @@ PathPlanner::PathPlanner(float current_x, float current_y, float target_x, float } void PathPlanner::initializeGraph(){ - for (int x = 0; x < FIELD_LENGTH; x++){ - for (int y = 0; y < FIELD_LENGTH; y++){ - if(x == start_node.getX() && y == start_node.getY()){ - graph[x][y] = start_node; - open_list.push(graph[x][y]); + for (float x = 0; x < FIELD_LENGTH; x += 0.1){ + for (float y = 0; y < FIELD_LENGTH; y += 0.1){ + if(isStart(x,y)){ + graph[coordToIndex(x)][coordToIndex(y)] = start_node; + // Push the start node to the open list so that the search starts from the start + open_list.push(graph[coordToIndex(x)][coordToIndex(y)]); } - /*else if(x == end_node.getX() && y == end_node.getY()){ - graph[x][y] = end_node; - }*/ else { - graph[x][y] = Node(x,y, std::numeric_limits::infinity(), calculateDiagonalHeuristic(x,y)); + graph[coordToIndex(x)][coordToIndex(y)] = Node(x,y, std::numeric_limits::infinity() + , calculateDiagonalHeuristic(x,y)); } } } } void PathPlanner::initializeClosedList(){ - for (int x = 0; x < FIELD_LENGTH; x++) { - for (int y = 0; y < FIELD_LENGTH; y++){ - closed_list[x][y] = false; + for (int i = 0; i < GRAPH_SIZE; i++) { + for (int j = 0; j < GRAPH_SIZE; j++){ + closed_list[i][j] = false; } } } -float PathPlanner::calculateEuclidianHeuristic(float x, float y){ - return static_cast(sqrt((x - end_node.getX()) * (x - end_node.getX()) + - (y - end_node.getY()) * (y - end_node.getY()))); +void PathPlanner::addObstacle(float center_x, float center_y) { + // Find the y value for each x in the circle + for (float x = center_x-OBSTACLE_RADIUS; x < center_x+OBSTACLE_RADIUS; x+=0.1) { + float y = sqrt(OBSTACLE_RADIUS*OBSTACLE_RADIUS-(x-center_x)*(x-center_x)); + // Do this to fill the circle + for(float i = center_y-y; i<=center_y+y; i+= 0.1) { + if (isValidCoordinate(x, i)) { + closed_list[coordToIndex(x)][coordToIndex(i)] = true; + } + } + + } + // Do the same as over, just switch x and y + for (float y = center_y-OBSTACLE_RADIUS; y < center_y+OBSTACLE_RADIUS; y+=0.1) { + float x = sqrt(OBSTACLE_RADIUS*OBSTACLE_RADIUS-(y-center_y)*(y-center_y)); + for(float i = center_x-x; i<=center_x+x; i+= 0.1) { + if (isValidCoordinate(i, y)) { + closed_list[coordToIndex(i)][coordToIndex(y)] = true; + } + } + } } + float PathPlanner::calculateDiagonalHeuristic(float x, float y) { return (std::max(abs(x-end_node.getX()), abs(y-end_node.getY()))); } void PathPlanner::printGraph(){ - for(int x = 0; x < FIELD_LENGTH; x++){ - for(int y = 0; y < FIELD_LENGTH; y++){ + for(int i = 0; i < GRAPH_SIZE; i++){ + for(int j = 0; j < GRAPH_SIZE; j++){ std::cout << std::fixed; std::cout << std::setprecision(2); - std::cout << graph[x][y].getF() << " "; + std::cout << graph[i][j].getF() << " "; } std::cout << std::endl; } @@ -58,77 +83,109 @@ void PathPlanner::printGraph(){ void PathPlanner::relaxGraph(){ Node current_node; while(!open_list.empty()){ + // Always search from the node with lowest f value current_node = open_list.top(); open_list.pop(); float x = current_node.getX(); float y = current_node.getY(); - closed_list[x][y] = true; + // Mark node as searched + closed_list[coordToIndex(x)][coordToIndex(y)] = true; + // Search all eight points around current point handleAllSuccessors(x,y); - if(destination_reached) {return;} + // Stop search if destination is reached + if(destination_reached){return;} } } void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent_y, float distance_to_parent) { - if(x>= 0 && x < FIELD_LENGTH && y >= 0 && y < FIELD_LENGTH){ + if(isValidCoordinate(x,y)){ + // Making sure the zero is actually zero and not something like 1.4895e-9 + if(x > 0.0 && x < 0.01){x = 0.0;} + if(y > 0.0 && y < 0.01){y = 0.0;} if(isDestination(x,y)){ - graph[x][y].setParentX(parent_x); - graph[x][y].setParentY(parent_y); + graph[coordToIndex(x)][coordToIndex(y)].setParentX(parent_x); + graph[coordToIndex(x)][coordToIndex(y)].setParentY(parent_y); destination_reached = true; std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; + std::cout << "Destination parent: x=" << graph[coordToIndex(x)][coordToIndex(y)].getParentX() + << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; } - else if(closed_list[x][y] == false){ - float new_g = graph[parent_x][parent_y].getG() + distance_to_parent; - if(graph[x][y].getG() > new_g) { - std::cout << "New node: " << x << ", " << y << " Parent: " - << parent_x << ", " << parent_y << std::endl; - graph[x][y].updateF(new_g); - graph[x][y].setParentX(parent_x); - graph[x][y].setParentY(parent_y); - open_list.push(graph[x][y]); + // If node is not destination and hasn't been searched yet + else if(closed_list[coordToIndex(x)][coordToIndex(y)] == false){ + // Calculate distance from start through the parent + float new_g = graph[coordToIndex(parent_x)][coordToIndex(parent_y)].getG() + distance_to_parent; + // Update graph and open list if distance is less than before through the parent + if(graph[coordToIndex(x)][coordToIndex(y)].getG() > new_g) { + graph[coordToIndex(x)][coordToIndex(y)].updateF(new_g); + // For visualization + if(max_f < graph[coordToIndex(x)][coordToIndex(y)].getF()){ + max_f = graph[coordToIndex(x)][coordToIndex(y)].getF(); + } + graph[coordToIndex(x)][coordToIndex(y)].setParentX(parent_x); + graph[coordToIndex(x)][coordToIndex(y)].setParentY(parent_y); + open_list.push(graph[coordToIndex(x)][coordToIndex(y)]); } } } } void PathPlanner::handleAllSuccessors(float x, float y) { - // LOVER Å LØSE DETTE PÅ EN BEDRE MÅTE!!!!!!!!!!!!!!!!!!!! - handleSuccessor(x+1,y, x, y, 1); + // LOVER Å LØSE DETTE PÅ EN BEDRE MÅTE + handleSuccessor(x+0.1,y, x, y, 0.1); if(destination_reached){return;} - handleSuccessor(x+1, y+1, x, y, sqrt(2)); + handleSuccessor(x+0.1, y+0.1, x, y, sqrt(0.02)); if(destination_reached){return;} - handleSuccessor(x+1, y-1, x, y, sqrt(2)); + handleSuccessor(x+0.1, y-0.1, x, y, sqrt(0.02)); if(destination_reached){return;} - handleSuccessor(x, y+1, x, y, 1); + handleSuccessor(x, y+0.1, x, y, 0.1); if(destination_reached){return;} - handleSuccessor(x, y-1, x, y, 1); + handleSuccessor(x, y-0.1, x, y, 0.1); if(destination_reached){return;} - handleSuccessor(x-1, y, x, y, 1); + handleSuccessor(x-0.1, y, x, y, 0.1); if(destination_reached){return;} - handleSuccessor(x-1, y+1, x, y, sqrt(2)); + handleSuccessor(x-0.1, y+0.1, x, y, sqrt(0.02)); if(destination_reached){return;} - handleSuccessor(x-1, y-1, x, y, sqrt(2)); + handleSuccessor(x-0.1, y-0.1, x, y, sqrt(0.02)); } bool PathPlanner::isDestination(float x, float y) { - return ((x == end_node.getX()) && (y == end_node.getY())); + return ((x < end_node.getX()+0.05 && x>end_node.getX()-0.05) + && (y < end_node.getY()+0.05 && y > end_node.getY()-0.05)); +} + +bool PathPlanner::isStart(float x, float y) { + return ((x < start_node.getX()+0.05 && x>start_node.getX()-0.05) + && (y < start_node.getY()+0.05 && y > start_node.getY()-0.05)); +} + +bool PathPlanner::isValidCoordinate(float x, float y) { + return(x>=0 && x=0 && y::iterator it = plan.begin(); it!= plan.end(); ++it){ - std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; + counter ++; + if(counter >250){break;} } + // Add start node to the plan (might not be necessary) + plan.push_front(graph[coordToIndex(x)][coordToIndex(y)]); + // Print the plan - can be removed + /*for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ + std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; + }*/ } \ No newline at end of file From 1f165baa6908082098cd5f323898e5b14d60a9d4 Mon Sep 17 00:00:00 2001 From: atussa Date: Tue, 9 Jan 2018 09:51:54 +0100 Subject: [PATCH 012/200] Add function to make plan with fewer points. --- include/control/planner/path_planner.hpp | 2 ++ src/nodes/path_planner.cpp | 36 ++++++++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 722d55b..e913ca9 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -60,6 +60,8 @@ class PathPlanner{ // Use A* to calculate the path void makePlan(); + // Same plan but with fewer points + void simplifyPlan(); // These functions are mainly for the visualization std::array, FIELD_LENGTH*10> getGraph(){return graph;} diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 8405f07..7a451de 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -188,4 +188,40 @@ void PathPlanner::makePlan() { /*for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; }*/ +} + + +void PathPlanner::simplifyPlan() { + std::list::iterator current = plan.begin(); + std::list::iterator next = plan.begin(); + next++; + simple_plan.push_back(*current); + while(next != plan.end()){ + if(coordToIndex(current->getX()) == coordToIndex(next->getX())){ + std::cout << "VERTICAL" << std::endl; + while(coordToIndex(current->getX()) == coordToIndex(next->getX()) && next != plan.end()){ + current++; + next++; + } + } + else if(coordToIndex(current->getY()) == coordToIndex(next->getY())){ + std::cout << "HORISONTAL" << std::endl; + while(coordToIndex(current->getY()) == coordToIndex(next->getY()) && next != plan.end()){ + current++; + next++; + } + } + else{ + std::cout << "DIAGONAL" << std::endl; + while(coordToIndex(current->getX()) != coordToIndex(next->getX()) && coordToIndex(current->getY()) != coordToIndex(next->getY()) && next != plan.end()){ + current++; + next++; + } + } + simple_plan.push_back(*current); + } + + for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ + std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; + } } \ No newline at end of file From 5897557699aaa3d47c28c33410b90a1008cd986e Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Tue, 9 Jan 2018 12:45:37 +0100 Subject: [PATCH 013/200] Implement first try at obstacle avoidance --- src/tools/obstacle_avoidance.cpp | 75 +++++++++++++++++++++++++++++++- 1 file changed, 73 insertions(+), 2 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 63381a0..74fda8d 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -1,14 +1,84 @@ #include "control/tools/obstacle_avoidance.hpp" +#include "control/tools/obstacle_state_handler.hpp" +#include "control/tools/drone_handler.hpp" + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include //Static instance std::shared_ptr control::ObstacleAvoidance::instance_p_; +constexpr float PI{3.14159265359f}; bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { - //TODO Implement obstacle avoidance + //TODO move to class + constexpr float clearence_side{0.7f}; + constexpr float clearence_back{0.5f}; + constexpr float clearence_front{1.6f}; + const float clearence_max = std::max({clearence_side, clearence_back, clearence_front}); + + bool setpoint_modified = false; + + const std::vector obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + const geometry_msgs::PoseStamped drone_pose = control::DroneHandler::getCurrentPose(); + + for (const auto& obstacle : obstacles){ + + geometry_msgs::Vector3 delta_drone_obstacle; + delta_drone_obstacle.x = obstacle.x - drone_pose.pose.position.x; + delta_drone_obstacle.y = obstacle.y - drone_pose.pose.position.y; + delta_drone_obstacle.z = drone_pose.pose.position.z; + const float distance_to_obstacle = std::sqrt(std::pow(delta_drone_obstacle.x, 2) + std::pow(delta_drone_obstacle.y, 2)); + if (distance_to_obstacle < clearence_max){ + // perform obstacle avoidance + const float angle = std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta; + ROS_DEBUG("[control]: angle to obstacle: %.3f", angle); + + // obstacle.theta + pi/2 is straight ahead for obstacle. + + const bool drone_in_front_of_obstacle = angle >= 0; + + geometry_msgs::Vector3 minimum_delta; + minimum_delta.z = 0.0; // not used + if (drone_in_front_of_obstacle){ + ROS_DEBUG_THROTTLE(1, "[control]: drone in front of obstacle"); + + minimum_delta.x = clearence_side * std::cos(angle); + minimum_delta.y = clearence_front/clearence_side * (clearence_side - std::abs(minimum_delta.x)); + } + else { + ROS_DEBUG_THROTTLE(1,"[control]: drone behind obstacle"); + + minimum_delta.x = clearence_side * std::cos(angle); + minimum_delta.y = clearence_back * std::sin(angle); + } + + const float minimum_distance = std::sqrt(std::pow(minimum_delta.x, 2) + std::pow(minimum_delta.y, 2)); + if (distance_to_obstacle < minimum_distance) { + // need to avoid obstacle + setpoint_modified = true; + setpoint->position.x = obstacle.x + minimum_delta.x; + setpoint->position.y = obstacle.y + minimum_delta.y; + + ROS_DEBUG_THROTTLE(1, "[control]: obstacle avoidance setpoint: %.2f, %.2f, %.2f", + setpoint->position.x, setpoint->position.y, setpoint->position.z); + } + } + } //Return true if setpoint has been modified. - return false; + return setpoint_modified; } void control::ObstacleAvoidance::onModified() { @@ -44,3 +114,4 @@ void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr Date: Tue, 9 Jan 2018 16:52:32 +0100 Subject: [PATCH 014/200] Improve function to have even fewer points in plan --- src/nodes/path_planner.cpp | 60 ++++++++++++++++++++++++++++++++++---- 1 file changed, 55 insertions(+), 5 deletions(-) diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 7a451de..f3079fd 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -188,39 +188,89 @@ void PathPlanner::makePlan() { /*for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; }*/ -} + // Delete unnecessary points + simplifyPlan(); +} void PathPlanner::simplifyPlan() { - std::list::iterator current = plan.begin(); + /*std::list::iterator current = plan.begin(); std::list::iterator next = plan.begin(); next++; simple_plan.push_back(*current); while(next != plan.end()){ if(coordToIndex(current->getX()) == coordToIndex(next->getX())){ - std::cout << "VERTICAL" << std::endl; + //std::cout << "VERTICAL" << std::endl; while(coordToIndex(current->getX()) == coordToIndex(next->getX()) && next != plan.end()){ current++; next++; } } else if(coordToIndex(current->getY()) == coordToIndex(next->getY())){ - std::cout << "HORISONTAL" << std::endl; + //std::cout << "HORISONTAL" << std::endl; while(coordToIndex(current->getY()) == coordToIndex(next->getY()) && next != plan.end()){ current++; next++; } } else{ - std::cout << "DIAGONAL" << std::endl; + //std::cout << "DIAGONAL" << std::endl; while(coordToIndex(current->getX()) != coordToIndex(next->getX()) && coordToIndex(current->getY()) != coordToIndex(next->getY()) && next != plan.end()){ current++; next++; } } simple_plan.push_back(*current); + }*/ + simple_plan = plan; + // Pointing at the three first elements + std::list::iterator first = simple_plan.begin(); + std::list::iterator second = simple_plan.begin(); + std::list::iterator third = simple_plan.begin(); + second++; + third++; + third++; + + float x1, x2, y1, y2, delta_x, delta_y, num_points; + bool hit_obstacle = false; + + while(third != simple_plan.end()){ + // Parameters for finding the straight line between the first and third point + x1 = first->getX(); + x2 = third->getX(); + y1 = first->getY(); + y2 = third->getY(); + delta_x = x2-x1; + delta_y = y2-y1; + num_points = 20*std::max(abs(delta_x),abs(delta_y)); + for(int i = 0; i < num_points; i++){ + x1 += delta_x/num_points; + y1 += delta_y/num_points; + // checking if the line goes through an obstacle at this point + if(graph[coordToIndex(x1)][coordToIndex(y1)].obstacle){ + hit_obstacle = true; + } + } + // If an obstacle is hit, the points are necessary and the search will start + // at the end of the current search. + if(hit_obstacle){ + second = first; + second++; + third = second; + third++; + } + // If the straight line does not go through an obstacle, delete the second point + // (no use in going through it when we can go in a straight line) + // The search continues with same starting point, but the second and third point is changed + else{ + simple_plan.erase(second); + second = third; + third++; + } + hit_obstacle = false; } + // Print the remaining points for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; } From b03b069059205138e307d869382bf835a042d5b8 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Wed, 10 Jan 2018 10:56:23 +0100 Subject: [PATCH 015/200] Fix spelling lol --- src/tools/obstacle_avoidance.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 74fda8d..c7e4634 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -22,11 +22,15 @@ std::shared_ptr control::ObstacleAvoidance::instance constexpr float PI{3.14159265359f}; bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { - //TODO move to class - constexpr float clearence_side{0.7f}; - constexpr float clearence_back{0.5f}; - constexpr float clearence_front{1.6f}; - const float clearence_max = std::max({clearence_side, clearence_back, clearence_front}); + //TODO move to class and also rosparams? + constexpr float clearance_side{0.7f}; + constexpr float clearance_back{0.5f}; + constexpr float clearance_front{1.6f}; +#if (__cplusplus >= 201402L) + constexpr float clearance_max = std::max({clearance_side, clearance_back, clearance_front}); +#else + const float clearance_max = std::max({clearance_side, clearance_back, clearance_front}); +#endif bool setpoint_modified = false; @@ -40,7 +44,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget delta_drone_obstacle.y = obstacle.y - drone_pose.pose.position.y; delta_drone_obstacle.z = drone_pose.pose.position.z; const float distance_to_obstacle = std::sqrt(std::pow(delta_drone_obstacle.x, 2) + std::pow(delta_drone_obstacle.y, 2)); - if (distance_to_obstacle < clearence_max){ + if (distance_to_obstacle < clearance_max){ // perform obstacle avoidance const float angle = std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta; ROS_DEBUG("[control]: angle to obstacle: %.3f", angle); @@ -54,14 +58,14 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget if (drone_in_front_of_obstacle){ ROS_DEBUG_THROTTLE(1, "[control]: drone in front of obstacle"); - minimum_delta.x = clearence_side * std::cos(angle); - minimum_delta.y = clearence_front/clearence_side * (clearence_side - std::abs(minimum_delta.x)); + minimum_delta.x = clearance_side * std::cos(angle); + minimum_delta.y = clearance_front/clearance_side * (clearance_side - std::abs(minimum_delta.x)); } else { ROS_DEBUG_THROTTLE(1,"[control]: drone behind obstacle"); - minimum_delta.x = clearence_side * std::cos(angle); - minimum_delta.y = clearence_back * std::sin(angle); + minimum_delta.x = clearance_side * std::cos(angle); + minimum_delta.y = clearance_back * std::sin(angle); } const float minimum_distance = std::sqrt(std::pow(minimum_delta.x, 2) + std::pow(minimum_delta.y, 2)); From 2f1ab4eb10910a376832573ecc8f2c32fabe69e7 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Wed, 10 Jan 2018 15:54:58 +0100 Subject: [PATCH 016/200] Move to rosparam --- include/control/tools/obstacle_avoidance.hpp | 7 ++++- src/tools/obstacle_avoidance.cpp | 29 +++++++++++++------- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index a66c83d..862dd70 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -12,6 +12,11 @@ class ObstacleAvoidance; class ObstacleAvoidance { private: + float clearance_side; + float clearance_back; + float clearance_front; + float clearance_max; + //Singleton pattern static std::shared_ptr instance_p_; @@ -27,7 +32,7 @@ class ObstacleAvoidance { bool doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint); ///Private constructor - ObstacleAvoidance() = default; + ObstacleAvoidance(); public: ///Deleted copy constructor ObstacleAvoidance(const ObstacleAvoidance&) = delete; diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index c7e4634..87ef90f 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -22,16 +22,6 @@ std::shared_ptr control::ObstacleAvoidance::instance constexpr float PI{3.14159265359f}; bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { - //TODO move to class and also rosparams? - constexpr float clearance_side{0.7f}; - constexpr float clearance_back{0.5f}; - constexpr float clearance_front{1.6f}; -#if (__cplusplus >= 201402L) - constexpr float clearance_max = std::max({clearance_side, clearance_back, clearance_front}); -#else - const float clearance_max = std::max({clearance_side, clearance_back, clearance_front}); -#endif - bool setpoint_modified = false; const std::vector obstacles = control::ObstacleStateHandler::getCurrentObstacles(); @@ -119,3 +109,22 @@ void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr Date: Thu, 11 Jan 2018 13:59:51 +0100 Subject: [PATCH 017/200] Add rosparam support, but it does not work yet --- include/control/tools/config.hpp | 8 ++++ include/control/tools/obstacle_avoidance.hpp | 7 +--- launch/test_control_fsm_sim.launch | 5 +++ src/tools/config.cpp | 18 ++++++++- src/tools/obstacle_avoidance.cpp | 39 ++++++++------------ 5 files changed, 46 insertions(+), 31 deletions(-) diff --git a/include/control/tools/config.hpp b/include/control/tools/config.hpp index 576224b..6924e79 100644 --- a/include/control/tools/config.hpp +++ b/include/control/tools/config.hpp @@ -45,6 +45,14 @@ class Config { static double safe_hover_altitude; ///Drone safezone static double obstacle_too_close_dist; + ///Clearance osbtacle avoidance to right and left of obstacle + static float obstacle_clearance_side; + ///Clearance osbtacle avoidance in front of obstacle + static float obstacle_clearance_front; + ///Clearance osbtacle avoidance behind obstacle + static float obstacle_clearance_back; + ///Radius in which obstacle collisions will be checked for + static float obstacle_clearance_checkradius; ///Topic to listen for info about obstacles static std::string lidar_topic; /**Load paramaters diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index 862dd70..a66c83d 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -12,11 +12,6 @@ class ObstacleAvoidance; class ObstacleAvoidance { private: - float clearance_side; - float clearance_back; - float clearance_front; - float clearance_max; - //Singleton pattern static std::shared_ptr instance_p_; @@ -32,7 +27,7 @@ class ObstacleAvoidance { bool doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint); ///Private constructor - ObstacleAvoidance(); + ObstacleAvoidance() = default; public: ///Deleted copy constructor ObstacleAvoidance(const ObstacleAvoidance&) = delete; diff --git a/launch/test_control_fsm_sim.launch b/launch/test_control_fsm_sim.launch index bb4f1c9..bf0fb1c 100644 --- a/launch/test_control_fsm_sim.launch +++ b/launch/test_control_fsm_sim.launch @@ -7,6 +7,11 @@ + + + + + diff --git a/src/tools/config.cpp b/src/tools/config.cpp index f793b34..64d7826 100644 --- a/src/tools/config.cpp +++ b/src/tools/config.cpp @@ -29,6 +29,10 @@ double Config::obstacle_too_close_dist = 2.0; std::string Config::lidar_topic = "perception/obstacles/lidar"; bool Config::require_all_data_streams = true; bool Config::require_obstacle_detection = true; +float Config::obstacle_clearance_side = 2.0f; +float Config::obstacle_clearance_front = 2.0f; +float Config::obstacle_clearance_back = 2.0f; +float Config::obstacle_clearance_checkradius = 2.0f; void Config::loadParams() { if(!ros::isInitialized()) { @@ -36,6 +40,13 @@ void Config::loadParams() { } ros::NodeHandle n("~"); + + auto getFloatParam = [&](const std::string& name, float& var) { + if(!n.getParam(name, var)) { + ROS_WARN("[Control FSM] Load param failed: %s, using %f", name.c_str(), var); + } + }; + auto getDoubleParam = [&](const std::string& name, double& var) { if(!n.getParam(name, var)) { ROS_WARN("[Control FSM] Load param failed: %s, using %f", name.c_str(), var); @@ -91,5 +102,10 @@ void Config::loadParams() { getStringParam("land_detector_topic", land_detector_topic); //Obstacles getStringParam("obstacle_state_topic", obstacle_state_topic); - + getFloatParam("obstacle_clearance_side", obstacle_clearance_side); + getFloatParam("obstacle_clearance_front", obstacle_clearance_front); + getFloatParam("obstacle_clearance_back", obstacle_clearance_back); + getFloatParam("obstacle_clearance_checkradius", obstacle_clearance_checkradius); + } + diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 87ef90f..e4138ec 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -1,6 +1,7 @@ #include "control/tools/obstacle_avoidance.hpp" #include "control/tools/obstacle_state_handler.hpp" #include "control/tools/drone_handler.hpp" +#include "control/tools/config.hpp" #include #include @@ -22,6 +23,11 @@ std::shared_ptr control::ObstacleAvoidance::instance constexpr float PI{3.14159265359f}; bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { + using control::Config; //::obstacle_clearance_side; +// using control::Config::obstacle_clearance_front; +// using control::Config::obstacle_clearance_back; +// using control::Config::obstacle_clearance_checkradius; + bool setpoint_modified = false; const std::vector obstacles = control::ObstacleStateHandler::getCurrentObstacles(); @@ -34,7 +40,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget delta_drone_obstacle.y = obstacle.y - drone_pose.pose.position.y; delta_drone_obstacle.z = drone_pose.pose.position.z; const float distance_to_obstacle = std::sqrt(std::pow(delta_drone_obstacle.x, 2) + std::pow(delta_drone_obstacle.y, 2)); - if (distance_to_obstacle < clearance_max){ + if (distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance const float angle = std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta; ROS_DEBUG("[control]: angle to obstacle: %.3f", angle); @@ -48,14 +54,14 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget if (drone_in_front_of_obstacle){ ROS_DEBUG_THROTTLE(1, "[control]: drone in front of obstacle"); - minimum_delta.x = clearance_side * std::cos(angle); - minimum_delta.y = clearance_front/clearance_side * (clearance_side - std::abs(minimum_delta.x)); + minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle); + minimum_delta.y = Config::obstacle_clearance_front/Config::obstacle_clearance_side * (Config::obstacle_clearance_side - std::abs(minimum_delta.x)); } else { ROS_DEBUG_THROTTLE(1,"[control]: drone behind obstacle"); - minimum_delta.x = clearance_side * std::cos(angle); - minimum_delta.y = clearance_back * std::sin(angle); + minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle); + minimum_delta.y = Config::obstacle_clearance_back * std::sin(angle); } const float minimum_distance = std::sqrt(std::pow(minimum_delta.x, 2) + std::pow(minimum_delta.y, 2)); @@ -71,6 +77,10 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } } + if (!setpoint_modified){ + ROS_INFO_THROTTLE(2, "Obstacles ignored"); + } + //Return true if setpoint has been modified. return setpoint_modified; } @@ -109,22 +119,3 @@ void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr Date: Thu, 11 Jan 2018 14:02:52 +0100 Subject: [PATCH 018/200] Make outline for ROS action. --- CMakeLists.txt | 5 ++++ src/nodes/path_planner_node.cpp | 43 +++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 src/nodes/path_planner_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e686275..1a32361 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,12 +69,17 @@ target_compile_features(control_fsm_main PRIVATE cxx_range_for) add_executable(land_detector_node src/nodes/land_detector_node) target_compile_features(land_detector_node PRIVATE cxx_range_for) +add_executable(path_planner_node src/nodes/path_planner_node) +target_compile_features(path_planner_node PRIVATE cxx_range_for) + ## Specify libraries to link a library or executable target against target_link_libraries(control_fsm_main control_fsm_lib) target_link_libraries(land_detector_node ${catkin_LIBRARIES}) +target_link_libraries(path_planner_node ${catkin_LIBRARIES}) + ## Add cmake target dependencies of the executable ## same as for the library above add_dependencies(control_fsm_main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp new file mode 100644 index 0000000..a78ab7c --- /dev/null +++ b/src/nodes/path_planner_node.cpp @@ -0,0 +1,43 @@ +#include +#include +#include "control/planner/path_planner.hpp" + +// typedefs + +using ActionServerType = actionlib::SimpleActionServer; + + +//Terminates current goal when requested. +void preemptCB(ActionServerType* server) { + //Abort whatever you are doing first! + ROS_WARN("Preempted!"); +} + +void newPlanCB(ActionServerType* server){ + + + if(server->isPreemptRequested()) { + //Goal is already stopped by client + return; + } +} + + +int main(int argc, char** argv){ + + int make_plan + + ros::init(argc, argv, "path_planner_server"); + ros::NodeHandle n; + + ActionServerType server(n, "plan_path", false); + + server.registerPlanCallback(boost::bind(newPlanCB, &server)); + server.registerPreemptCallback(boost::bind(preemptCB, &server)); + server.start(); + + ROS_INFO("Hello world!"); + + ros::spin(); + +} \ No newline at end of file From c47ab0feb08ed2759f27b7f9f0f448c4af60cba7 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 11 Jan 2018 17:00:44 +0100 Subject: [PATCH 019/200] Implement action for path planner --- src/nodes/path_planner_node.cpp | 63 +++++++++++++++++++++++++++++---- 1 file changed, 57 insertions(+), 6 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index a78ab7c..c269b1f 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -1,43 +1,94 @@ #include #include #include "control/planner/path_planner.hpp" +#include "control/tools/drone_handler.hpp" +#include + // typedefs using ActionServerType = actionlib::SimpleActionServer; + +struct PlannerState{ + float current_x, current_y, goal_x, goal_y; + bool make_plan; +} + + //Terminates current goal when requested. -void preemptCB(ActionServerType* server) { +void preemptCB(ActionServerType* server, PlannerState* planner_state) { + planner_state.make_plan = false; //Abort whatever you are doing first! ROS_WARN("Preempted!"); } -void newPlanCB(ActionServerType* server){ - +void newPlanCB(ActionServerType* server, PlannerState* planner_state){ + planner_state->make_plan = true; + + geometry_msgs::PoseStamped current_pose = DroneHandler::getCurrentPose(); + auto& position = current_pose.pose.position; + planner_state->current_x = position.x; + planner_state->current_y = position.y; + + if(!server->isNewGoalAvailable()){ + return; + } + + //Accept the new goal + auto goal = server->acceptNewGoal(); + planner_state->goal_x = goal.x; + planner_state->goal_y = goal.y; if(server->isPreemptRequested()) { //Goal is already stopped by client return; } + + + } int main(int argc, char** argv){ - int make_plan + PlannerState planner_state; ros::init(argc, argv, "path_planner_server"); ros::NodeHandle n; + ros::Rate rate(30.0); ActionServerType server(n, "plan_path", false); - server.registerPlanCallback(boost::bind(newPlanCB, &server)); - server.registerPreemptCallback(boost::bind(preemptCB, &server)); + server.registerPlanCallback(boost::bind(newPlanCB, &server, &planner_state)); + server.registerPreemptCallback(boost::bind(preemptCB, &server, &planner_state)); server.start(); ROS_INFO("Hello world!"); + while(ros::ok()){ + ros::spinOnce(); + if(make_plan){ + PathPlanner plan(current_x, current_y, goal_x, goal_y); + std::list points = plan.getPlan(); + geometry_msgs/Point32 point; + int index = 0; + for(std::list::iterator it = points.begin(); it != points.end(); it++){ + point.x = it->getX(); + point.y = it->getY(); + + ascend_msgs::PathPlannerFeedback feedback; + feedback.points_in_plan[index] = point; + + server.publishFeedback(feedback); + } + } + else{ + rate.sleep(); + } + } + ros::spin(); } \ No newline at end of file From 8b660b0c4c1a178d8b2e87bbac789802c6e89291 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 12 Jan 2018 15:38:09 +0100 Subject: [PATCH 020/200] Fix drone-to-obstacle angle calculation --- launch/control_fsm.launch | 11 +++++++++ launch/test_control_fsm_sim.launch | 6 ----- src/tools/config.cpp | 3 ++- src/tools/obstacle_avoidance.cpp | 39 ++++++++++++++++-------------- 4 files changed, 34 insertions(+), 25 deletions(-) diff --git a/launch/control_fsm.launch b/launch/control_fsm.launch index b4e4ad1..b165139 100644 --- a/launch/control_fsm.launch +++ b/launch/control_fsm.launch @@ -24,6 +24,13 @@ + + + + + + + @@ -49,5 +56,9 @@ + + + + diff --git a/launch/test_control_fsm_sim.launch b/launch/test_control_fsm_sim.launch index bf0fb1c..245b636 100644 --- a/launch/test_control_fsm_sim.launch +++ b/launch/test_control_fsm_sim.launch @@ -8,11 +8,5 @@ - - - - - - diff --git a/src/tools/config.cpp b/src/tools/config.cpp index 64d7826..b343e8a 100644 --- a/src/tools/config.cpp +++ b/src/tools/config.cpp @@ -42,7 +42,7 @@ void Config::loadParams() { ros::NodeHandle n("~"); auto getFloatParam = [&](const std::string& name, float& var) { - if(!n.getParam(name, var)) { + if(n.getParam(name, var)) { ROS_WARN("[Control FSM] Load param failed: %s, using %f", name.c_str(), var); } }; @@ -102,6 +102,7 @@ void Config::loadParams() { getStringParam("land_detector_topic", land_detector_topic); //Obstacles getStringParam("obstacle_state_topic", obstacle_state_topic); + //Obstacle avoidance getFloatParam("obstacle_clearance_side", obstacle_clearance_side); getFloatParam("obstacle_clearance_front", obstacle_clearance_front); getFloatParam("obstacle_clearance_back", obstacle_clearance_back); diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index e4138ec..0726472 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -23,10 +23,7 @@ std::shared_ptr control::ObstacleAvoidance::instance constexpr float PI{3.14159265359f}; bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { - using control::Config; //::obstacle_clearance_side; -// using control::Config::obstacle_clearance_front; -// using control::Config::obstacle_clearance_back; -// using control::Config::obstacle_clearance_checkradius; + using control::Config; bool setpoint_modified = false; @@ -36,29 +33,31 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget for (const auto& obstacle : obstacles){ geometry_msgs::Vector3 delta_drone_obstacle; - delta_drone_obstacle.x = obstacle.x - drone_pose.pose.position.x; - delta_drone_obstacle.y = obstacle.y - drone_pose.pose.position.y; + delta_drone_obstacle.x = drone_pose.pose.position.x - obstacle.x; + delta_drone_obstacle.y = drone_pose.pose.position.y - obstacle.y; delta_drone_obstacle.z = drone_pose.pose.position.z; const float distance_to_obstacle = std::sqrt(std::pow(delta_drone_obstacle.x, 2) + std::pow(delta_drone_obstacle.y, 2)); - if (distance_to_obstacle < Config::obstacle_clearance_checkradius){ + +// if (distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance const float angle = std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta; - ROS_DEBUG("[control]: angle to obstacle: %.3f", angle); +// ROS_INFO_THROTTLE(1, "[control] angles: %.3f, %.3f, %.3f", +// obstacle.theta, std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x), angle); - // obstacle.theta + pi/2 is straight ahead for obstacle. + // obstacle.theta + pi/2 is straight ahead of obstacle. const bool drone_in_front_of_obstacle = angle >= 0; geometry_msgs::Vector3 minimum_delta; minimum_delta.z = 0.0; // not used if (drone_in_front_of_obstacle){ - ROS_DEBUG_THROTTLE(1, "[control]: drone in front of obstacle"); + ROS_INFO("[control]: drone in front of obstacle"); minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle); minimum_delta.y = Config::obstacle_clearance_front/Config::obstacle_clearance_side * (Config::obstacle_clearance_side - std::abs(minimum_delta.x)); } else { - ROS_DEBUG_THROTTLE(1,"[control]: drone behind obstacle"); + ROS_INFO("[control]: drone behind obstacle"); minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle); minimum_delta.y = Config::obstacle_clearance_back * std::sin(angle); @@ -68,18 +67,22 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget if (distance_to_obstacle < minimum_distance) { // need to avoid obstacle setpoint_modified = true; - setpoint->position.x = obstacle.x + minimum_delta.x; - setpoint->position.y = obstacle.y + minimum_delta.y; + //setpoint->position.x = obstacle.x + minimum_delta.x; + //setpoint->position.y = obstacle.y + minimum_delta.y; - ROS_DEBUG_THROTTLE(1, "[control]: obstacle avoidance setpoint: %.2f, %.2f, %.2f", - setpoint->position.x, setpoint->position.y, setpoint->position.z); +// ROS_INFO("[control]: obstacle avoidance setpoint: %.2f, %.2f, %.2f", +// setpoint->position.x, setpoint->position.y, setpoint->position.z); } - } + //} } - - if (!setpoint_modified){ + + // debug helpers + if (!setpoint_modified && obstacles.size() > 0){ ROS_INFO_THROTTLE(2, "Obstacles ignored"); } + if (obstacles.size() == 0){ + ROS_INFO_THROTTLE(2, "No obstacles"); + } //Return true if setpoint has been modified. return setpoint_modified; From c23361c72b48faec2c5994bae4d5dfea66d7dac5 Mon Sep 17 00:00:00 2001 From: atussa Date: Fri, 12 Jan 2018 17:18:01 +0100 Subject: [PATCH 021/200] Fix bugs in path planner node --- CMakeLists.txt | 6 ++++-- include/control/planner/node.hpp | 2 ++ include/control/planner/path_planner.hpp | 1 + src/nodes/path_planner_node.cpp | 22 ++++++++++------------ 4 files changed, 17 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 84a6b58..8e6eacd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,7 +67,7 @@ add_executable(control_fsm_main src/nodes/control_fsm_main) add_executable(land_detector_node src/nodes/land_detector_node) add_executable(action_interface_client src/nodes/action_interface_client.cpp) -add_executable(path_planner_node src/nodes/path_planner_node) +add_executable(path_planner_node src/nodes/path_planner_node src/nodes/path_planner src/nodes/node) target_compile_features(path_planner_node PRIVATE cxx_range_for) ## Specify libraries to link a library or executable target against @@ -76,7 +76,9 @@ target_link_libraries(control_fsm_main control_fsm_lib) target_link_libraries(land_detector_node ${catkin_LIBRARIES}) target_link_libraries(action_interface_client ${catkin_LIBRARIES}) -target_link_libraries(path_planner_node ${catkin_LIBRARIES}) +target_link_libraries(path_planner_node + ${catkin_LIBRARIES} + control_tools_lib) ## Add cmake target dependencies of the executable ## same as for the library above diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp index cb6e17a..9ef46d1 100644 --- a/include/control/planner/node.hpp +++ b/include/control/planner/node.hpp @@ -32,6 +32,8 @@ class Node{ void setParentY(float parent_y){this->parent_y = parent_y;} void updateF(float new_g); + bool obstacle = false; + // Implemented only for the closed list priority queue friend bool operator< (const Node &lhs, const Node &rhs); }; diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index e913ca9..9e7e3cd 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -26,6 +26,7 @@ class PathPlanner{ std::array, GRAPH_SIZE> closed_list; std::list plan; + std::list simple_plan; Node end_node; Node start_node; diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index c269b1f..1a59a3a 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -3,6 +3,7 @@ #include "control/planner/path_planner.hpp" #include "control/tools/drone_handler.hpp" #include +#include "ascend_msgs/PathPlannerAction.h" // typedefs @@ -14,12 +15,12 @@ using ActionServerType = actionlib::SimpleActionServermake_plan = false; //Abort whatever you are doing first! ROS_WARN("Preempted!"); } @@ -27,7 +28,7 @@ void preemptCB(ActionServerType* server, PlannerState* planner_state) { void newPlanCB(ActionServerType* server, PlannerState* planner_state){ planner_state->make_plan = true; - geometry_msgs::PoseStamped current_pose = DroneHandler::getCurrentPose(); + geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); auto& position = current_pose.pose.position; planner_state->current_x = position.x; planner_state->current_y = position.y; @@ -38,16 +39,13 @@ void newPlanCB(ActionServerType* server, PlannerState* planner_state){ //Accept the new goal auto goal = server->acceptNewGoal(); - planner_state->goal_x = goal.x; - planner_state->goal_y = goal.y; + planner_state->goal_x = goal->x; + planner_state->goal_y = goal->y; if(server->isPreemptRequested()) { //Goal is already stopped by client return; } - - - } @@ -61,7 +59,7 @@ int main(int argc, char** argv){ ActionServerType server(n, "plan_path", false); - server.registerPlanCallback(boost::bind(newPlanCB, &server, &planner_state)); + server.registerGoalCallback(boost::bind(newPlanCB, &server, &planner_state)); server.registerPreemptCallback(boost::bind(preemptCB, &server, &planner_state)); server.start(); @@ -69,10 +67,10 @@ int main(int argc, char** argv){ while(ros::ok()){ ros::spinOnce(); - if(make_plan){ - PathPlanner plan(current_x, current_y, goal_x, goal_y); + if(planner_state.make_plan){ + PathPlanner plan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); std::list points = plan.getPlan(); - geometry_msgs/Point32 point; + geometry_msgs::Point32 point; int index = 0; for(std::list::iterator it = points.begin(); it != points.end(); it++){ point.x = it->getX(); From 1d9be29fb01731c5480e861ea8035246d206eb52 Mon Sep 17 00:00:00 2001 From: atussa Date: Sat, 13 Jan 2018 13:17:04 +0100 Subject: [PATCH 022/200] Add function to confirm existing plan --- include/control/planner/path_planner.hpp | 10 +++- src/nodes/path_planner.cpp | 72 ++++++++++++++++++------ src/nodes/path_planner_node.cpp | 3 +- 3 files changed, 64 insertions(+), 21 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 9e7e3cd..0923f09 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -17,6 +17,8 @@ #define GRAPH_SIZE 200 // number of nodes in one direction #define OBSTACLE_RADIUS 0.8 // meters +#define NODE_DISTANCE 0.1 //0.2 // meters +#define DIAGONAL_NODE_DISTANCE 0.1414 //0.283 // meters class PathPlanner{ private: @@ -34,7 +36,7 @@ class PathPlanner{ bool destination_reached = false; public: - PathPlanner(float current_x, float current_y, float target_x, float target_y); + PathPlanner(); void initializeGraph(); void initializeClosedList(); @@ -60,13 +62,15 @@ class PathPlanner{ bool isValidCoordinate(float x, float y); // Use A* to calculate the path - void makePlan(); + void makePlan(float current_x, float current_y, float target_x, float target_y); // Same plan but with fewer points void simplifyPlan(); + bool isPlanSafe(float current_x, float current_y); // These functions are mainly for the visualization - std::array, FIELD_LENGTH*10> getGraph(){return graph;} + std::array, GRAPH_SIZE> getGraph(){return graph;} std::list getPlan(){return plan;} + std::list getSimplePlan(){return simple_plan;} // For the colors in the visualization float max_f = 0; }; diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index f3079fd..a1a402a 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -8,14 +8,7 @@ int coordToIndex(float k) { -PathPlanner::PathPlanner(float current_x, float current_y, float target_x, float target_y){ - start_node = Node(current_x, current_y, 0, 0); - start_node.setParentX(0); - start_node.setParentY(0); - end_node = Node(target_x,target_y, std::numeric_limits::infinity(), 0); - initializeGraph(); - initializeClosedList(); -} +PathPlanner::PathPlanner(){} void PathPlanner::initializeGraph(){ for (float x = 0; x < FIELD_LENGTH; x += 0.1){ @@ -131,21 +124,21 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent void PathPlanner::handleAllSuccessors(float x, float y) { // LOVER Å LØSE DETTE PÅ EN BEDRE MÅTE - handleSuccessor(x+0.1,y, x, y, 0.1); + handleSuccessor(x+NODE_DISTANCE,y, x, y, NODE_DISTANCE); if(destination_reached){return;} - handleSuccessor(x+0.1, y+0.1, x, y, sqrt(0.02)); + handleSuccessor(x+NODE_DISTANCE, y+NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); if(destination_reached){return;} - handleSuccessor(x+0.1, y-0.1, x, y, sqrt(0.02)); + handleSuccessor(x+NODE_DISTANCE, y-NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); if(destination_reached){return;} - handleSuccessor(x, y+0.1, x, y, 0.1); + handleSuccessor(x, y+NODE_DISTANCE, x, y, NODE_DISTANCE); if(destination_reached){return;} - handleSuccessor(x, y-0.1, x, y, 0.1); + handleSuccessor(x, y-NODE_DISTANCE, x, y, NODE_DISTANCE); if(destination_reached){return;} - handleSuccessor(x-0.1, y, x, y, 0.1); + handleSuccessor(x-NODE_DISTANCE, y, x, y, NODE_DISTANCE); if(destination_reached){return;} - handleSuccessor(x-0.1, y+0.1, x, y, sqrt(0.02)); + handleSuccessor(x-NODE_DISTANCE, y+NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); if(destination_reached){return;} - handleSuccessor(x-0.1, y-0.1, x, y, sqrt(0.02)); + handleSuccessor(x-NODE_DISTANCE, y-NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); } bool PathPlanner::isDestination(float x, float y) { @@ -162,7 +155,15 @@ bool PathPlanner::isValidCoordinate(float x, float y) { return(x>=0 && x=0 && y::infinity(), 0); + initializeGraph(); + initializeClosedList(); + // Calculate all f values and set the parents relaxGraph(); @@ -274,4 +275,41 @@ void PathPlanner::simplifyPlan() { for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; } +} + + + +bool PathPlanner::isPlanSafe(float current_x, float current_y) { + if(simple_plan.empty()){ + return false; + } + + std::list::iterator current = simple_plan.begin(); + std::list::iterator next = current; + next++; + while(next != simple_plan.end()){ + if(((current_x < current->getX() && current_x > next->getX()) + || (current_x > current->getX() && current_x < next->getX())) + && ((current_y < current->getY() && current_y > next->getY()) + || (current_y > current->getY() && current_y < next->getY()))){ + std::cout << "between " << current->getX() << " and " << next->getX() << std::endl; + break; + } else{ + simple_plan.erase(current); + current = next; + next++; + } + } + + current = simple_plan.begin(); + next = current; + next++; + while(next != simple_plan.end()){ + if(!isSafeLine(current->getX(), current->getY(), next->getX(), next->getY())){ + return false; + } + current++; + next++; + } + return true; } \ No newline at end of file diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 1a59a3a..74ad071 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -69,7 +69,8 @@ int main(int argc, char** argv){ ros::spinOnce(); if(planner_state.make_plan){ PathPlanner plan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); - std::list points = plan.getPlan(); + plan.makePlan(); + std::list points = plan.getSimplePlan(); geometry_msgs::Point32 point; int index = 0; for(std::list::iterator it = points.begin(); it != points.end(); it++){ From 8fa9a649845789d810f3e59daf225c1f42624a7f Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Sat, 13 Jan 2018 15:39:22 +0100 Subject: [PATCH 023/200] Angle and distance calculation fix --- launch/control_fsm.launch | 2 +- src/tools/obstacle_avoidance.cpp | 41 ++++++++++++++++---------------- 2 files changed, 21 insertions(+), 22 deletions(-) diff --git a/launch/control_fsm.launch b/launch/control_fsm.launch index b165139..2610f3e 100644 --- a/launch/control_fsm.launch +++ b/launch/control_fsm.launch @@ -28,7 +28,7 @@ - + diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 0726472..54f5a5b 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -22,6 +22,10 @@ std::shared_ptr control::ObstacleAvoidance::instance constexpr float PI{3.14159265359f}; +inline float angleWrapper(float angle){ + return angle - 2*PI*floor(angle/(2*PI)); +} + bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { using control::Config; @@ -38,50 +42,45 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget delta_drone_obstacle.z = drone_pose.pose.position.z; const float distance_to_obstacle = std::sqrt(std::pow(delta_drone_obstacle.x, 2) + std::pow(delta_drone_obstacle.y, 2)); -// if (distance_to_obstacle < Config::obstacle_clearance_checkradius){ + if (distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance - const float angle = std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta; -// ROS_INFO_THROTTLE(1, "[control] angles: %.3f, %.3f, %.3f", -// obstacle.theta, std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x), angle); + const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta); +// ROS_INFO_THROTTLE(1, "[control] angle: %.3f", +// /*obstacle.theta, std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x),*/ angle); // obstacle.theta + pi/2 is straight ahead of obstacle. - const bool drone_in_front_of_obstacle = angle >= 0; + const bool drone_in_front_of_obstacle = angle_to_obstacle <= PI; geometry_msgs::Vector3 minimum_delta; minimum_delta.z = 0.0; // not used if (drone_in_front_of_obstacle){ - ROS_INFO("[control]: drone in front of obstacle"); + //ROS_INFO_THROTTLE(0.2, "[control]: %.2f -> drone in front of obstacle", angle_to_obstacle); - minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle); + minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle_to_obstacle); minimum_delta.y = Config::obstacle_clearance_front/Config::obstacle_clearance_side * (Config::obstacle_clearance_side - std::abs(minimum_delta.x)); } else { - ROS_INFO("[control]: drone behind obstacle"); + //ROS_INFO_THROTTLE(0.2, "[control]: %.2f -> drone behind obstacle", angle_to_obstacle); - minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle); - minimum_delta.y = Config::obstacle_clearance_back * std::sin(angle); + minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle_to_obstacle); + minimum_delta.y = Config::obstacle_clearance_back * std::sin(angle_to_obstacle); } const float minimum_distance = std::sqrt(std::pow(minimum_delta.x, 2) + std::pow(minimum_delta.y, 2)); + ROS_INFO_THROTTLE(1, "distances: %.2f\t[%.2f]", distance_to_obstacle, minimum_distance); if (distance_to_obstacle < minimum_distance) { // need to avoid obstacle setpoint_modified = true; - //setpoint->position.x = obstacle.x + minimum_delta.x; - //setpoint->position.y = obstacle.y + minimum_delta.y; - -// ROS_INFO("[control]: obstacle avoidance setpoint: %.2f, %.2f, %.2f", -// setpoint->position.x, setpoint->position.y, setpoint->position.z); + setpoint->position.x = obstacle.x + minimum_delta.x; + setpoint->position.y = obstacle.y + minimum_delta.y; } - //} + } } // debug helpers - if (!setpoint_modified && obstacles.size() > 0){ - ROS_INFO_THROTTLE(2, "Obstacles ignored"); - } - if (obstacles.size() == 0){ - ROS_INFO_THROTTLE(2, "No obstacles"); + if (setpoint_modified){ + ROS_INFO_THROTTLE(1 , "setpoint changed"); } //Return true if setpoint has been modified. From 4bf8bfa6791abbd15e4e8af4cf63fd69468fac88 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 18 Jan 2018 16:23:34 +0100 Subject: [PATCH 024/200] Obstacle was avoided in gazebo --- include/control/tools/setpoint_msg_defines.h | 2 +- launch/control_fsm.launch | 4 +-- src/tools/obstacle_avoidance.cpp | 33 ++++++++++++++------ 3 files changed, 26 insertions(+), 13 deletions(-) diff --git a/include/control/tools/setpoint_msg_defines.h b/include/control/tools/setpoint_msg_defines.h index fa16c3f..33ebcf8 100644 --- a/include/control/tools/setpoint_msg_defines.h +++ b/include/control/tools/setpoint_msg_defines.h @@ -23,4 +23,4 @@ constexpr uint16_t default_mask = IGNORE_VX | IGNORE_VY | IGNORE_VZ | IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ | IGNORE_YAW_RATE; -#endif \ No newline at end of file +#endif diff --git a/launch/control_fsm.launch b/launch/control_fsm.launch index 2610f3e..64beb73 100644 --- a/launch/control_fsm.launch +++ b/launch/control_fsm.launch @@ -28,8 +28,8 @@ - - + + diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 54f5a5b..058dcab 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -26,6 +27,26 @@ inline float angleWrapper(float angle){ return angle - 2*PI*floor(angle/(2*PI)); } +inline float getDistanceToObstacle(const geometry_msgs::Pose& pose, const ascend_msgs::GRState& obstacle){ + geometry_msgs::Vector3 delta_drone_obstacle; + delta_drone_obstacle.x = pose.position.x - obstacle.x; + delta_drone_obstacle.y = pose.position.y - obstacle.y; + delta_drone_obstacle.z = pose.position.z; + const float distance_to_obstacle = std::sqrt(std::pow(delta_drone_obstacle.x, 2) + std::pow(delta_drone_obstacle.y, 2)); + return distance_to_obstacle; +} + +inline float getAngleToObstacle(const geometry_msgs::Pose& pose, const ascend_msgs::GRState obstacle){ + + geometry_msgs::Vector3 delta_drone_obstacle; + delta_drone_obstacle.x = pose.position.x - obstacle.x; + delta_drone_obstacle.y = pose.position.y - obstacle.y; + delta_drone_obstacle.z = pose.position.z; + + const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta); + return angle_to_obstacle; +} + bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { using control::Config; @@ -36,20 +57,12 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget for (const auto& obstacle : obstacles){ - geometry_msgs::Vector3 delta_drone_obstacle; - delta_drone_obstacle.x = drone_pose.pose.position.x - obstacle.x; - delta_drone_obstacle.y = drone_pose.pose.position.y - obstacle.y; - delta_drone_obstacle.z = drone_pose.pose.position.z; - const float distance_to_obstacle = std::sqrt(std::pow(delta_drone_obstacle.x, 2) + std::pow(delta_drone_obstacle.y, 2)); + const float distance_to_obstacle = getDistanceToObstacle(drone_pose.pose, obstacle); if (distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance - const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta); -// ROS_INFO_THROTTLE(1, "[control] angle: %.3f", -// /*obstacle.theta, std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x),*/ angle); + const float angle_to_obstacle = getAngleToObstacle(drone_pose.pose, obstacle); - // obstacle.theta + pi/2 is straight ahead of obstacle. - const bool drone_in_front_of_obstacle = angle_to_obstacle <= PI; geometry_msgs::Vector3 minimum_delta; From ef9132fc67d87341d6048d144c7e205bda832860 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 18 Jan 2018 17:41:12 +0100 Subject: [PATCH 025/200] Make resolution of field optional. --- include/control/planner/node.hpp | 2 +- include/control/planner/path_planner.hpp | 14 ++- src/nodes/path_planner.cpp | 107 ++++++++++++++++------- src/nodes/path_planner_node.cpp | 4 +- 4 files changed, 86 insertions(+), 41 deletions(-) diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp index 9ef46d1..86e5d1b 100644 --- a/include/control/planner/node.hpp +++ b/include/control/planner/node.hpp @@ -1,7 +1,7 @@ #ifndef NODE_HPP #define NODE_HPP -#define FIELD_LENGTH 20 +#define FIELD_LENGTH 20.0 class Node{ private: diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 0923f09..40619c2 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -15,10 +15,12 @@ #include #include -#define GRAPH_SIZE 200 // number of nodes in one direction -#define OBSTACLE_RADIUS 0.8 // meters -#define NODE_DISTANCE 0.1 //0.2 // meters -#define DIAGONAL_NODE_DISTANCE 0.1414 //0.283 // meters +#define OBSTACLE_RADIUS 1 //meters +#define NODE_DISTANCE 0.4 // meters +constexpr float DIAGONAL_NODE_DISTANCE = sqrt(2*NODE_DISTANCE*NODE_DISTANCE); // meters + +constexpr int GRAPH_SIZE = FIELD_LENGTH/NODE_DISTANCE+1.5; // number of nodes in one direction + class PathPlanner{ private: @@ -46,6 +48,8 @@ class PathPlanner{ // Diagonal heuristic - can move in 8 directions from current point float calculateDiagonalHeuristic(float x, float y); + float calculateManhattanHeuristic(float x, float y); + float calculateEuclidianHeuristic(float x, float y); // Print the f-value of each node void printGraph(); @@ -60,11 +64,13 @@ class PathPlanner{ bool isDestination(float x, float y); bool isStart(float x, float y); bool isValidCoordinate(float x, float y); + bool isSafeLine(float x1, float y1, float x2, float y2); // Use A* to calculate the path void makePlan(float current_x, float current_y, float target_x, float target_y); // Same plan but with fewer points void simplifyPlan(); + // Verify the plan from where the drone is to the end point bool isPlanSafe(float current_x, float current_y); // These functions are mainly for the visualization diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index a1a402a..7e5bf0c 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -2,7 +2,8 @@ int coordToIndex(float k) { - int index = static_cast(k*10.0+0.5); + int index = static_cast(k/NODE_DISTANCE+0.5); + //std::cout << k << " ---> " << index << std::endl; return (index); } @@ -11,16 +12,16 @@ int coordToIndex(float k) { PathPlanner::PathPlanner(){} void PathPlanner::initializeGraph(){ - for (float x = 0; x < FIELD_LENGTH; x += 0.1){ - for (float y = 0; y < FIELD_LENGTH; y += 0.1){ + for (float x = 0; x < FIELD_LENGTH; x += NODE_DISTANCE){ + for (float y = 0; y < FIELD_LENGTH; y += NODE_DISTANCE){ if(isStart(x,y)){ graph[coordToIndex(x)][coordToIndex(y)] = start_node; // Push the start node to the open list so that the search starts from the start open_list.push(graph[coordToIndex(x)][coordToIndex(y)]); } else { - graph[coordToIndex(x)][coordToIndex(y)] = Node(x,y, std::numeric_limits::infinity() - , calculateDiagonalHeuristic(x,y)); + graph[coordToIndex(x)][coordToIndex(y)] = + Node(x,y, std::numeric_limits::infinity(), calculateDiagonalHeuristic(x,y)); } } } @@ -36,22 +37,24 @@ void PathPlanner::initializeClosedList(){ void PathPlanner::addObstacle(float center_x, float center_y) { // Find the y value for each x in the circle - for (float x = center_x-OBSTACLE_RADIUS; x < center_x+OBSTACLE_RADIUS; x+=0.1) { + for (float x = center_x-OBSTACLE_RADIUS; x <= center_x+OBSTACLE_RADIUS; x+=NODE_DISTANCE) { float y = sqrt(OBSTACLE_RADIUS*OBSTACLE_RADIUS-(x-center_x)*(x-center_x)); // Do this to fill the circle - for(float i = center_y-y; i<=center_y+y; i+= 0.1) { + for(float i = center_y-y; i<=center_y+y; i+= NODE_DISTANCE) { if (isValidCoordinate(x, i)) { closed_list[coordToIndex(x)][coordToIndex(i)] = true; + graph[coordToIndex(x)][coordToIndex(i)].obstacle = true; } } } // Do the same as over, just switch x and y - for (float y = center_y-OBSTACLE_RADIUS; y < center_y+OBSTACLE_RADIUS; y+=0.1) { + for (float y = center_y-OBSTACLE_RADIUS; y <= center_y+OBSTACLE_RADIUS; y+=NODE_DISTANCE) { float x = sqrt(OBSTACLE_RADIUS*OBSTACLE_RADIUS-(y-center_y)*(y-center_y)); - for(float i = center_x-x; i<=center_x+x; i+= 0.1) { + for(float i = center_x-x; i<=center_x+x; i+= NODE_DISTANCE) { if (isValidCoordinate(i, y)) { closed_list[coordToIndex(i)][coordToIndex(y)] = true; + graph[coordToIndex(i)][coordToIndex(y)].obstacle = true; } } } @@ -59,9 +62,18 @@ void PathPlanner::addObstacle(float center_x, float center_y) { float PathPlanner::calculateDiagonalHeuristic(float x, float y) { + return (calculateEuclidianHeuristic(x,y)); return (std::max(abs(x-end_node.getX()), abs(y-end_node.getY()))); } +float PathPlanner::calculateManhattanHeuristic(float x, float y){ + return (abs(x-end_node.getX()) + abs(y-end_node.getY())); +} + +float PathPlanner::calculateEuclidianHeuristic(float x, float y) { + return(sqrt((x-end_node.getX())*(x-end_node.getX())+(y-end_node.getY())*(y-end_node.getY()))); +} + void PathPlanner::printGraph(){ for(int i = 0; i < GRAPH_SIZE; i++){ for(int j = 0; j < GRAPH_SIZE; j++){ @@ -76,6 +88,7 @@ void PathPlanner::printGraph(){ void PathPlanner::relaxGraph(){ Node current_node; while(!open_list.empty()){ + //std::cout << "open list open" << std::endl; // Always search from the node with lowest f value current_node = open_list.top(); open_list.pop(); @@ -91,6 +104,8 @@ void PathPlanner::relaxGraph(){ } void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent_y, float distance_to_parent) { + //std::cout << "Point: " << x << ", " << y << " Parent: " << parent_x + // << ", " << parent_y << std::endl; if(isValidCoordinate(x,y)){ // Making sure the zero is actually zero and not something like 1.4895e-9 if(x > 0.0 && x < 0.01){x = 0.0;} @@ -100,8 +115,7 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent graph[coordToIndex(x)][coordToIndex(y)].setParentY(parent_y); destination_reached = true; std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; - std::cout << "Destination parent: x=" << graph[coordToIndex(x)][coordToIndex(y)].getParentX() - << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; + std::cout << "Destination parent: x=" << graph[coordToIndex(x)][coordToIndex(y)].getParentX() << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; } // If node is not destination and hasn't been searched yet else if(closed_list[coordToIndex(x)][coordToIndex(y)] == false){ @@ -142,21 +156,38 @@ void PathPlanner::handleAllSuccessors(float x, float y) { } bool PathPlanner::isDestination(float x, float y) { - return ((x < end_node.getX()+0.05 && x>end_node.getX()-0.05) - && (y < end_node.getY()+0.05 && y > end_node.getY()-0.05)); + float margin = NODE_DISTANCE/2; + return ((x < end_node.getX()+margin && x>end_node.getX()-margin) + && (y < end_node.getY()+margin && y > end_node.getY()-margin)); } bool PathPlanner::isStart(float x, float y) { - return ((x < start_node.getX()+0.05 && x>start_node.getX()-0.05) - && (y < start_node.getY()+0.05 && y > start_node.getY()-0.05)); + float margin = NODE_DISTANCE/2; + return ((x < start_node.getX()+margin && x>start_node.getX()-margin) + && (y < start_node.getY()+margin && y > start_node.getY()-margin)); } bool PathPlanner::isValidCoordinate(float x, float y) { return(x>=0 && x=0 && y::iterator it = plan.begin(); it!= plan.end(); ++it){ + std::cout << std::endl << "Whole plan:" << std::endl; + for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; - }*/ + } // Delete unnecessary points simplifyPlan(); @@ -241,20 +288,13 @@ void PathPlanner::simplifyPlan() { x2 = third->getX(); y1 = first->getY(); y2 = third->getY(); - delta_x = x2-x1; - delta_y = y2-y1; - num_points = 20*std::max(abs(delta_x),abs(delta_y)); - for(int i = 0; i < num_points; i++){ - x1 += delta_x/num_points; - y1 += delta_y/num_points; - // checking if the line goes through an obstacle at this point - if(graph[coordToIndex(x1)][coordToIndex(y1)].obstacle){ - hit_obstacle = true; - } - } + // If an obstacle is hit, the points are necessary and the search will start // at the end of the current search. - if(hit_obstacle){ + if(!isSafeLine(x1,y1,x2,y2)){ + //std::cout << "Hit: first = (" << x1 << ", " << y1 << ") Third = (" + // << x2 << ", " << y2 << ")" << std::endl; + first = second; second = first; second++; third = second; @@ -272,13 +312,12 @@ void PathPlanner::simplifyPlan() { } // Print the remaining points + std::cout << "Simple plan: " << std::endl; for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; } } - - bool PathPlanner::isPlanSafe(float current_x, float current_y) { if(simple_plan.empty()){ return false; diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 74ad071..5d3f9df 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -68,8 +68,8 @@ int main(int argc, char** argv){ while(ros::ok()){ ros::spinOnce(); if(planner_state.make_plan){ - PathPlanner plan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); - plan.makePlan(); + PathPlanner plan; + plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); std::list points = plan.getSimplePlan(); geometry_msgs::Point32 point; int index = 0; From 711b3256ed5374aeb1c3f76e0b282e0dd8aba83f Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 22 Jan 2018 11:46:04 +0100 Subject: [PATCH 026/200] Start handling of drone outside field. --- src/nodes/path_planner.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 7e5bf0c..d04eae3 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -188,6 +188,16 @@ bool PathPlanner::isSafeLine(float x1, float y1, float x2, float y2) { void PathPlanner::makePlan(float current_x, float current_y, float target_x, float target_y) { + // If start or end point is not valid, a plan is not created + if(!isValidCoordinate(current_x,current_y)){ + // send error to fsm + return; + } + if(!isValidCoordinate(target_x,target_y)){ + // send error to fsm + return; + } + start_node = Node(current_x, current_y, 0, 0); start_node.setParentX(0); start_node.setParentY(0); From 48d1795eb2554f8d05810ece0418601b795a4b16 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Mon, 29 Jan 2018 07:47:51 +0100 Subject: [PATCH 027/200] More seperation into to functions --- launch/control_fsm.launch | 9 +- src/fsm/state_interface.cpp | 2 +- src/tools/obstacle_avoidance.cpp | 144 ++++++++++++++++++++----------- 3 files changed, 99 insertions(+), 56 deletions(-) diff --git a/launch/control_fsm.launch b/launch/control_fsm.launch index 64beb73..1aa9504 100644 --- a/launch/control_fsm.launch +++ b/launch/control_fsm.launch @@ -25,11 +25,10 @@ - - - - - + + + + diff --git a/src/fsm/state_interface.cpp b/src/fsm/state_interface.cpp index 182a273..8f636c0 100644 --- a/src/fsm/state_interface.cpp +++ b/src/fsm/state_interface.cpp @@ -2,7 +2,7 @@ //This should maby be moved to another file, but which one? -std::vector* StateInterface::getAllStatesVector() { +std::vector* StateInterface::getAllStatesVector() { static std::vector all_states_; return &all_states_; } diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 058dcab..d76a513 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -23,80 +24,118 @@ std::shared_ptr control::ObstacleAvoidance::instance constexpr float PI{3.14159265359f}; -inline float angleWrapper(float angle){ +// wrap any angle to range [0, 2pi) +inline float angleWrapper(const float angle){ return angle - 2*PI*floor(angle/(2*PI)); } -inline float getDistanceToObstacle(const geometry_msgs::Pose& pose, const ascend_msgs::GRState& obstacle){ - geometry_msgs::Vector3 delta_drone_obstacle; - delta_drone_obstacle.x = pose.position.x - obstacle.x; - delta_drone_obstacle.y = pose.position.y - obstacle.y; - delta_drone_obstacle.z = pose.position.z; - const float distance_to_obstacle = std::sqrt(std::pow(delta_drone_obstacle.x, 2) + std::pow(delta_drone_obstacle.y, 2)); +inline float calcDistanceToObstacle(const geometry_msgs::Point& point, const ascend_msgs::GRState& obstacle){ + geometry_msgs::Vector3 delta_to_obstacle; + delta_to_obstacle.x = point.x - obstacle.x; + delta_to_obstacle.y = point.y - obstacle.y; + delta_to_obstacle.z = point.z; + + const float distance_to_obstacle = std::sqrt(std::pow(delta_to_obstacle.x, 2) + std::pow(delta_to_obstacle.y, 2)); + return distance_to_obstacle; } -inline float getAngleToObstacle(const geometry_msgs::Pose& pose, const ascend_msgs::GRState obstacle){ - +inline float calcAngleToObstacle(const geometry_msgs::Point& point, const ascend_msgs::GRState& obstacle){ geometry_msgs::Vector3 delta_drone_obstacle; - delta_drone_obstacle.x = pose.position.x - obstacle.x; - delta_drone_obstacle.y = pose.position.y - obstacle.y; - delta_drone_obstacle.z = pose.position.z; + delta_drone_obstacle.x = point.x - obstacle.x; + delta_drone_obstacle.y = point.y - obstacle.y; + delta_drone_obstacle.z = point.z; const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta); + return angle_to_obstacle; } +inline geometry_msgs::Vector3 rotateXY(geometry_msgs::Vector3 point, float angle){ + // Apply 2d transformation matrix + point.x = point.x * std::cos(angle) - point.y * std::sin(angle); + point.y = point.x * std::sin(angle) + point.y * std::cos(angle); + + return point; +} + +/// Return a vector (x,y,z) which corresponds to the point closest to the obstacle +/// which is allowed, referenced from the obstacles coordinate system. y-axis is positive +/// direction of motion, and x-axis is the rightward direction. +/// As of jan. 2018, z is always 0.0 since only the xy-plane is concidered +/// TODO: The angle references are most likely wrong and may need a constant offset to be correct. +/// Fix once perception algorithm is implemented!!!! +inline geometry_msgs::Vector3 avoidZone(const float angle, const float front_clearance, const float back_clearance, const float side_clearance){ + const bool drone_in_front_of_obstacle = angle <= PI; + + geometry_msgs::Vector3 minimum_vector; + minimum_vector.z = 0.0f; + + if (drone_in_front_of_obstacle){ + minimum_vector.x = side_clearance * std::cos(angle); + minimum_vector.y = front_clearance/side_clearance * (side_clearance - std::abs(minimum_vector.x)); + } + else { + minimum_vector.x = side_clearance * std::cos(angle); + minimum_vector.y = back_clearance * std::sin(angle); + } + + return minimum_vector; +} + bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { using control::Config; - - bool setpoint_modified = false; + bool setpoint_modified{false}; - const std::vector obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); const geometry_msgs::PoseStamped drone_pose = control::DroneHandler::getCurrentPose(); for (const auto& obstacle : obstacles){ + + const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle); + const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle); - const float distance_to_obstacle = getDistanceToObstacle(drone_pose.pose, obstacle); - - if (distance_to_obstacle < Config::obstacle_clearance_checkradius){ + if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance - const float angle_to_obstacle = getAngleToObstacle(drone_pose.pose, obstacle); - - const bool drone_in_front_of_obstacle = angle_to_obstacle <= PI; + const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle); + + const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle); + // True if setpoint is within a 120 deg cone away from the obstacle + // TODO: turn this angle into a param once complete + const bool setpoint_reachable = + (drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && + setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); - geometry_msgs::Vector3 minimum_delta; - minimum_delta.z = 0.0; // not used - if (drone_in_front_of_obstacle){ - //ROS_INFO_THROTTLE(0.2, "[control]: %.2f -> drone in front of obstacle", angle_to_obstacle); + geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, + Config::obstacle_clearance_front, Config::obstacle_clearance_back, Config::obstacle_clearance_side); - minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle_to_obstacle); - minimum_delta.y = Config::obstacle_clearance_front/Config::obstacle_clearance_side * (Config::obstacle_clearance_side - std::abs(minimum_delta.x)); - } - else { - //ROS_INFO_THROTTLE(0.2, "[control]: %.2f -> drone behind obstacle", angle_to_obstacle); + // Rotate to global coordinate system + minimum_vector = rotateXY(minimum_vector, -obstacle.theta); - minimum_delta.x = Config::obstacle_clearance_side * std::cos(angle_to_obstacle); - minimum_delta.y = Config::obstacle_clearance_back * std::sin(angle_to_obstacle); - } + const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); - const float minimum_distance = std::sqrt(std::pow(minimum_delta.x, 2) + std::pow(minimum_delta.y, 2)); - ROS_INFO_THROTTLE(1, "distances: %.2f\t[%.2f]", distance_to_obstacle, minimum_distance); - if (distance_to_obstacle < minimum_distance) { + if (setpoint_reachable + && setpoint_distance_to_obstacle > drone_distance_to_obstacle + && setpoint_distance_to_obstacle > minimum_distance + && drone_distance_to_obstacle < minimum_distance){ + // no action, maybe logging? + ROS_INFO("Going to setpoint instead"); + } + else if (drone_distance_to_obstacle < minimum_distance) { + ROS_WARN_COND(setpoint_modified, "[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); // need to avoid obstacle setpoint_modified = true; - setpoint->position.x = obstacle.x + minimum_delta.x; - setpoint->position.y = obstacle.y + minimum_delta.y; + setpoint->position.x = obstacle.x + minimum_vector.x; + setpoint->position.y = obstacle.y + minimum_vector.y; } } - } - - // debug helpers - if (setpoint_modified){ - ROS_INFO_THROTTLE(1 , "setpoint changed"); + + if (setpoint_modified){ + const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle); + ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); + } } - //Return true if setpoint has been modified. return setpoint_modified; } @@ -125,12 +164,17 @@ std::shared_ptr control::ObstacleAvoidance::getShare } void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr >& cb_p) { - for(auto it = on_modified_cb_set_.begin(); it != on_modified_cb_set_.end(); ++it) { - if(*it == cb_p) { - on_modified_cb_set_.erase(it); - //std::set - no duplicates - return; - } + const auto it = on_modified_cb_set_.find(cb_p); + if (it != on_modified_cb_set_.cend()){ + on_modified_cb_set_.erase(it); } + + //for(auto it = on_modified_cb_set_.begin(); it != on_modified_cb_set_.end(); ++it) { + // if(*it == cb_p) { + // on_modified_cb_set_.erase(it); + // //std::set - no duplicates + // return; + // } + //} } From 2a09967b7ff3c51e385bbce5d776caf1af865315 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Feb 2018 14:45:24 +0100 Subject: [PATCH 028/200] Add callback for refreshing obstacle coordinates. --- include/control/planner/path_planner.hpp | 5 ++ src/nodes/path_planner.cpp | 85 +++++++++++++++--------- src/nodes/path_planner_node.cpp | 25 ++++++- 3 files changed, 83 insertions(+), 32 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 40619c2..eb2e9c5 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -32,9 +32,12 @@ class PathPlanner{ std::list plan; std::list simple_plan; + std::list obstacle_coordinates; + Node end_node; Node start_node; + bool noPlan = true; bool destination_reached = false; public: @@ -45,6 +48,7 @@ class PathPlanner{ // Add a circular obstacle void addObstacle(float center_x, float center_y); + void refreshObstacles(std::list obstacle_coordinates); // Diagonal heuristic - can move in 8 directions from current point float calculateDiagonalHeuristic(float x, float y); @@ -72,6 +76,7 @@ class PathPlanner{ void simplifyPlan(); // Verify the plan from where the drone is to the end point bool isPlanSafe(float current_x, float current_y); + void resetParameters(); // These functions are mainly for the visualization std::array, GRAPH_SIZE> getGraph(){return graph;} diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index d04eae3..55ce088 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -3,7 +3,6 @@ int coordToIndex(float k) { int index = static_cast(k/NODE_DISTANCE+0.5); - //std::cout << k << " ---> " << index << std::endl; return (index); } @@ -60,6 +59,21 @@ void PathPlanner::addObstacle(float center_x, float center_y) { } } +void PathPlanner::refreshObstacles(std::list obstacle_coordinates) { + this->obstacle_coordinates = obstacle_coordinates; + if(noPlan){ + return; + } + std::list::iterator x = obstacle_coordinates.begin(); + std::list::iterator y = x; + y++; + while(y != obstacle_coordinates.end()){ + std::cout << "adding obstacle" << std::endl; + addObstacle(*x, *y); + x++; + y++; + } +} float PathPlanner::calculateDiagonalHeuristic(float x, float y) { return (calculateEuclidianHeuristic(x,y)); @@ -88,7 +102,6 @@ void PathPlanner::printGraph(){ void PathPlanner::relaxGraph(){ Node current_node; while(!open_list.empty()){ - //std::cout << "open list open" << std::endl; // Always search from the node with lowest f value current_node = open_list.top(); open_list.pop(); @@ -162,7 +175,7 @@ bool PathPlanner::isDestination(float x, float y) { } bool PathPlanner::isStart(float x, float y) { - float margin = NODE_DISTANCE/2; + float margin = NODE_DISTANCE/1.5; return ((x < start_node.getX()+margin && x>start_node.getX()-margin) && (y < start_node.getY()+margin && y > start_node.getY()-margin)); } @@ -179,8 +192,10 @@ bool PathPlanner::isSafeLine(float x1, float y1, float x2, float y2) { x1 += delta_x/num_points; y1 += delta_y/num_points; // checking if the line goes through an obstacle at this point - if(graph[coordToIndex(x1)][coordToIndex(y1)].obstacle){ - return false; + if(isValidCoordinate(x1,y1)){ + if (graph[coordToIndex(x1)][coordToIndex(y1)].obstacle) { + return false; + } } } return true; @@ -188,6 +203,8 @@ bool PathPlanner::isSafeLine(float x1, float y1, float x2, float y2) { void PathPlanner::makePlan(float current_x, float current_y, float target_x, float target_y) { + noPlan = false; + // If start or end point is not valid, a plan is not created if(!isValidCoordinate(current_x,current_y)){ // send error to fsm @@ -198,6 +215,8 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo return; } + resetParameters(); + start_node = Node(current_x, current_y, 0, 0); start_node.setParentX(0); start_node.setParentY(0); @@ -205,12 +224,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo initializeGraph(); initializeClosedList(); - //addObstacle(8.8,13); - addObstacle(7.2,13); - addObstacle(9,15); - addObstacle(10.4,13); - addObstacle(15.5,8); - + refreshObstacles(obstacle_coordinates); float x = end_node.getX(); float y = end_node.getY(); @@ -229,7 +243,6 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // Add nodes to the plan while(!isStart(x, y)){ - std::cout << "Iterate backwards through plan" << std::endl; plan.push_front(graph[coordToIndex(x)][coordToIndex(y)]); float x_temp = graph[coordToIndex(x)][coordToIndex(y)].getParentX(); float y_temp = graph[coordToIndex(x)][coordToIndex(y)].getParentY(); @@ -242,10 +255,10 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // Add start node to the plan (might not be necessary) plan.push_front(graph[coordToIndex(x)][coordToIndex(y)]); // Print the plan - can be removed - std::cout << std::endl << "Whole plan:" << std::endl; + /*std::cout << std::endl << "Whole plan:" << std::endl; for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; - } + }*/ // Delete unnecessary points simplifyPlan(); @@ -328,30 +341,33 @@ void PathPlanner::simplifyPlan() { } } -bool PathPlanner::isPlanSafe(float current_x, float current_y) { +bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { if(simple_plan.empty()){ return false; } - std::list::iterator current = simple_plan.begin(); - std::list::iterator next = current; - next++; - while(next != simple_plan.end()){ - if(((current_x < current->getX() && current_x > next->getX()) - || (current_x > current->getX() && current_x < next->getX())) - && ((current_y < current->getY() && current_y > next->getY()) - || (current_y > current->getY() && current_y < next->getY()))){ - std::cout << "between " << current->getX() << " and " << next->getX() << std::endl; + std::list::iterator prev = simple_plan.begin(); + std::list::iterator current = prev; + current++; + + while(current != simple_plan.end()){ + if(abs(current->getX()-setpoint_x)<0.1 && abs(current->getY()-setpoint_y) < 0.1){ + std::cout << "Setpoint: " << current->getX() << ", " << current->getY() << std::endl; + std::cout << "Search from:" << prev->getX() << ", " << current->getY() << std::endl; break; - } else{ - simple_plan.erase(current); - current = next; - next++; + } + else{ + //simple_plan.erase(current); + prev = current; + current++; + if(current == simple_plan.end()){ + return false; + } } } - current = simple_plan.begin(); - next = current; + current = prev; + std::list::iterator next = current; next++; while(next != simple_plan.end()){ if(!isSafeLine(current->getX(), current->getY(), next->getX(), next->getY())){ @@ -361,4 +377,13 @@ bool PathPlanner::isPlanSafe(float current_x, float current_y) { next++; } return true; +} + +void PathPlanner::resetParameters() { + while(!open_list.empty()){ + open_list.pop(); + } + plan.clear(); + simple_plan.clear(); + destination_reached = false; } \ No newline at end of file diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 5d3f9df..83bd4dd 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -3,7 +3,9 @@ #include "control/planner/path_planner.hpp" #include "control/tools/drone_handler.hpp" #include +#include #include "ascend_msgs/PathPlannerAction.h" +#include "ascend_msgs/GRStateArray.h" // typedefs @@ -49,9 +51,20 @@ void newPlanCB(ActionServerType* server, PlannerState* planner_state){ } +void updateObstaclesCB(std::list *obstacle_coordinates ,ascend_msgs::GRStateArray::ConstPtr& msg_p){ + obstacle_coordinates->clear(); + for(std::vector::iterator it = msg_p.begin(); it != msg_p.end(); ++it) { + obstacle_coordinates.push_back(msg_p->x); + obstacle_coordinates.push_back(msg_p->y); + } +} + + int main(int argc, char** argv){ PlannerState planner_state; + std::list obstacle_coordinates; + PathPlanner plan; ros::init(argc, argv, "path_planner_server"); ros::NodeHandle n; @@ -63,12 +76,20 @@ int main(int argc, char** argv){ server.registerPreemptCallback(boost::bind(preemptCB, &server, &planner_state)); server.start(); + ros::Subscriber sub = n.subscribe("", 1, boost::bind(updateObstaclesCB, _1,&obstacle_coordinates)); + ROS_INFO("Hello world!"); while(ros::ok()){ + ros::spinOnce(); - if(planner_state.make_plan){ - PathPlanner plan; + + plan.refreshObstacles(obstacle_coordinates); + + float setpoint_x = 0; + float setpoint_y = 0; + // SELVOM FORRIGE PLAN ER SAFE MÅ EN NY PLAN LAGES HVIS DET ER GITT NYTT MÅL + if(planner_state.make_plan && !plan.isPlanSafe(setpoint_x,setpoint_y)){ // MÅ HENTE INN NESTE SETPOINT FRA GOTO STATE plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); std::list points = plan.getSimplePlan(); geometry_msgs::Point32 point; From dfdaca4a7084b1d4177f8d9cb2c163420f8d5cd9 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Feb 2018 17:09:13 +0100 Subject: [PATCH 029/200] Change from CB function parameter to global variable. --- src/nodes/path_planner_node.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 83bd4dd..d80232b 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -12,7 +12,7 @@ using ActionServerType = actionlib::SimpleActionServer; - +std::list obstacle_coordinates; struct PlannerState{ float current_x, current_y, goal_x, goal_y; @@ -51,11 +51,11 @@ void newPlanCB(ActionServerType* server, PlannerState* planner_state){ } -void updateObstaclesCB(std::list *obstacle_coordinates ,ascend_msgs::GRStateArray::ConstPtr& msg_p){ - obstacle_coordinates->clear(); - for(std::vector::iterator it = msg_p.begin(); it != msg_p.end(); ++it) { - obstacle_coordinates.push_back(msg_p->x); - obstacle_coordinates.push_back(msg_p->y); +void updateObstaclesCB(ascend_msgs::GRStateArray::ConstPtr msg_p){ + obstacle_coordinates.clear(); + for(auto it = msg_p->states.begin(); it != msg_p->states.end(); ++it) { + obstacle_coordinates.push_back(it->x); + obstacle_coordinates.push_back(it->y); } } @@ -63,7 +63,6 @@ void updateObstaclesCB(std::list *obstacle_coordinates ,ascend_msgs::GRSt int main(int argc, char** argv){ PlannerState planner_state; - std::list obstacle_coordinates; PathPlanner plan; ros::init(argc, argv, "path_planner_server"); @@ -76,7 +75,7 @@ int main(int argc, char** argv){ server.registerPreemptCallback(boost::bind(preemptCB, &server, &planner_state)); server.start(); - ros::Subscriber sub = n.subscribe("", 1, boost::bind(updateObstaclesCB, _1,&obstacle_coordinates)); + ros::Subscriber sub = n.subscribe("", 1, updateObstaclesCB); ROS_INFO("Hello world!"); From 520ef9cef7e6524f4eb412da685fd33c88766ef1 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 5 Feb 2018 17:38:14 +0100 Subject: [PATCH 030/200] Write error message for drone or target outside field. --- src/nodes/path_planner.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 55ce088..ac3c7c5 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -207,11 +207,11 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // If start or end point is not valid, a plan is not created if(!isValidCoordinate(current_x,current_y)){ - // send error to fsm + ROS_INFO("Drone outside field!"); return; } if(!isValidCoordinate(target_x,target_y)){ - // send error to fsm + ROS_INFO("Target outside field!"); return; } From 412d943177dd5c1b8d33e16dc718e33ed1540fa3 Mon Sep 17 00:00:00 2001 From: atussa Date: Tue, 6 Feb 2018 13:36:16 +0100 Subject: [PATCH 031/200] Create namespace for path planner --- include/control/planner/path_planner.hpp | 4 ++++ src/nodes/path_planner.cpp | 4 +++- src/nodes/path_planner_node.cpp | 1 + 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index eb2e9c5..f0af612 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -22,6 +22,9 @@ constexpr float DIAGONAL_NODE_DISTANCE = sqrt(2*NODE_DISTANCE*NODE_DISTANCE); // constexpr int GRAPH_SIZE = FIELD_LENGTH/NODE_DISTANCE+1.5; // number of nodes in one direction +namespace control{ + +class PathPlanner; class PathPlanner{ private: std::array, GRAPH_SIZE> graph; @@ -85,6 +88,7 @@ class PathPlanner{ // For the colors in the visualization float max_f = 0; }; +} // Convert between coordinate x or y in metres and index in graph int coordToIndex(float k); diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index ac3c7c5..4942a95 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -1,6 +1,8 @@ #include "control/planner/path_planner.hpp" +using control::PathPlanner; + int coordToIndex(float k) { int index = static_cast(k/NODE_DISTANCE+0.5); return (index); @@ -302,7 +304,7 @@ void PathPlanner::simplifyPlan() { third++; third++; - float x1, x2, y1, y2, delta_x, delta_y, num_points; + float x1, x2, y1, y2; bool hit_obstacle = false; while(third != simple_plan.end()){ diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index d80232b..b2f7878 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -7,6 +7,7 @@ #include "ascend_msgs/PathPlannerAction.h" #include "ascend_msgs/GRStateArray.h" +using control::PathPlanner; // typedefs From f51f8d7c7ff83afa3ff67ce2a415ebc6535b1999 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Feb 2018 13:45:05 +0100 Subject: [PATCH 032/200] Make namespace more specific --- include/control/planner/path_planner.hpp | 14 ++++++++------ src/nodes/path_planner.cpp | 8 +++----- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index f0af612..ea4e7eb 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -15,15 +15,16 @@ #include #include -#define OBSTACLE_RADIUS 1 //meters -#define NODE_DISTANCE 0.4 // meters +namespace control{ +namespace pathplanner{ + +constexpr float OBSTACLE_RADIUS = 1.0; //meters +constexpr float NODE_DISTANCE = 0.4; // meters constexpr float DIAGONAL_NODE_DISTANCE = sqrt(2*NODE_DISTANCE*NODE_DISTANCE); // meters constexpr int GRAPH_SIZE = FIELD_LENGTH/NODE_DISTANCE+1.5; // number of nodes in one direction -namespace control{ - class PathPlanner; class PathPlanner{ private: @@ -88,10 +89,11 @@ class PathPlanner{ // For the colors in the visualization float max_f = 0; }; -} // Convert between coordinate x or y in metres and index in graph -int coordToIndex(float k); +//int coordToIndex(float k); +} +} #endif // PATH_PLANNER_HPP diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp index 4942a95..af4fe66 100644 --- a/src/nodes/path_planner.cpp +++ b/src/nodes/path_planner.cpp @@ -1,7 +1,7 @@ #include "control/planner/path_planner.hpp" -using control::PathPlanner; +using namespace control::pathplanner; int coordToIndex(float k) { int index = static_cast(k/NODE_DISTANCE+0.5); @@ -9,7 +9,7 @@ int coordToIndex(float k) { } - +using control::pathplanner::PathPlanner; PathPlanner::PathPlanner(){} void PathPlanner::initializeGraph(){ @@ -305,7 +305,6 @@ void PathPlanner::simplifyPlan() { third++; float x1, x2, y1, y2; - bool hit_obstacle = false; while(third != simple_plan.end()){ // Parameters for finding the straight line between the first and third point @@ -333,7 +332,6 @@ void PathPlanner::simplifyPlan() { second = third; third++; } - hit_obstacle = false; } // Print the remaining points @@ -353,13 +351,13 @@ bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { current++; while(current != simple_plan.end()){ + // Find next point in plan by checking distance between setpoint and points in plan if(abs(current->getX()-setpoint_x)<0.1 && abs(current->getY()-setpoint_y) < 0.1){ std::cout << "Setpoint: " << current->getX() << ", " << current->getY() << std::endl; std::cout << "Search from:" << prev->getX() << ", " << current->getY() << std::endl; break; } else{ - //simple_plan.erase(current); prev = current; current++; if(current == simple_plan.end()){ From daeb06b5cf3a4e3cec4aae1f47cff94a9765ce3e Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Feb 2018 13:46:22 +0100 Subject: [PATCH 033/200] Subscribe to setpoint --- src/nodes/path_planner_node.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index b2f7878..7a11a0e 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -6,18 +6,22 @@ #include #include "ascend_msgs/PathPlannerAction.h" #include "ascend_msgs/GRStateArray.h" +#include -using control::PathPlanner; +using control::pathplanner::PathPlanner; // typedefs using ActionServerType = actionlib::SimpleActionServer; std::list obstacle_coordinates; +float setpoint_x = 0; +float setpoint_y = 0; struct PlannerState{ float current_x, current_y, goal_x, goal_y; bool make_plan; + bool new_goal; }; @@ -41,6 +45,7 @@ void newPlanCB(ActionServerType* server, PlannerState* planner_state){ } //Accept the new goal + planner_state->new_goal = true; auto goal = server->acceptNewGoal(); planner_state->goal_x = goal->x; planner_state->goal_y = goal->y; @@ -60,6 +65,11 @@ void updateObstaclesCB(ascend_msgs::GRStateArray::ConstPtr msg_p){ } } +void updateSetpointCB(mavros_msgs::PositionTarget::ConstPtr msg_p){ + setpoint_x = msg_p->position.x; + setpoint_y = msg_p->position.y; +} + int main(int argc, char** argv){ @@ -76,9 +86,10 @@ int main(int argc, char** argv){ server.registerPreemptCallback(boost::bind(preemptCB, &server, &planner_state)); server.start(); - ros::Subscriber sub = n.subscribe("", 1, updateObstaclesCB); + ros::Subscriber sub_obstacles = n.subscribe("", 1, updateObstaclesCB); + ros::Subscriber sub_setpoint = n.subscribe("", 1, updateSetpointCB); + - ROS_INFO("Hello world!"); while(ros::ok()){ @@ -86,10 +97,10 @@ int main(int argc, char** argv){ plan.refreshObstacles(obstacle_coordinates); - float setpoint_x = 0; - float setpoint_y = 0; // SELVOM FORRIGE PLAN ER SAFE MÅ EN NY PLAN LAGES HVIS DET ER GITT NYTT MÅL - if(planner_state.make_plan && !plan.isPlanSafe(setpoint_x,setpoint_y)){ // MÅ HENTE INN NESTE SETPOINT FRA GOTO STATE + if(planner_state.make_plan && (!plan.isPlanSafe(setpoint_x,setpoint_y) || planner_state.new_goal)){ + ROS_INFO("Make new plan."); + planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); std::list points = plan.getSimplePlan(); geometry_msgs::Point32 point; From 8f6d11d774e0e763c52de1641a1a6928e0f4f2c0 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Feb 2018 14:01:00 +0100 Subject: [PATCH 034/200] Minor changes --- src/nodes/path_planner_node.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 7a11a0e..c59c937 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -90,14 +90,13 @@ int main(int argc, char** argv){ ros::Subscriber sub_setpoint = n.subscribe("", 1, updateSetpointCB); - while(ros::ok()){ ros::spinOnce(); plan.refreshObstacles(obstacle_coordinates); - // SELVOM FORRIGE PLAN ER SAFE MÅ EN NY PLAN LAGES HVIS DET ER GITT NYTT MÅL + if(planner_state.make_plan && (!plan.isPlanSafe(setpoint_x,setpoint_y) || planner_state.new_goal)){ ROS_INFO("Make new plan."); planner_state.new_goal = false; From b46c532b2819fdafeda17dab909091a77ed029db Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 8 Feb 2018 14:06:39 +0100 Subject: [PATCH 035/200] Minor changes --- launch/control_fsm.launch | 4 ++-- src/tools/obstacle_avoidance.cpp | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/launch/control_fsm.launch b/launch/control_fsm.launch index 1aa9504..7b8d82d 100644 --- a/launch/control_fsm.launch +++ b/launch/control_fsm.launch @@ -25,8 +25,8 @@ - - + + diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index d76a513..284eaf8 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -72,10 +72,12 @@ inline geometry_msgs::Vector3 avoidZone(const float angle, const float front_cle minimum_vector.z = 0.0f; if (drone_in_front_of_obstacle){ + // Triangle shape minimum_vector.x = side_clearance * std::cos(angle); minimum_vector.y = front_clearance/side_clearance * (side_clearance - std::abs(minimum_vector.x)); } else { + // Ellipse minimum_vector.x = side_clearance * std::cos(angle); minimum_vector.y = back_clearance * std::sin(angle); } @@ -87,7 +89,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget using control::Config; bool setpoint_modified{false}; - const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); // Format of isn't finalized yet const geometry_msgs::PoseStamped drone_pose = control::DroneHandler::getCurrentPose(); for (const auto& obstacle : obstacles){ @@ -114,6 +116,8 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); + ROS_INFO_THROTTLE(1, "Minimum distance: %.3f", minimum_distance); + if (setpoint_reachable && setpoint_distance_to_obstacle > drone_distance_to_obstacle && setpoint_distance_to_obstacle > minimum_distance From 01a4157490c7bfa057dd495e69e52d726a70d416 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Feb 2018 15:32:00 +0100 Subject: [PATCH 036/200] Add em pty config file. --- src/nodes/path_planner_node.cpp | 2 +- src/tools/planner_config.cpp | 0 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 src/tools/planner_config.cpp diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index c59c937..49886bc 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -109,7 +109,7 @@ int main(int argc, char** argv){ point.y = it->getY(); ascend_msgs::PathPlannerFeedback feedback; - feedback.points_in_plan[index] = point; + feedback.points_in_plan[index++] = point; server.publishFeedback(feedback); } diff --git a/src/tools/planner_config.cpp b/src/tools/planner_config.cpp new file mode 100644 index 0000000..e69de29 From bd8a1477fcd9f2c77b38ceb899a7aade430167a3 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Feb 2018 15:40:14 +0100 Subject: [PATCH 037/200] Copy relevant code from config to planner_config --- include/control/tools/planner_config.hpp | 43 +++++++++ src/tools/planner_config.cpp | 107 +++++++++++++++++++++++ 2 files changed, 150 insertions(+) create mode 100644 include/control/tools/planner_config.hpp diff --git a/include/control/tools/planner_config.hpp b/include/control/tools/planner_config.hpp new file mode 100644 index 0000000..6793bfd --- /dev/null +++ b/include/control/tools/planner_config.hpp @@ -0,0 +1,43 @@ +#ifndef FSM_CONFIG_HPP +#define FSM_CONFIG_HPP +#include +#include +#include + +/** @page fsm_params FSM Params + * @brief Parameter page + * + * Contains all fsm params + */ + + +namespace control { +class Config; +class Config { +private: + ///Node handler + ros::NodeHandle nh_; + ///Reload service + ros::ServiceServer reload_config_service; + static std::set missing_param_set_; + ///Shared instance ptr + static std::unique_ptr shared_instance_p_; + ///Constructor + Config(); + + +public: + /// \fsmparam Topic for recieving obstacle positions + static std::string obstacle_state_topic; + + /**Load paramaters + * @throw control::ROSNotInitializedException + */ + static void loadParams(); + + ///Returns set of unloaded params + static const std::set& getMissingParamSet() { return missing_param_set_; } +}; +} + +#endif diff --git a/src/tools/planner_config.cpp b/src/tools/planner_config.cpp index e69de29..102fbd7 100644 --- a/src/tools/planner_config.cpp +++ b/src/tools/planner_config.cpp @@ -0,0 +1,107 @@ +#include "control/tools/config.hpp" +#include +#include +#include + +using control::Config; +using control::ROSNotInitializedException; + + +std::set Config::missing_param_set_; +std::unique_ptr Config::shared_instance_p_ = nullptr; + +//Global config params + +std::string Config::obstacle_state_topic = "/perception_obstacle_states"; + + +void Config::loadParams() { + if(!ros::isInitialized()) { + throw ROSNotInitializedException(); + } + if(shared_instance_p_ == nullptr) { + shared_instance_p_ = std::unique_ptr(new Config); + } + ros::NodeHandle n("~"); + auto getDoubleParam = [&](const std::string& name, double& var, double min, double max) { + double temp; + if(!n.getParam(name, temp) || temp < min || temp > max) { + std::string warn_msg = "Load param failed: "; + warn_msg += name; + warn_msg += ", using "; + warn_msg += std::to_string(var); + control::handleWarnMsg(warn_msg); + missing_param_set_.insert(name); + } else { + var = temp; + control::handleInfoMsg("Param " + name + " loaded: " + std::to_string(var)); + } + }; + + auto getStringParam = [&](const std::string& name, std::string& var) { + if(!n.getParam(name, var)) { + std::string warn_msg = "Load param failed: "; + warn_msg += name; + warn_msg += ", using "; + warn_msg += var; + control::handleWarnMsg(warn_msg); + missing_param_set_.insert(name); + } else { + control::handleInfoMsg("Param " + name + " loaded: " + var); + } + }; + + auto getIntParam = [&](const std::string& name, int& var, int min, int max) { + int temp; + if(!n.getParam(name, temp) || temp < min || temp > max) { + std::string warn_msg = "Load param failed: "; + warn_msg += name; + warn_msg += ", using "; + warn_msg += std::to_string(var); + control::handleWarnMsg(warn_msg); + missing_param_set_.insert(name); + } else { + var = temp; + control::handleInfoMsg("Param " + name + " loaded: " + std::to_string(var)); + } + }; + + auto getBoolParam = [&](const std::string& name, bool& var) { + if(!n.getParam(name, var)) { + std::string warn_msg = "Load param failed: "; + warn_msg += name; + warn_msg += ", using "; + warn_msg += std::to_string(var); + control::handleWarnMsg(warn_msg); + missing_param_set_.insert(name); + } else { + control::handleInfoMsg("Param " + name + " loaded: " + std::to_string(var)); + } + }; + + //Global params (used by multiple states) + + //Obstacles + getStringParam("obstacle_state_topic", obstacle_state_topic); + +} + +using Request = ascend_msgs::ReloadConfig::Request; +using Response = ascend_msgs::ReloadConfig::Response; +bool reloadConfigCB(Request&, Response& resp) { + control::Config::loadParams(); + control::handleInfoMsg("Config reloaded by service!"); + + //Missing param set should be empty! + for(auto& s : control::Config::getMissingParamSet()) { + resp.missing_params.emplace_back(s); + } + return true; +} + +control::Config::Config() { + if(!ros::isInitialized()) { + throw control::ROSNotInitializedException(); + } + reload_config_service = nh_.advertiseService("/control_fsm_reload_config", reloadConfigCB); +} From 886c0f644ed97d7140e4aa78b79b82ab5fdaeb2a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Sat, 10 Feb 2018 16:37:16 +0100 Subject: [PATCH 038/200] Update config.cpp --- src/tools/config.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tools/config.cpp b/src/tools/config.cpp index c2de5d0..4ebb982 100644 --- a/src/tools/config.cpp +++ b/src/tools/config.cpp @@ -168,4 +168,4 @@ control::Config::Config() { throw control::ROSNotInitializedException(); } reload_config_service = nh_.advertiseService("/control_fsm_reload_config", reloadConfigCB); -} \ No newline at end of file +} From 3a1eae3a83f1906575dacc3badac41de0a32b3bf Mon Sep 17 00:00:00 2001 From: atussa Date: Tue, 13 Feb 2018 12:38:50 +0100 Subject: [PATCH 039/200] Commentpath planner node. --- src/nodes/path_planner_node.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 49886bc..90b50c5 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -7,6 +7,7 @@ #include "ascend_msgs/PathPlannerAction.h" #include "ascend_msgs/GRStateArray.h" #include +#include "control/tools/planner_config.hpp" using control::pathplanner::PathPlanner; @@ -15,6 +16,7 @@ using control::pathplanner::PathPlanner; using ActionServerType = actionlib::SimpleActionServer; std::list obstacle_coordinates; +// Only setpoints within plan (not goal) float setpoint_x = 0; float setpoint_y = 0; @@ -35,6 +37,7 @@ void preemptCB(ActionServerType* server, PlannerState* planner_state) { void newPlanCB(ActionServerType* server, PlannerState* planner_state){ planner_state->make_plan = true; + // Update planner state geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); auto& position = current_pose.pose.position; planner_state->current_x = position.x; @@ -80,13 +83,15 @@ int main(int argc, char** argv){ ros::NodeHandle n; ros::Rate rate(30.0); + control::PlannerConfig::loadParams(); + ActionServerType server(n, "plan_path", false); server.registerGoalCallback(boost::bind(newPlanCB, &server, &planner_state)); server.registerPreemptCallback(boost::bind(preemptCB, &server, &planner_state)); server.start(); - ros::Subscriber sub_obstacles = n.subscribe("", 1, updateObstaclesCB); + ros::Subscriber sub_obstacles = n.subscribe(control::PlannerConfig::obstacle_state_topic, 1, updateObstaclesCB); ros::Subscriber sub_setpoint = n.subscribe("", 1, updateSetpointCB); @@ -96,7 +101,7 @@ int main(int argc, char** argv){ plan.refreshObstacles(obstacle_coordinates); - + // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed if(planner_state.make_plan && (!plan.isPlanSafe(setpoint_x,setpoint_y) || planner_state.new_goal)){ ROS_INFO("Make new plan."); planner_state.new_goal = false; @@ -104,6 +109,7 @@ int main(int argc, char** argv){ std::list points = plan.getSimplePlan(); geometry_msgs::Point32 point; int index = 0; + // Iterate through the points in the plan and publish to pathplanner-action for(std::list::iterator it = points.begin(); it != points.end(); it++){ point.x = it->getX(); point.y = it->getY(); From 5ded9762c878b03d5ab118a8d0e324549b392323 Mon Sep 17 00:00:00 2001 From: atussa Date: Tue, 13 Feb 2018 12:40:00 +0100 Subject: [PATCH 040/200] Start implementation of path planner config --- include/control/tools/planner_config.hpp | 8 +++---- src/tools/planner_config.cpp | 28 ++++++++++++------------ 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/include/control/tools/planner_config.hpp b/include/control/tools/planner_config.hpp index 6793bfd..db53add 100644 --- a/include/control/tools/planner_config.hpp +++ b/include/control/tools/planner_config.hpp @@ -12,8 +12,8 @@ namespace control { -class Config; -class Config { +class PlannerConfig; +class PlannerConfig { private: ///Node handler ros::NodeHandle nh_; @@ -21,9 +21,9 @@ class Config { ros::ServiceServer reload_config_service; static std::set missing_param_set_; ///Shared instance ptr - static std::unique_ptr shared_instance_p_; + static std::unique_ptr shared_instance_p_; ///Constructor - Config(); + PlannerConfig(); public: diff --git a/src/tools/planner_config.cpp b/src/tools/planner_config.cpp index 102fbd7..a5af9f5 100644 --- a/src/tools/planner_config.cpp +++ b/src/tools/planner_config.cpp @@ -1,26 +1,26 @@ -#include "control/tools/config.hpp" +#include "control/tools/planner_config.hpp" #include #include #include -using control::Config; +using control::PlannerConfig; using control::ROSNotInitializedException; -std::set Config::missing_param_set_; -std::unique_ptr Config::shared_instance_p_ = nullptr; +std::set PlannerConfig::missing_param_set_; +std::unique_ptr PlannerConfig::shared_instance_p_ = nullptr; -//Global config params +//Global PlannerConfig params -std::string Config::obstacle_state_topic = "/perception_obstacle_states"; +std::string PlannerConfig::obstacle_state_topic = "/perception_obstacle_states"; -void Config::loadParams() { +void PlannerConfig::loadParams() { if(!ros::isInitialized()) { throw ROSNotInitializedException(); } if(shared_instance_p_ == nullptr) { - shared_instance_p_ = std::unique_ptr(new Config); + shared_instance_p_ = std::unique_ptr(new PlannerConfig); } ros::NodeHandle n("~"); auto getDoubleParam = [&](const std::string& name, double& var, double min, double max) { @@ -88,20 +88,20 @@ void Config::loadParams() { using Request = ascend_msgs::ReloadConfig::Request; using Response = ascend_msgs::ReloadConfig::Response; -bool reloadConfigCB(Request&, Response& resp) { - control::Config::loadParams(); - control::handleInfoMsg("Config reloaded by service!"); +bool reloadPlannerConfigCB(Request&, Response& resp) { + control::PlannerConfig::loadParams(); + control::handleInfoMsg("PlannerConfig reloaded by service!"); //Missing param set should be empty! - for(auto& s : control::Config::getMissingParamSet()) { + for(auto& s : control::PlannerConfig::getMissingParamSet()) { resp.missing_params.emplace_back(s); } return true; } -control::Config::Config() { +control::PlannerConfig::PlannerConfig() { if(!ros::isInitialized()) { throw control::ROSNotInitializedException(); } - reload_config_service = nh_.advertiseService("/control_fsm_reload_config", reloadConfigCB); + reload_config_service = nh_.advertiseService("/control_fsm_reload_Config", reloadPlannerConfigCB); } From 8a9f4d3f213a2afe976ec768227847a82f603833 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Tue, 13 Feb 2018 18:28:23 +0100 Subject: [PATCH 041/200] Add note, pluss minor changes --- src/tools/obstacle_avoidance.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 284eaf8..ec2fe42 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -127,6 +127,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } else if (drone_distance_to_obstacle < minimum_distance) { ROS_WARN_COND(setpoint_modified, "[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); + // TODO: find out a better solution to this problem // need to avoid obstacle setpoint_modified = true; setpoint->position.x = obstacle.x + minimum_vector.x; From 609bf42ffc637be66b8372e36e8b15ecdef17d19 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Tue, 13 Feb 2018 23:23:27 +0100 Subject: [PATCH 042/200] Fix mind blowing bug --- src/tools/obstacle_avoidance.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index f1efd9e..e53b42c 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -48,20 +48,19 @@ inline float calcAngleToObstacle(const geometry_msgs::Point& point, const ascend return angle_to_obstacle; } -inline geometry_msgs::Vector3 rotateXY(geometry_msgs::Vector3 point, float angle){ +inline geometry_msgs::Vector3 rotateXY(const geometry_msgs::Vector3 point, const float angle){ // Apply 2d transformation matrix - point.x = point.x * std::cos(angle) - point.y * std::sin(angle); - point.y = point.x * std::sin(angle) + point.y * std::cos(angle); + geometry_msgs::Vector3 new_point; + new_point.x = point.x * std::cos(angle) - point.y * std::sin(angle); + new_point.y = point.x * std::sin(angle) + point.y * std::cos(angle); + new_point.z = point.z; - return point; + return new_point; } /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle -/// which is allowed, referenced from the obstacles coordinate system. y-axis is positive -/// direction of motion, and x-axis is the rightward direction. -/// As of jan. 2018, z is always 0.0 since only the xy-plane is concidered -/// TODO: The angle references are most likely wrong and may need a constant offset to be correct. -/// Fix once perception algorithm is implemented!!!! +/// which is allowed, referenced from the obstacles coordinate system. x-axis is positive +/// direction of motion. Obstacle driving in with angle=0 would be driving i x-direction inline geometry_msgs::Vector3 avoidZone(const float angle, const float front_clearance, const float back_clearance, const float side_clearance){ const bool drone_in_front_of_obstacle = angle <= PI; @@ -90,7 +89,6 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const geometry_msgs::PoseStamped drone_pose = control::DroneHandler::getCurrentPose(); for (const auto& obstacle : obstacles){ - const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle); const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle); From d694d9a32ccbd4163170053d70e62a722f613238 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Feb 2018 10:45:47 +0100 Subject: [PATCH 043/200] Fix bug in angle calculation. Change obstacle_state_handler to work with DetectedRobotsGlobalPositions.msg. Make obstacle avoidance algorithm more independent of format of obstacle msg. --- control_fsm_default_config.yaml | 2 +- .../control/tools/obstacle_state_handler.hpp | 9 +- src/tools/obstacle_avoidance.cpp | 128 ++++++++++++++---- src/tools/obstacle_state_handler.cpp | 16 +-- 4 files changed, 112 insertions(+), 43 deletions(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index d3fe7d6..066a56f 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -52,7 +52,7 @@ #Minimum flying altitude min_in_air_alt: 0.5, #Topic for recieving obstacle state data - obstacle_state_topic: perception/obstacles, + obstacle_state_topic: perception/obstacles/lidar, #Margin to determine if velocity is reached velocity_reached_margin: 0.2, #Obstacle avoidance clearance on sides diff --git a/include/control/tools/obstacle_state_handler.hpp b/include/control/tools/obstacle_state_handler.hpp index a912b3d..9015d4b 100644 --- a/include/control/tools/obstacle_state_handler.hpp +++ b/include/control/tools/obstacle_state_handler.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "control/tools/config.hpp" #include "control/tools/logger.hpp" #include "control_message.hpp" @@ -17,18 +18,18 @@ class ObstacleStateHandler { ///Subscriber to obstacle stream ros::Subscriber obs_sub_; ///Last recieved message - ascend_msgs::GRStateArray::ConstPtr last_msg_p_; + ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr last_msg_p_; ///Callback to run when new message is recieved - void onMsgRecievedCB(ascend_msgs::GRStateArray::ConstPtr msg_p); + void onMsgRecievedCB(ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr msg_p); ///Private constructor ObstacleStateHandler(); public: ///Get shared pointer to shared instance static const ObstacleStateHandler* getSharedObstacleHandlerPtr(); ///Get last obstacles from shared instance - static const std::vector& getCurrentObstacles(); + static const ascend_msgs::DetectedRobotsGlobalPositions& getCurrentObstacles(); ///Returns obstacles from instance - const std::vector& getObstacles() const; + const ascend_msgs::DetectedRobotsGlobalPositions& getObstacles() const; ///Is shared instance ready? static bool isInstanceReady(); ///is current instance ready diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index e53b42c..1c6843e 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -26,10 +27,11 @@ inline float angleWrapper(const float angle){ return angle - 2*PI*floor(angle/(2*PI)); } -inline float calcDistanceToObstacle(const geometry_msgs::Point& point, const ascend_msgs::GRState& obstacle){ +template +inline float calcDistanceToObstacle(const T& point, const K& obstacle_position){ geometry_msgs::Vector3 delta_to_obstacle; - delta_to_obstacle.x = point.x - obstacle.x; - delta_to_obstacle.y = point.y - obstacle.y; + delta_to_obstacle.x = point.x - obstacle_position.x; + delta_to_obstacle.y = point.y - obstacle_position.y; delta_to_obstacle.z = point.z; const float distance_to_obstacle = std::sqrt(std::pow(delta_to_obstacle.x, 2) + std::pow(delta_to_obstacle.y, 2)); @@ -37,20 +39,22 @@ inline float calcDistanceToObstacle(const geometry_msgs::Point& point, const asc return distance_to_obstacle; } -inline float calcAngleToObstacle(const geometry_msgs::Point& point, const ascend_msgs::GRState& obstacle){ +template +inline float calcAngleToObstacle(const T& point, const K& obstacle_position, const float obstacle_direction){ geometry_msgs::Vector3 delta_drone_obstacle; - delta_drone_obstacle.x = point.x - obstacle.x; - delta_drone_obstacle.y = point.y - obstacle.y; + delta_drone_obstacle.x = point.x - obstacle_position.x; + delta_drone_obstacle.y = point.y - obstacle_position.y; delta_drone_obstacle.z = point.z; - const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle.theta); + const float angle_to_obstacle = angleWrapper(-PI/2 + std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); return angle_to_obstacle; } -inline geometry_msgs::Vector3 rotateXY(const geometry_msgs::Vector3 point, const float angle){ +template +inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle){ // Apply 2d transformation matrix - geometry_msgs::Vector3 new_point; + T new_point; new_point.x = point.x * std::cos(angle) - point.y * std::sin(angle); new_point.y = point.x * std::sin(angle) + point.y * std::cos(angle); new_point.z = point.z; @@ -60,23 +64,26 @@ inline geometry_msgs::Vector3 rotateXY(const geometry_msgs::Vector3 point, const /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle /// which is allowed, referenced from the obstacles coordinate system. x-axis is positive -/// direction of motion. Obstacle driving in with angle=0 would be driving i x-direction -inline geometry_msgs::Vector3 avoidZone(const float angle, const float front_clearance, const float back_clearance, const float side_clearance){ - const bool drone_in_front_of_obstacle = angle <= PI; +/// direction of motion. Obstacle driving in with angle=0 would be driving in x-direction +inline geometry_msgs::Vector3 avoidZone(const float angle, + const float front_clearance, const float back_clearance, const float side_clearance){ + + const auto anglePositive = angleWrapper(angle); + const bool drone_in_front_of_obstacle = PI/2 <= anglePositive || anglePositive >= 3*PI/2; geometry_msgs::Vector3 minimum_vector; minimum_vector.z = 0.0f; if (drone_in_front_of_obstacle){ // Triangle shape - minimum_vector.x = side_clearance * std::cos(angle); - minimum_vector.y = front_clearance/side_clearance * (side_clearance - std::abs(minimum_vector.x)); + minimum_vector.y = side_clearance * std::sin(angle); + minimum_vector.x = front_clearance/side_clearance*(side_clearance - abs(minimum_vector.y)); } else { // Ellipse - minimum_vector.x = side_clearance * std::cos(angle); - minimum_vector.y = back_clearance * std::sin(angle); - } + minimum_vector.y = side_clearance*std::sin(angle); + minimum_vector.x = front_clearance*std::cos(angle); + } return minimum_vector; } @@ -84,13 +91,76 @@ inline geometry_msgs::Vector3 avoidZone(const float angle, const float front_cle bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { using control::Config; bool setpoint_modified{false}; - + const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); // Format of isn't finalized yet const geometry_msgs::PoseStamped drone_pose = control::DroneHandler::getCurrentPose(); + for (int i = 0; i < obstacles.count; i++){ + const auto obstacle_position = obstacles.global_robot_position[i]; + const float obstacle_direction = obstacles.direction[i]; + const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_position); + const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); + + if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ + // perform obstacle avoidance + const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_position, obstacle_direction); + ROS_INFO("Angle: %f", drone_angle_to_obstacle); + + const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_position, obstacle_direction); + // True if setpoint is within a 120 deg cone away from the obstacle + // TODO: turn this angle into a param once complete + const bool setpoint_reachable = + (drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && + setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); + + geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, + Config::obstacle_clearance_front, Config::obstacle_clearance_back, Config::obstacle_clearance_side); + + // Rotate to global coordinate system + minimum_vector = rotateXY(minimum_vector, -obstacle_direction); + + const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); + + ROS_INFO_THROTTLE(1, "Minimum distance: %.3f", minimum_distance); + + if (setpoint_reachable + && setpoint_distance_to_obstacle > drone_distance_to_obstacle + && setpoint_distance_to_obstacle > minimum_distance + && drone_distance_to_obstacle < minimum_distance){ + // no action, maybe logging? + ROS_INFO("Going to setpoint instead"); + } + else if (drone_distance_to_obstacle < minimum_distance) { + ROS_WARN_COND(setpoint_modified, "[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); + if (setpoint_modified){ + setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) + } + // TODO: find out a better solution to this problem + // need to avoid obstacle + setpoint_modified = true; + setpoint->position.x = obstacle_position.x + minimum_vector.x; + setpoint->position.y = obstacle_position.y + minimum_vector.y; + if (setpoint->position.z < Config::min_in_air_alt){ + setpoint->position.z = Config::min_in_air_alt; + } + } + } + + if (setpoint_modified){ + const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); + ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); + } + + } +/* for (const auto& obstacle : obstacles){ - const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle); - const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle); + geometry_msgs::Point32 obstacle_position; + obstacle_position.x = obstacle.x; + obstacle_position.y = obstacle.y; + obstacle_position.z = 0.f; + + const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_position); + const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance @@ -122,20 +192,26 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } else if (drone_distance_to_obstacle < minimum_distance) { ROS_WARN_COND(setpoint_modified, "[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); + if (setpoint_modified){ + setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) + } // TODO: find out a better solution to this problem // need to avoid obstacle setpoint_modified = true; setpoint->position.x = obstacle.x + minimum_vector.x; setpoint->position.y = obstacle.y + minimum_vector.y; + if (setpoint->position.z < Config::min_air_alt){ + setpoint->position.z = Config::min_air_alt; + } } } if (setpoint_modified){ - const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle); + const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); } } - +*/ return setpoint_modified; } @@ -160,13 +236,5 @@ void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr ObstacleStateHandler::shared_instance_p_ = nullptr; -ObstacleStateHandler::ObstacleStateHandler() : last_msg_p_(new ascend_msgs::GRStateArray) { +ObstacleStateHandler::ObstacleStateHandler() : last_msg_p_(new ascend_msgs::DetectedRobotsGlobalPositions) { using control::Config; - std::string& topic = Config::obstacle_state_topic; + std::string& topic = Config::lidar_topic; obs_sub_ = n_.subscribe(topic.c_str(), 1, &ObstacleStateHandler::onMsgRecievedCB, this); } -void ObstacleStateHandler::onMsgRecievedCB(ascend_msgs::GRStateArray::ConstPtr msg_p) { +void ObstacleStateHandler::onMsgRecievedCB(ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr msg_p) { last_msg_p_ = msg_p; } @@ -26,13 +26,13 @@ const ObstacleStateHandler* ObstacleStateHandler::getSharedObstacleHandlerPtr() return shared_instance_p_.get(); } -const std::vector& ObstacleStateHandler::getCurrentObstacles() { +const ascend_msgs::DetectedRobotsGlobalPositions& ObstacleStateHandler::getCurrentObstacles() { return getSharedObstacleHandlerPtr()->getObstacles(); } -const std::vector& ObstacleStateHandler::getObstacles() const { +const ascend_msgs::DetectedRobotsGlobalPositions& ObstacleStateHandler::getObstacles() const { isReady(); - return last_msg_p_->states; + return *last_msg_p_; } bool ObstacleStateHandler::isInstanceReady() { @@ -40,10 +40,10 @@ bool ObstacleStateHandler::isInstanceReady() { } bool ObstacleStateHandler::isReady() const { - if(last_msg_p_->states.empty()) { + if(last_msg_p_->count == 0) { control::handleWarnMsg("Obstacle handler: No data available"); return false; - } else if(control::message::hasTimedOut(last_msg_p_->states[0])) { + } else if(control::message::hasTimedOut(*last_msg_p_)) { control::handleWarnMsg("Obstacle handler: Using old data"); return false; } From eff0ac05836aabfd977c318b1bf3a724f7164779 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Feb 2018 14:22:32 +0100 Subject: [PATCH 044/200] Fix bugs --- src/tools/obstacle_avoidance.cpp | 80 +++++--------------------------- 1 file changed, 12 insertions(+), 68 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 1c6843e..21e9e2e 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -68,21 +68,23 @@ inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle){ inline geometry_msgs::Vector3 avoidZone(const float angle, const float front_clearance, const float back_clearance, const float side_clearance){ - const auto anglePositive = angleWrapper(angle); - const bool drone_in_front_of_obstacle = PI/2 <= anglePositive || anglePositive >= 3*PI/2; + const auto angle_positive = angleWrapper(angle); + const bool drone_in_front_of_obstacle = angle_positive <= PI/2 || angle_positive >= 3*PI/2; geometry_msgs::Vector3 minimum_vector; minimum_vector.z = 0.0f; if (drone_in_front_of_obstacle){ + //ROS_INFO_THROTTLE(1, "Drone in front of obstacle: %f", angle_positive); // Triangle shape minimum_vector.y = side_clearance * std::sin(angle); - minimum_vector.x = front_clearance/side_clearance*(side_clearance - abs(minimum_vector.y)); + minimum_vector.x = (front_clearance/side_clearance)*(side_clearance - abs(minimum_vector.y)); } else { // Ellipse + //ROS_INFO_THROTTLE(1, "Drone behind obstacle: %f", angle_positive); minimum_vector.y = side_clearance*std::sin(angle); - minimum_vector.x = front_clearance*std::cos(angle); + minimum_vector.x = back_clearance*std::cos(angle); } return minimum_vector; @@ -104,7 +106,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_position, obstacle_direction); - ROS_INFO("Angle: %f", drone_angle_to_obstacle); + //ROS_INFO("Angle: %f", drone_angle_to_obstacle); const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_position, obstacle_direction); // True if setpoint is within a 120 deg cone away from the obstacle @@ -116,24 +118,25 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, Config::obstacle_clearance_front, Config::obstacle_clearance_back, Config::obstacle_clearance_side); + ROS_INFO_THROTTLE(1, "Local minimum vector: %.3f\t%.3f", minimum_vector.x, minimum_vector.y); + // Rotate to global coordinate system minimum_vector = rotateXY(minimum_vector, -obstacle_direction); const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); - ROS_INFO_THROTTLE(1, "Minimum distance: %.3f", minimum_distance); if (setpoint_reachable && setpoint_distance_to_obstacle > drone_distance_to_obstacle && setpoint_distance_to_obstacle > minimum_distance && drone_distance_to_obstacle < minimum_distance){ // no action, maybe logging? - ROS_INFO("Going to setpoint instead"); + ROS_INFO_THROTTLE(1, "Going to setpoint instead"); } else if (drone_distance_to_obstacle < minimum_distance) { - ROS_WARN_COND(setpoint_modified, "[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); if (setpoint_modified){ - setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) + ROS_WARN("[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); + setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) } // TODO: find out a better solution to this problem // need to avoid obstacle @@ -152,66 +155,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } } -/* - for (const auto& obstacle : obstacles){ - geometry_msgs::Point32 obstacle_position; - obstacle_position.x = obstacle.x; - obstacle_position.y = obstacle.y; - obstacle_position.z = 0.f; - - const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_position); - const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); - - if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ - // perform obstacle avoidance - const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle); - - const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle); - // True if setpoint is within a 120 deg cone away from the obstacle - // TODO: turn this angle into a param once complete - const bool setpoint_reachable = - (drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && - setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); - - geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, - Config::obstacle_clearance_front, Config::obstacle_clearance_back, Config::obstacle_clearance_side); - - // Rotate to global coordinate system - minimum_vector = rotateXY(minimum_vector, -obstacle.theta); - - const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); - - ROS_INFO_THROTTLE(1, "Minimum distance: %.3f", minimum_distance); - if (setpoint_reachable - && setpoint_distance_to_obstacle > drone_distance_to_obstacle - && setpoint_distance_to_obstacle > minimum_distance - && drone_distance_to_obstacle < minimum_distance){ - // no action, maybe logging? - ROS_INFO("Going to setpoint instead"); - } - else if (drone_distance_to_obstacle < minimum_distance) { - ROS_WARN_COND(setpoint_modified, "[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); - if (setpoint_modified){ - setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) - } - // TODO: find out a better solution to this problem - // need to avoid obstacle - setpoint_modified = true; - setpoint->position.x = obstacle.x + minimum_vector.x; - setpoint->position.y = obstacle.y + minimum_vector.y; - if (setpoint->position.z < Config::min_air_alt){ - setpoint->position.z = Config::min_air_alt; - } - } - } - - if (setpoint_modified){ - const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); - ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); - } - } -*/ return setpoint_modified; } From 3c783903b000375823c087a32b92b386b122b2bf Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Feb 2018 14:35:02 +0100 Subject: [PATCH 045/200] Undo commit --- include/control/tools/obstacle_state_handler.hpp | 9 ++++----- src/tools/obstacle_state_handler.cpp | 16 ++++++++-------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/include/control/tools/obstacle_state_handler.hpp b/include/control/tools/obstacle_state_handler.hpp index 9015d4b..a912b3d 100644 --- a/include/control/tools/obstacle_state_handler.hpp +++ b/include/control/tools/obstacle_state_handler.hpp @@ -3,7 +3,6 @@ #include #include #include -#include #include "control/tools/config.hpp" #include "control/tools/logger.hpp" #include "control_message.hpp" @@ -18,18 +17,18 @@ class ObstacleStateHandler { ///Subscriber to obstacle stream ros::Subscriber obs_sub_; ///Last recieved message - ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr last_msg_p_; + ascend_msgs::GRStateArray::ConstPtr last_msg_p_; ///Callback to run when new message is recieved - void onMsgRecievedCB(ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr msg_p); + void onMsgRecievedCB(ascend_msgs::GRStateArray::ConstPtr msg_p); ///Private constructor ObstacleStateHandler(); public: ///Get shared pointer to shared instance static const ObstacleStateHandler* getSharedObstacleHandlerPtr(); ///Get last obstacles from shared instance - static const ascend_msgs::DetectedRobotsGlobalPositions& getCurrentObstacles(); + static const std::vector& getCurrentObstacles(); ///Returns obstacles from instance - const ascend_msgs::DetectedRobotsGlobalPositions& getObstacles() const; + const std::vector& getObstacles() const; ///Is shared instance ready? static bool isInstanceReady(); ///is current instance ready diff --git a/src/tools/obstacle_state_handler.cpp b/src/tools/obstacle_state_handler.cpp index 92bdd0e..ff6c879 100644 --- a/src/tools/obstacle_state_handler.cpp +++ b/src/tools/obstacle_state_handler.cpp @@ -5,13 +5,13 @@ using control::ObstacleStateHandler; std::unique_ptr ObstacleStateHandler::shared_instance_p_ = nullptr; -ObstacleStateHandler::ObstacleStateHandler() : last_msg_p_(new ascend_msgs::DetectedRobotsGlobalPositions) { +ObstacleStateHandler::ObstacleStateHandler() : last_msg_p_(new ascend_msgs::GRStateArray) { using control::Config; - std::string& topic = Config::lidar_topic; + std::string& topic = Config::obstacle_state_topic; obs_sub_ = n_.subscribe(topic.c_str(), 1, &ObstacleStateHandler::onMsgRecievedCB, this); } -void ObstacleStateHandler::onMsgRecievedCB(ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr msg_p) { +void ObstacleStateHandler::onMsgRecievedCB(ascend_msgs::GRStateArray::ConstPtr msg_p) { last_msg_p_ = msg_p; } @@ -26,13 +26,13 @@ const ObstacleStateHandler* ObstacleStateHandler::getSharedObstacleHandlerPtr() return shared_instance_p_.get(); } -const ascend_msgs::DetectedRobotsGlobalPositions& ObstacleStateHandler::getCurrentObstacles() { +const std::vector& ObstacleStateHandler::getCurrentObstacles() { return getSharedObstacleHandlerPtr()->getObstacles(); } -const ascend_msgs::DetectedRobotsGlobalPositions& ObstacleStateHandler::getObstacles() const { +const std::vector& ObstacleStateHandler::getObstacles() const { isReady(); - return *last_msg_p_; + return last_msg_p_->states; } bool ObstacleStateHandler::isInstanceReady() { @@ -40,10 +40,10 @@ bool ObstacleStateHandler::isInstanceReady() { } bool ObstacleStateHandler::isReady() const { - if(last_msg_p_->count == 0) { + if(last_msg_p_->states.empty()) { control::handleWarnMsg("Obstacle handler: No data available"); return false; - } else if(control::message::hasTimedOut(*last_msg_p_)) { + } else if(control::message::hasTimedOut(last_msg_p_->states[0])) { control::handleWarnMsg("Obstacle handler: Using old data"); return false; } From 4ada387b2ceebba28628494aab96ac96e0609a5e Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Feb 2018 14:47:21 +0100 Subject: [PATCH 046/200] Really undo commit --- include/control/tools/obstacle_state_handler.hpp | 9 +++++---- src/tools/obstacle_state_handler.cpp | 16 ++++++++-------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/include/control/tools/obstacle_state_handler.hpp b/include/control/tools/obstacle_state_handler.hpp index a912b3d..9015d4b 100644 --- a/include/control/tools/obstacle_state_handler.hpp +++ b/include/control/tools/obstacle_state_handler.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "control/tools/config.hpp" #include "control/tools/logger.hpp" #include "control_message.hpp" @@ -17,18 +18,18 @@ class ObstacleStateHandler { ///Subscriber to obstacle stream ros::Subscriber obs_sub_; ///Last recieved message - ascend_msgs::GRStateArray::ConstPtr last_msg_p_; + ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr last_msg_p_; ///Callback to run when new message is recieved - void onMsgRecievedCB(ascend_msgs::GRStateArray::ConstPtr msg_p); + void onMsgRecievedCB(ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr msg_p); ///Private constructor ObstacleStateHandler(); public: ///Get shared pointer to shared instance static const ObstacleStateHandler* getSharedObstacleHandlerPtr(); ///Get last obstacles from shared instance - static const std::vector& getCurrentObstacles(); + static const ascend_msgs::DetectedRobotsGlobalPositions& getCurrentObstacles(); ///Returns obstacles from instance - const std::vector& getObstacles() const; + const ascend_msgs::DetectedRobotsGlobalPositions& getObstacles() const; ///Is shared instance ready? static bool isInstanceReady(); ///is current instance ready diff --git a/src/tools/obstacle_state_handler.cpp b/src/tools/obstacle_state_handler.cpp index ff6c879..92bdd0e 100644 --- a/src/tools/obstacle_state_handler.cpp +++ b/src/tools/obstacle_state_handler.cpp @@ -5,13 +5,13 @@ using control::ObstacleStateHandler; std::unique_ptr ObstacleStateHandler::shared_instance_p_ = nullptr; -ObstacleStateHandler::ObstacleStateHandler() : last_msg_p_(new ascend_msgs::GRStateArray) { +ObstacleStateHandler::ObstacleStateHandler() : last_msg_p_(new ascend_msgs::DetectedRobotsGlobalPositions) { using control::Config; - std::string& topic = Config::obstacle_state_topic; + std::string& topic = Config::lidar_topic; obs_sub_ = n_.subscribe(topic.c_str(), 1, &ObstacleStateHandler::onMsgRecievedCB, this); } -void ObstacleStateHandler::onMsgRecievedCB(ascend_msgs::GRStateArray::ConstPtr msg_p) { +void ObstacleStateHandler::onMsgRecievedCB(ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr msg_p) { last_msg_p_ = msg_p; } @@ -26,13 +26,13 @@ const ObstacleStateHandler* ObstacleStateHandler::getSharedObstacleHandlerPtr() return shared_instance_p_.get(); } -const std::vector& ObstacleStateHandler::getCurrentObstacles() { +const ascend_msgs::DetectedRobotsGlobalPositions& ObstacleStateHandler::getCurrentObstacles() { return getSharedObstacleHandlerPtr()->getObstacles(); } -const std::vector& ObstacleStateHandler::getObstacles() const { +const ascend_msgs::DetectedRobotsGlobalPositions& ObstacleStateHandler::getObstacles() const { isReady(); - return last_msg_p_->states; + return *last_msg_p_; } bool ObstacleStateHandler::isInstanceReady() { @@ -40,10 +40,10 @@ bool ObstacleStateHandler::isInstanceReady() { } bool ObstacleStateHandler::isReady() const { - if(last_msg_p_->states.empty()) { + if(last_msg_p_->count == 0) { control::handleWarnMsg("Obstacle handler: No data available"); return false; - } else if(control::message::hasTimedOut(last_msg_p_->states[0])) { + } else if(control::message::hasTimedOut(*last_msg_p_)) { control::handleWarnMsg("Obstacle handler: Using old data"); return false; } From 5ec30a9e2b46df218ae2f9feed6a857643b3e4ea Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 15 Feb 2018 16:15:39 +0100 Subject: [PATCH 047/200] Add launch file. --- launch/path_planner.launch | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 launch/path_planner.launch diff --git a/launch/path_planner.launch b/launch/path_planner.launch new file mode 100644 index 0000000..fb5114e --- /dev/null +++ b/launch/path_planner.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + From 64f6b4984243b1867bc8ffdd0033c820445f27d5 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 15 Feb 2018 16:16:58 +0100 Subject: [PATCH 048/200] Add publisher for plan. --- CMakeLists.txt | 2 +- include/control/tools/planner_config.hpp | 3 ++- src/nodes/path_planner_node.cpp | 26 ++++++++++++++------ src/nodes/test_path_planner_node.cpp | 12 +++++++++ src/{nodes => path_planner}/node.cpp | 0 src/{nodes => path_planner}/path_planner.cpp | 0 src/tools/planner_config.cpp | 5 +++- 7 files changed, 37 insertions(+), 11 deletions(-) create mode 100644 src/nodes/test_path_planner_node.cpp rename src/{nodes => path_planner}/node.cpp (100%) rename src/{nodes => path_planner}/path_planner.cpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6dd81e9..9fcc7b8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ target_link_libraries(control_fsm_lib add_executable(control_fsm_main src/nodes/control_fsm_main) add_executable(action_interface_client src/nodes/action_interface_client.cpp) -add_executable(path_planner_node src/nodes/path_planner_node src/nodes/path_planner src/nodes/node) +add_executable(path_planner_node src/nodes/path_planner_node src/path_planner/path_planner src/path_planner/node) target_compile_features(path_planner_node PRIVATE cxx_range_for) ## Specify libraries to link a library or executable target against diff --git a/include/control/tools/planner_config.hpp b/include/control/tools/planner_config.hpp index db53add..2138e5a 100644 --- a/include/control/tools/planner_config.hpp +++ b/include/control/tools/planner_config.hpp @@ -29,7 +29,8 @@ class PlannerConfig { public: /// \fsmparam Topic for recieving obstacle positions static std::string obstacle_state_topic; - + static std::string mavros_setpoint_topic; + static std::string plan_points_topic; /**Load paramaters * @throw control::ROSNotInitializedException */ diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 90b50c5..a0071c9 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -8,6 +8,7 @@ #include "ascend_msgs/GRStateArray.h" #include #include "control/tools/planner_config.hpp" +#include "ascend_msgs/PointArray.h" using control::pathplanner::PathPlanner; @@ -92,7 +93,9 @@ int main(int argc, char** argv){ server.start(); ros::Subscriber sub_obstacles = n.subscribe(control::PlannerConfig::obstacle_state_topic, 1, updateObstaclesCB); - ros::Subscriber sub_setpoint = n.subscribe("", 1, updateSetpointCB); + ros::Subscriber sub_setpoint = n.subscribe(control::PlannerConfig::mavros_setpoint_topic, 1, updateSetpointCB); + + ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); while(ros::ok()){ @@ -107,18 +110,25 @@ int main(int argc, char** argv){ planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); std::list points = plan.getSimplePlan(); - geometry_msgs::Point32 point; - int index = 0; + geometry_msgs::Point32 point_32; + geometry_msgs::Point point; // Iterate through the points in the plan and publish to pathplanner-action + ascend_msgs::PathPlannerFeedback feedback; + // For saving memory + feedback.points_in_plan.reserve(20); + ascend_msgs::PointArray plan_to_send; + plan_to_send.points.reserve(20); for(std::list::iterator it = points.begin(); it != points.end(); it++){ + point_32.x = it->getX(); + point_32.y = it->getY(); point.x = it->getX(); point.y = it->getY(); - - ascend_msgs::PathPlannerFeedback feedback; - feedback.points_in_plan[index++] = point; - - server.publishFeedback(feedback); + + feedback.points_in_plan.push_back(point_32); + plan_to_send.points.push_back(point); } + server.publishFeedback(feedback); + pub_plan.publish(plan_to_send); } else{ rate.sleep(); diff --git a/src/nodes/test_path_planner_node.cpp b/src/nodes/test_path_planner_node.cpp new file mode 100644 index 0000000..a6390b4 --- /dev/null +++ b/src/nodes/test_path_planner_node.cpp @@ -0,0 +1,12 @@ +#include + + + +int main(int argc, char** argv){ + + ros::init(argc, argv, "path_planner_test"); + ros::NodeHandle n; + ros::Rate rate(30.0); + + ros::Publisher pub_goal = n.advertise<>(); +} \ No newline at end of file diff --git a/src/nodes/node.cpp b/src/path_planner/node.cpp similarity index 100% rename from src/nodes/node.cpp rename to src/path_planner/node.cpp diff --git a/src/nodes/path_planner.cpp b/src/path_planner/path_planner.cpp similarity index 100% rename from src/nodes/path_planner.cpp rename to src/path_planner/path_planner.cpp diff --git a/src/tools/planner_config.cpp b/src/tools/planner_config.cpp index a5af9f5..f2641ac 100644 --- a/src/tools/planner_config.cpp +++ b/src/tools/planner_config.cpp @@ -13,7 +13,8 @@ std::unique_ptr PlannerConfig::shared_instance_p_ = nullptr; //Global PlannerConfig params std::string PlannerConfig::obstacle_state_topic = "/perception_obstacle_states"; - +std::string PlannerConfig::mavros_setpoint_topic = "/mavros/setpoint_raw/local"; +std::string PlannerConfig::plan_points_topic = "/control/planner/plan"; void PlannerConfig::loadParams() { if(!ros::isInitialized()) { @@ -83,6 +84,8 @@ void PlannerConfig::loadParams() { //Obstacles getStringParam("obstacle_state_topic", obstacle_state_topic); + getStringParam("mavros_setpoint_topic", mavros_setpoint_topic); + getStringParam("plan_points_topic", plan_points_topic); } From 51e4b110b2a10eb3622227c255e514973884bbaf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 15 Feb 2018 20:13:47 +0100 Subject: [PATCH 049/200] Update node.hpp New line --- include/control/planner/node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp index 86e5d1b..9a1734d 100644 --- a/include/control/planner/node.hpp +++ b/include/control/planner/node.hpp @@ -38,4 +38,4 @@ class Node{ friend bool operator< (const Node &lhs, const Node &rhs); }; -#endif // NODE_HPP \ No newline at end of file +#endif // NODE_HPP From 859f34145891b8258086d20e41e9a40c3efa0472 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 15 Feb 2018 20:54:13 +0100 Subject: [PATCH 050/200] Update path_planner.cpp Add new line --- src/path_planner/path_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index af4fe66..7e5b9ca 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -386,4 +386,4 @@ void PathPlanner::resetParameters() { plan.clear(); simple_plan.clear(); destination_reached = false; -} \ No newline at end of file +} From 4c418cadcd8e925d436262c974faf136e3ee1ccf Mon Sep 17 00:00:00 2001 From: atussa Date: Fri, 16 Feb 2018 16:28:27 +0100 Subject: [PATCH 051/200] Change destination margin and add exact destination to plan --- include/control/planner/path_planner.hpp | 5 +++++ src/path_planner/path_planner.cpp | 21 +++++++++++++++------ 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index ea4e7eb..ebbe9c2 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -44,6 +44,11 @@ class PathPlanner{ bool noPlan = true; bool destination_reached = false; + // Because of the NODE_DISTANCE, the destination found and the one requested + // might not be exactly equal + float destination_found_x; + float destination_found_y; + public: PathPlanner(); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index af4fe66..6ab00d4 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -129,8 +129,10 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent graph[coordToIndex(x)][coordToIndex(y)].setParentX(parent_x); graph[coordToIndex(x)][coordToIndex(y)].setParentY(parent_y); destination_reached = true; - std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; - std::cout << "Destination parent: x=" << graph[coordToIndex(x)][coordToIndex(y)].getParentX() << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; + destination_found_x = x; + destination_found_y = y; + //std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; + //std::cout << "Destination parent: x=" << graph[coordToIndex(x)][coordToIndex(y)].getParentX() << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; } // If node is not destination and hasn't been searched yet else if(closed_list[coordToIndex(x)][coordToIndex(y)] == false){ @@ -152,7 +154,6 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent } void PathPlanner::handleAllSuccessors(float x, float y) { - // LOVER Å LØSE DETTE PÅ EN BEDRE MÅTE handleSuccessor(x+NODE_DISTANCE,y, x, y, NODE_DISTANCE); if(destination_reached){return;} handleSuccessor(x+NODE_DISTANCE, y+NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); @@ -171,7 +172,7 @@ void PathPlanner::handleAllSuccessors(float x, float y) { } bool PathPlanner::isDestination(float x, float y) { - float margin = NODE_DISTANCE/2; + float margin = NODE_DISTANCE/2+0.001; return ((x < end_node.getX()+margin && x>end_node.getX()-margin) && (y < end_node.getY()+margin && y > end_node.getY()-margin)); } @@ -244,6 +245,10 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // Go through the graph from end to start through the parents of each node // Add nodes to the plan + plan.push_front(graph[coordToIndex(x)][coordToIndex(y)]); + x = destination_found_x; + y = destination_found_y; + while(!isStart(x, y)){ plan.push_front(graph[coordToIndex(x)][coordToIndex(y)]); float x_temp = graph[coordToIndex(x)][coordToIndex(y)].getParentX(); @@ -343,6 +348,7 @@ void PathPlanner::simplifyPlan() { bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { if(simple_plan.empty()){ + std::cout << "Simple plan empty" << std::endl; return false; } @@ -351,16 +357,18 @@ bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { current++; while(current != simple_plan.end()){ + // Find next point in plan by checking distance between setpoint and points in plan if(abs(current->getX()-setpoint_x)<0.1 && abs(current->getY()-setpoint_y) < 0.1){ - std::cout << "Setpoint: " << current->getX() << ", " << current->getY() << std::endl; - std::cout << "Search from:" << prev->getX() << ", " << current->getY() << std::endl; + //std::cout << "Setpoint: " << current->getX() << ", " << current->getY() << std::endl; + //std::cout << "Search from:" << prev->getX() << ", " << prev->getY() << std::endl; break; } else{ prev = current; current++; if(current == simple_plan.end()){ + // iterated through list without finding match in plan for the setpoint return false; } } @@ -376,6 +384,7 @@ bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { current++; next++; } + return true; } From f15a7301adcb14e946101f280120c5794f624df3 Mon Sep 17 00:00:00 2001 From: atussa Date: Fri, 16 Feb 2018 16:29:47 +0100 Subject: [PATCH 052/200] Make service for goal point --- CMakeLists.txt | 7 ++- src/nodes/path_planner_node.cpp | 74 ++++++++++++++-------------- src/nodes/test_path_planner_node.cpp | 36 +++++++++++++- 3 files changed, 77 insertions(+), 40 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9fcc7b8..2068d4e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,7 +67,8 @@ add_executable(control_fsm_main src/nodes/control_fsm_main) add_executable(action_interface_client src/nodes/action_interface_client.cpp) add_executable(path_planner_node src/nodes/path_planner_node src/path_planner/path_planner src/path_planner/node) -target_compile_features(path_planner_node PRIVATE cxx_range_for) + +add_executable(test_path_planner_node src/nodes/test_path_planner_node) ## Specify libraries to link a library or executable target against @@ -78,6 +79,10 @@ target_link_libraries(path_planner_node ${catkin_LIBRARIES} control_tools_lib) +target_link_libraries(test_path_planner_node + ${catkin_LIBRARIES} + control_tools_lib) + ## Add cmake target dependencies of the executable ## same as for the library above add_dependencies(control_fsm_main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index a0071c9..b6d6b43 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -9,6 +9,7 @@ #include #include "control/tools/planner_config.hpp" #include "ascend_msgs/PointArray.h" +#include "ascend_msgs/PathPlanner.h" using control::pathplanner::PathPlanner; @@ -28,14 +29,14 @@ struct PlannerState{ }; -//Terminates current goal when requested. -void preemptCB(ActionServerType* server, PlannerState* planner_state) { - planner_state->make_plan = false; - //Abort whatever you are doing first! - ROS_WARN("Preempted!"); -} -void newPlanCB(ActionServerType* server, PlannerState* planner_state){ +using Request = ascend_msgs::PathPlanner::Request; +using Response = ascend_msgs::PathPlanner::Response; + +bool newPlanCB(Request &req, Response &res, PlannerState* planner_state){ + + ROS_INFO("newPlanCB"); + planner_state->make_plan = true; // Update planner state @@ -44,20 +45,13 @@ void newPlanCB(ActionServerType* server, PlannerState* planner_state){ planner_state->current_x = position.x; planner_state->current_y = position.y; - if(!server->isNewGoalAvailable()){ - return; - } - //Accept the new goal planner_state->new_goal = true; - auto goal = server->acceptNewGoal(); - planner_state->goal_x = goal->x; - planner_state->goal_y = goal->y; - if(server->isPreemptRequested()) { - //Goal is already stopped by client - return; - } + planner_state->goal_x = req.goal_x; + planner_state->goal_y = req.goal_y; + + return true; } @@ -77,6 +71,8 @@ void updateSetpointCB(mavros_msgs::PositionTarget::ConstPtr msg_p){ int main(int argc, char** argv){ + ROS_INFO("Path planner node started"); + PlannerState planner_state; PathPlanner plan; @@ -86,49 +82,53 @@ int main(int argc, char** argv){ control::PlannerConfig::loadParams(); - ActionServerType server(n, "plan_path", false); - - server.registerGoalCallback(boost::bind(newPlanCB, &server, &planner_state)); - server.registerPreemptCallback(boost::bind(preemptCB, &server, &planner_state)); - server.start(); - ros::Subscriber sub_obstacles = n.subscribe(control::PlannerConfig::obstacle_state_topic, 1, updateObstaclesCB); + //ros::Subscriber sub_obstacles = n.subscribe(control::PlannerConfig::obstacle_state_topic, 1, updateObstaclesCB); ros::Subscriber sub_setpoint = n.subscribe(control::PlannerConfig::mavros_setpoint_topic, 1, updateSetpointCB); ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); + ros::ServiceServer server = n.advertiseService("path_planner_service", boost::bind(&newPlanCB, _1, _2, &planner_state)); + while(ros::ok()){ ros::spinOnce(); - plan.refreshObstacles(obstacle_coordinates); + //plan.refreshObstacles(obstacle_coordinates); // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed - if(planner_state.make_plan && (!plan.isPlanSafe(setpoint_x,setpoint_y) || planner_state.new_goal)){ - ROS_INFO("Make new plan."); + if(planner_state.make_plan && (!plan.isPlanSafe(/*setpoint_x,setpoint_y*/19.9,19.9) || planner_state.new_goal)){ + //ROS_INFO("Make new plan."); planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); std::list points = plan.getSimplePlan(); - geometry_msgs::Point32 point_32; + geometry_msgs::Point point; // Iterate through the points in the plan and publish to pathplanner-action ascend_msgs::PathPlannerFeedback feedback; // For saving memory feedback.points_in_plan.reserve(20); - ascend_msgs::PointArray plan_to_send; - plan_to_send.points.reserve(20); - for(std::list::iterator it = points.begin(); it != points.end(); it++){ - point_32.x = it->getX(); - point_32.y = it->getY(); + ascend_msgs::PointArray points_in_plan; + points_in_plan.points.reserve(20); + + // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan + std::list::iterator second_point = points.begin(); + second_point++; + + std::cout << "Published points:\t"; + for(std::list::iterator it = second_point; it != points.end(); it++){ + point.x = it->getX(); point.y = it->getY(); - feedback.points_in_plan.push_back(point_32); - plan_to_send.points.push_back(point); + points_in_plan.points.push_back(point); + + std::cout << point.x << ", " << point.y << "\t"; + } - server.publishFeedback(feedback); - pub_plan.publish(plan_to_send); + pub_plan.publish(points_in_plan); + std::cout << std::endl; } else{ rate.sleep(); diff --git a/src/nodes/test_path_planner_node.cpp b/src/nodes/test_path_planner_node.cpp index a6390b4..a149af7 100644 --- a/src/nodes/test_path_planner_node.cpp +++ b/src/nodes/test_path_planner_node.cpp @@ -1,12 +1,44 @@ #include - +#include "ascend_msgs/PathPlanner.h" int main(int argc, char** argv){ + ROS_INFO("Test path planner node started"); + ros::init(argc, argv, "path_planner_test"); ros::NodeHandle n; ros::Rate rate(30.0); - ros::Publisher pub_goal = n.advertise<>(); + ros::ServiceClient client = n.serviceClient("path_planner_service"); + + ascend_msgs::PathPlanner clientCall; + + clientCall.request.goal_x = 19.9; + clientCall.request.goal_y = 19.9; + + if(client.call(clientCall)){ + ROS_INFO("Request handled"); + } + else{ + ROS_WARN("Error!"); + } + + /*while(ros::ok()){ + ROS_INFO("Ros is ok"); + ros::spin(); + } + + ros::Duration(3).sleep(); + + clientCall.request.goal_x = 1; + clientCall.request.goal_y = 8; + + if(client.call(clientCall)){ + ROS_INFO("Request handled"); + } + else{ + ROS_WARN("Error!"); + }*/ + } \ No newline at end of file From 6f0be7b972f6431aa9a1d884be1ad8ec23cc774b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Fri, 16 Feb 2018 22:21:41 +0100 Subject: [PATCH 053/200] Update test_path_planner_node.cpp --- src/nodes/test_path_planner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodes/test_path_planner_node.cpp b/src/nodes/test_path_planner_node.cpp index a149af7..6807bcf 100644 --- a/src/nodes/test_path_planner_node.cpp +++ b/src/nodes/test_path_planner_node.cpp @@ -41,4 +41,4 @@ int main(int argc, char** argv){ ROS_WARN("Error!"); }*/ -} \ No newline at end of file +} From bfae13cc7cc64369b6dafc22e7acfda6c04fbd37 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Fri, 16 Feb 2018 22:21:53 +0100 Subject: [PATCH 054/200] Update node.cpp --- src/path_planner/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/path_planner/node.cpp b/src/path_planner/node.cpp index cc4a690..b536f9a 100644 --- a/src/path_planner/node.cpp +++ b/src/path_planner/node.cpp @@ -8,4 +8,4 @@ bool operator< (const Node &lhs, const Node &rhs){ void Node::updateF(float new_g){ g = new_g; f = g+h; -} \ No newline at end of file +} From c2dd60cff1d85fec6d38204ac99dcbcc17c26f10 Mon Sep 17 00:00:00 2001 From: atussa Date: Sat, 17 Feb 2018 09:34:57 +0100 Subject: [PATCH 055/200] Minor improvements --- include/control/planner/path_planner.hpp | 2 +- src/path_planner/path_planner.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index ebbe9c2..b425186 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -41,7 +41,7 @@ class PathPlanner{ Node end_node; Node start_node; - bool noPlan = true; + bool no_plan = true; bool destination_reached = false; // Because of the NODE_DISTANCE, the destination found and the one requested diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index dc00501..b486b0b 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -4,7 +4,7 @@ using namespace control::pathplanner; int coordToIndex(float k) { - int index = static_cast(k/NODE_DISTANCE+0.5); + int index = round(k/NODE_DISTANCE+0.5); return (index); } @@ -63,7 +63,7 @@ void PathPlanner::addObstacle(float center_x, float center_y) { void PathPlanner::refreshObstacles(std::list obstacle_coordinates) { this->obstacle_coordinates = obstacle_coordinates; - if(noPlan){ + if(no_plan){ return; } std::list::iterator x = obstacle_coordinates.begin(); @@ -206,7 +206,7 @@ bool PathPlanner::isSafeLine(float x1, float y1, float x2, float y2) { void PathPlanner::makePlan(float current_x, float current_y, float target_x, float target_y) { - noPlan = false; + no_plan = false; // If start or end point is not valid, a plan is not created if(!isValidCoordinate(current_x,current_y)){ From cf0ff93e07f3e92149f021b0a575ce30df958ed2 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 18 Feb 2018 19:03:57 +0100 Subject: [PATCH 056/200] Simplify code --- src/nodes/path_planner_node.cpp | 2 +- src/nodes/test_path_planner_node.cpp | 4 +- src/path_planner/path_planner.cpp | 78 ++++++++++++++-------------- 3 files changed, 41 insertions(+), 43 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index b6d6b43..6e7931f 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -98,7 +98,7 @@ int main(int argc, char** argv){ //plan.refreshObstacles(obstacle_coordinates); // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed - if(planner_state.make_plan && (!plan.isPlanSafe(/*setpoint_x,setpoint_y*/19.9,19.9) || planner_state.new_goal)){ + if(planner_state.make_plan && (!plan.isPlanSafe(/*setpoint_x,setpoint_y*/1,13) || planner_state.new_goal)){ //ROS_INFO("Make new plan."); planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); diff --git a/src/nodes/test_path_planner_node.cpp b/src/nodes/test_path_planner_node.cpp index 6807bcf..c7f6115 100644 --- a/src/nodes/test_path_planner_node.cpp +++ b/src/nodes/test_path_planner_node.cpp @@ -14,8 +14,8 @@ int main(int argc, char** argv){ ascend_msgs::PathPlanner clientCall; - clientCall.request.goal_x = 19.9; - clientCall.request.goal_y = 19.9; + clientCall.request.goal_x = 1; + clientCall.request.goal_y = 13; if(client.call(clientCall)){ ROS_INFO("Request handled"); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index b486b0b..b411324 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -4,12 +4,11 @@ using namespace control::pathplanner; int coordToIndex(float k) { - int index = round(k/NODE_DISTANCE+0.5); + int index = round(k/NODE_DISTANCE); return (index); } -using control::pathplanner::PathPlanner; PathPlanner::PathPlanner(){} void PathPlanner::initializeGraph(){ @@ -122,38 +121,39 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent //std::cout << "Point: " << x << ", " << y << " Parent: " << parent_x // << ", " << parent_y << std::endl; if(isValidCoordinate(x,y)){ + int x_index = coordToIndex(x); + int y_index = coordToIndex(y); // Making sure the zero is actually zero and not something like 1.4895e-9 if(x > 0.0 && x < 0.01){x = 0.0;} if(y > 0.0 && y < 0.01){y = 0.0;} if(isDestination(x,y)){ - graph[coordToIndex(x)][coordToIndex(y)].setParentX(parent_x); - graph[coordToIndex(x)][coordToIndex(y)].setParentY(parent_y); + graph[x_index][y_index].setParentX(parent_x); + graph[x_index][y_index].setParentY(parent_y); destination_reached = true; - destination_found_x = x; - destination_found_y = y; - //std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; - //std::cout << "Destination parent: x=" << graph[coordToIndex(x)][coordToIndex(y)].getParentX() << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; + std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; + std::cout << "Destination parent: x=" << graph[x_index][y_index].getParentX() << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; } - // If node is not destination and hasn't been searched yet - else if(closed_list[coordToIndex(x)][coordToIndex(y)] == false){ + // If node is not destination and hasn't been searched yet + else if(closed_list[x_index][y_index] == false){ // Calculate distance from start through the parent float new_g = graph[coordToIndex(parent_x)][coordToIndex(parent_y)].getG() + distance_to_parent; // Update graph and open list if distance is less than before through the parent - if(graph[coordToIndex(x)][coordToIndex(y)].getG() > new_g) { - graph[coordToIndex(x)][coordToIndex(y)].updateF(new_g); + if(graph[x_index][y_index].getG() > new_g) { + graph[x_index][y_index].updateF(new_g); // For visualization - if(max_f < graph[coordToIndex(x)][coordToIndex(y)].getF()){ - max_f = graph[coordToIndex(x)][coordToIndex(y)].getF(); + if(max_f < graph[x_index][y_index].getF()){ + max_f = graph[x_index][y_index].getF(); } - graph[coordToIndex(x)][coordToIndex(y)].setParentX(parent_x); - graph[coordToIndex(x)][coordToIndex(y)].setParentY(parent_y); - open_list.push(graph[coordToIndex(x)][coordToIndex(y)]); + graph[x_index][y_index].setParentX(parent_x); + graph[x_index][y_index].setParentY(parent_y); + open_list.push(graph[x_index][y_index]); } } } } void PathPlanner::handleAllSuccessors(float x, float y) { + // LOVER Å LØSE DETTE PÅ EN BEDRE MÅTE handleSuccessor(x+NODE_DISTANCE,y, x, y, NODE_DISTANCE); if(destination_reached){return;} handleSuccessor(x+NODE_DISTANCE, y+NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); @@ -172,7 +172,7 @@ void PathPlanner::handleAllSuccessors(float x, float y) { } bool PathPlanner::isDestination(float x, float y) { - float margin = NODE_DISTANCE/2+0.001; + float margin = NODE_DISTANCE/2; return ((x < end_node.getX()+margin && x>end_node.getX()-margin) && (y < end_node.getY()+margin && y > end_node.getY()-margin)); } @@ -210,11 +210,11 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // If start or end point is not valid, a plan is not created if(!isValidCoordinate(current_x,current_y)){ - ROS_INFO("Drone outside field!"); + // send error to fsm return; } if(!isValidCoordinate(target_x,target_y)){ - ROS_INFO("Target outside field!"); + // send error to fsm return; } @@ -232,7 +232,10 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo float x = end_node.getX(); float y = end_node.getY(); - if(graph[coordToIndex(x)][coordToIndex(y)].obstacle){ + int x_index = coordToIndex(x); + int y_index = coordToIndex(y); + + if(graph[x_index][y_index].obstacle){ plan.push_front(start_node); return; } @@ -245,22 +248,21 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // Go through the graph from end to start through the parents of each node // Add nodes to the plan - plan.push_front(graph[coordToIndex(x)][coordToIndex(y)]); - x = destination_found_x; - y = destination_found_y; - while(!isStart(x, y)){ - plan.push_front(graph[coordToIndex(x)][coordToIndex(y)]); - float x_temp = graph[coordToIndex(x)][coordToIndex(y)].getParentX(); - float y_temp = graph[coordToIndex(x)][coordToIndex(y)].getParentY(); + plan.push_front(graph[x_index][y_index]); + float x_temp = graph[x_index][y_index].getParentX(); + float y_temp = graph[x_index][y_index].getParentY(); x = x_temp; y = y_temp; + x_index = coordToIndex(x); + y_index = coordToIndex(y); + counter ++; - if(counter >250){break;} + if(counter >200){break;} } // Add start node to the plan (might not be necessary) - plan.push_front(graph[coordToIndex(x)][coordToIndex(y)]); + plan.push_front(graph[x_index][y_index]); // Print the plan - can be removed /*std::cout << std::endl << "Whole plan:" << std::endl; for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ @@ -329,9 +331,9 @@ void PathPlanner::simplifyPlan() { third = second; third++; } - // If the straight line does not go through an obstacle, delete the second point - // (no use in going through it when we can go in a straight line) - // The search continues with same starting point, but the second and third point is changed + // If the straight line does not go through an obstacle, delete the second point + // (no use in going through it when we can go in a straight line) + // The search continues with same starting point, but the second and third point is changed else{ simple_plan.erase(second); second = third; @@ -348,7 +350,6 @@ void PathPlanner::simplifyPlan() { bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { if(simple_plan.empty()){ - std::cout << "Simple plan empty" << std::endl; return false; } @@ -357,18 +358,16 @@ bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { current++; while(current != simple_plan.end()){ - - // Find next point in plan by checking distance between setpoint and points in plan if(abs(current->getX()-setpoint_x)<0.1 && abs(current->getY()-setpoint_y) < 0.1){ //std::cout << "Setpoint: " << current->getX() << ", " << current->getY() << std::endl; - //std::cout << "Search from:" << prev->getX() << ", " << prev->getY() << std::endl; + //std::cout << "Search from:" << prev->getX() << ", " << current->getY() << std::endl; break; } else{ + //simple_plan.erase(current); prev = current; current++; if(current == simple_plan.end()){ - // iterated through list without finding match in plan for the setpoint return false; } } @@ -384,7 +383,6 @@ bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { current++; next++; } - return true; } @@ -395,4 +393,4 @@ void PathPlanner::resetParameters() { plan.clear(); simple_plan.clear(); destination_reached = false; -} +} \ No newline at end of file From 861cced74796123fb68ab80c2508ad099730bb18 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 18 Feb 2018 19:27:06 +0100 Subject: [PATCH 057/200] Change from closed list to closed variable for each node --- include/control/planner/node.hpp | 1 + include/control/planner/path_planner.hpp | 1 - src/path_planner/path_planner.cpp | 16 ++++------------ 3 files changed, 5 insertions(+), 13 deletions(-) diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp index 9a1734d..9d79cf7 100644 --- a/include/control/planner/node.hpp +++ b/include/control/planner/node.hpp @@ -33,6 +33,7 @@ class Node{ void updateF(float new_g); bool obstacle = false; + bool closed = false; // Implemented only for the closed list priority queue friend bool operator< (const Node &lhs, const Node &rhs); diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index b425186..e8641d3 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -53,7 +53,6 @@ class PathPlanner{ PathPlanner(); void initializeGraph(); - void initializeClosedList(); // Add a circular obstacle void addObstacle(float center_x, float center_y); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index b411324..8df45cf 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -27,13 +27,6 @@ void PathPlanner::initializeGraph(){ } } -void PathPlanner::initializeClosedList(){ - for (int i = 0; i < GRAPH_SIZE; i++) { - for (int j = 0; j < GRAPH_SIZE; j++){ - closed_list[i][j] = false; - } - } -} void PathPlanner::addObstacle(float center_x, float center_y) { // Find the y value for each x in the circle @@ -42,7 +35,7 @@ void PathPlanner::addObstacle(float center_x, float center_y) { // Do this to fill the circle for(float i = center_y-y; i<=center_y+y; i+= NODE_DISTANCE) { if (isValidCoordinate(x, i)) { - closed_list[coordToIndex(x)][coordToIndex(i)] = true; + graph[coordToIndex(x)][coordToIndex(i)].closed = true; graph[coordToIndex(x)][coordToIndex(i)].obstacle = true; } } @@ -53,7 +46,7 @@ void PathPlanner::addObstacle(float center_x, float center_y) { float x = sqrt(OBSTACLE_RADIUS*OBSTACLE_RADIUS-(y-center_y)*(y-center_y)); for(float i = center_x-x; i<=center_x+x; i+= NODE_DISTANCE) { if (isValidCoordinate(i, y)) { - closed_list[coordToIndex(i)][coordToIndex(y)] = true; + graph[coordToIndex(i)][coordToIndex(y)].closed = true; graph[coordToIndex(i)][coordToIndex(y)].obstacle = true; } } @@ -109,7 +102,7 @@ void PathPlanner::relaxGraph(){ float x = current_node.getX(); float y = current_node.getY(); // Mark node as searched - closed_list[coordToIndex(x)][coordToIndex(y)] = true; + graph[coordToIndex(x)][coordToIndex(y)].closed = true; // Search all eight points around current point handleAllSuccessors(x,y); // Stop search if destination is reached @@ -134,7 +127,7 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent std::cout << "Destination parent: x=" << graph[x_index][y_index].getParentX() << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; } // If node is not destination and hasn't been searched yet - else if(closed_list[x_index][y_index] == false){ + else if(!graph[x_index][y_index].closed){ // Calculate distance from start through the parent float new_g = graph[coordToIndex(parent_x)][coordToIndex(parent_y)].getG() + distance_to_parent; // Update graph and open list if distance is less than before through the parent @@ -225,7 +218,6 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo start_node.setParentY(0); end_node = Node(target_x,target_y, std::numeric_limits::infinity(), 0); initializeGraph(); - initializeClosedList(); refreshObstacles(obstacle_coordinates); From 6e56b1d39ba948ac879cf8ef093ff54bfb65e891 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 18 Feb 2018 19:39:39 +0100 Subject: [PATCH 058/200] Change to euclidian heuristic --- src/path_planner/path_planner.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 8df45cf..94f6be0 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -21,7 +21,7 @@ void PathPlanner::initializeGraph(){ } else { graph[coordToIndex(x)][coordToIndex(y)] = - Node(x,y, std::numeric_limits::infinity(), calculateDiagonalHeuristic(x,y)); + Node(x,y, std::numeric_limits::infinity(), calculateEuclidianHeuristic(x,y)); } } } @@ -70,7 +70,6 @@ void PathPlanner::refreshObstacles(std::list obstacle_coordinates) { } float PathPlanner::calculateDiagonalHeuristic(float x, float y) { - return (calculateEuclidianHeuristic(x,y)); return (std::max(abs(x-end_node.getX()), abs(y-end_node.getY()))); } From b09f81da9cc32e17a9bb01e442daeb930d89f4f6 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 18 Feb 2018 19:45:18 +0100 Subject: [PATCH 059/200] Remove unneccesary code --- src/path_planner/path_planner.cpp | 28 ---------------------------- 1 file changed, 28 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 94f6be0..cd66f11 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -265,34 +265,6 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo } void PathPlanner::simplifyPlan() { - /*std::list::iterator current = plan.begin(); - std::list::iterator next = plan.begin(); - next++; - simple_plan.push_back(*current); - while(next != plan.end()){ - if(coordToIndex(current->getX()) == coordToIndex(next->getX())){ - //std::cout << "VERTICAL" << std::endl; - while(coordToIndex(current->getX()) == coordToIndex(next->getX()) && next != plan.end()){ - current++; - next++; - } - } - else if(coordToIndex(current->getY()) == coordToIndex(next->getY())){ - //std::cout << "HORISONTAL" << std::endl; - while(coordToIndex(current->getY()) == coordToIndex(next->getY()) && next != plan.end()){ - current++; - next++; - } - } - else{ - //std::cout << "DIAGONAL" << std::endl; - while(coordToIndex(current->getX()) != coordToIndex(next->getX()) && coordToIndex(current->getY()) != coordToIndex(next->getY()) && next != plan.end()){ - current++; - next++; - } - } - simple_plan.push_back(*current); - }*/ simple_plan = plan; // Pointing at the three first elements std::list::iterator first = simple_plan.begin(); From 2a0bbdaa9b13ee3ff7d61eb3b8a008f32d27ed25 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 19 Feb 2018 12:14:02 +0100 Subject: [PATCH 060/200] Fix bug with nodes being eachothers parents! --- src/nodes/path_planner_node.cpp | 5 ++--- src/nodes/test_path_planner_node.cpp | 2 +- src/path_planner/path_planner.cpp | 6 ++---- 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 6e7931f..1a3ce1a 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -98,7 +98,7 @@ int main(int argc, char** argv){ //plan.refreshObstacles(obstacle_coordinates); // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed - if(planner_state.make_plan && (!plan.isPlanSafe(/*setpoint_x,setpoint_y*/1,13) || planner_state.new_goal)){ + if(planner_state.make_plan && (!plan.isPlanSafe(/*setpoint_x,setpoint_y*/1,1) || planner_state.new_goal)){ //ROS_INFO("Make new plan."); planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); @@ -113,8 +113,7 @@ int main(int argc, char** argv){ points_in_plan.points.reserve(20); // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan - std::list::iterator second_point = points.begin(); - second_point++; + std::list::iterator second_point = ++(points.begin()); std::cout << "Published points:\t"; for(std::list::iterator it = second_point; it != points.end(); it++){ diff --git a/src/nodes/test_path_planner_node.cpp b/src/nodes/test_path_planner_node.cpp index c7f6115..e83ea0e 100644 --- a/src/nodes/test_path_planner_node.cpp +++ b/src/nodes/test_path_planner_node.cpp @@ -15,7 +15,7 @@ int main(int argc, char** argv){ ascend_msgs::PathPlanner clientCall; clientCall.request.goal_x = 1; - clientCall.request.goal_y = 13; + clientCall.request.goal_y = 1; if(client.call(clientCall)){ ROS_INFO("Request handled"); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index cd66f11..ff3277e 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -118,7 +118,7 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent // Making sure the zero is actually zero and not something like 1.4895e-9 if(x > 0.0 && x < 0.01){x = 0.0;} if(y > 0.0 && y < 0.01){y = 0.0;} - if(isDestination(x,y)){ + if(isDestination(x,y) && !graph[x_index][y_index].closed){ graph[x_index][y_index].setParentX(parent_x); graph[x_index][y_index].setParentY(parent_y); destination_reached = true; @@ -288,10 +288,8 @@ void PathPlanner::simplifyPlan() { if(!isSafeLine(x1,y1,x2,y2)){ //std::cout << "Hit: first = (" << x1 << ", " << y1 << ") Third = (" // << x2 << ", " << y2 << ")" << std::endl; - first = second; - second = first; + first++; second++; - third = second; third++; } // If the straight line does not go through an obstacle, delete the second point From fcbda36e78b917f002c9a205580e8d487a3091e5 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 19 Feb 2018 13:36:39 +0100 Subject: [PATCH 061/200] Use current pos instead of setpoint to check if plan is safe --- src/nodes/path_planner_node.cpp | 2 +- src/path_planner/path_planner.cpp | 39 ++++++++++++++----------------- 2 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 1a3ce1a..fdad992 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -98,7 +98,7 @@ int main(int argc, char** argv){ //plan.refreshObstacles(obstacle_coordinates); // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed - if(planner_state.make_plan && (!plan.isPlanSafe(/*setpoint_x,setpoint_y*/1,1) || planner_state.new_goal)){ + if(planner_state.make_plan && (!plan.isPlanSafe(planner_state.current_x,planner_state.current_y) || planner_state.new_goal)){ //ROS_INFO("Make new plan."); planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index ff3277e..aec3e6e 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -309,35 +309,30 @@ void PathPlanner::simplifyPlan() { } } -bool PathPlanner::isPlanSafe(float setpoint_x, float setpoint_y) { +bool PathPlanner::isPlanSafe(float current_x, float current_y) { if(simple_plan.empty()){ return false; } - std::list::iterator prev = simple_plan.begin(); - std::list::iterator current = prev; - current++; + // Check if current point is the first point of the plan + float margin = NODE_DISTANCE; - while(current != simple_plan.end()){ - if(abs(current->getX()-setpoint_x)<0.1 && abs(current->getY()-setpoint_y) < 0.1){ - //std::cout << "Setpoint: " << current->getX() << ", " << current->getY() << std::endl; - //std::cout << "Search from:" << prev->getX() << ", " << current->getY() << std::endl; - break; - } - else{ - //simple_plan.erase(current); - prev = current; - current++; - if(current == simple_plan.end()){ - return false; - } - } + if(abs(current_x-simple_plan.begin()->getX()) < margin && abs(current_y-simple_plan.begin()->getY())getX() << ", " + << simple_plan.begin()->getY() << std::endl; + simple_plan.pop_front(); + } + + std::list::iterator current = simple_plan.begin(); + std::list::iterator next = ++(simple_plan.begin()); + + // Check if current point to first point in remaining plan is safe + if(!isSafeLine(current_x, current_y, current->getX(), current->getY())){ + return false; } - current = prev; - std::list::iterator next = current; - next++; - while(next != simple_plan.end()){ + // Check if rest of plan is safe + while(next != simple_plan.end() && current != simple_plan.end()){ if(!isSafeLine(current->getX(), current->getY(), next->getX(), next->getY())){ return false; } From f464fcacdf334e3879ec4c6c4ac35939a802abf0 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 19 Feb 2018 13:47:02 +0100 Subject: [PATCH 062/200] Remove setpoint subscriber --- src/nodes/path_planner_node.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index fdad992..8fdebed 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -18,9 +18,6 @@ using control::pathplanner::PathPlanner; using ActionServerType = actionlib::SimpleActionServer; std::list obstacle_coordinates; -// Only setpoints within plan (not goal) -float setpoint_x = 0; -float setpoint_y = 0; struct PlannerState{ float current_x, current_y, goal_x, goal_y; @@ -63,11 +60,6 @@ void updateObstaclesCB(ascend_msgs::GRStateArray::ConstPtr msg_p){ } } -void updateSetpointCB(mavros_msgs::PositionTarget::ConstPtr msg_p){ - setpoint_x = msg_p->position.x; - setpoint_y = msg_p->position.y; -} - int main(int argc, char** argv){ @@ -84,7 +76,6 @@ int main(int argc, char** argv){ //ros::Subscriber sub_obstacles = n.subscribe(control::PlannerConfig::obstacle_state_topic, 1, updateObstaclesCB); - ros::Subscriber sub_setpoint = n.subscribe(control::PlannerConfig::mavros_setpoint_topic, 1, updateSetpointCB); ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); From 6cb3f9ed3e85f3ae24577617d307556334eea738 Mon Sep 17 00:00:00 2001 From: atussa Date: Tue, 20 Feb 2018 15:59:59 +0100 Subject: [PATCH 063/200] Add function for removing passed points in plan --- include/control/planner/path_planner.hpp | 1 + src/nodes/path_planner_node.cpp | 26 +++++++++++++++++------- src/path_planner/path_planner.cpp | 22 ++++++++++++++------ 3 files changed, 36 insertions(+), 13 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index e8641d3..e06adce 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -85,6 +85,7 @@ class PathPlanner{ // Verify the plan from where the drone is to the end point bool isPlanSafe(float current_x, float current_y); void resetParameters(); + void removeOldPoints(float current_x, float current_y); // These functions are mainly for the visualization std::array, GRAPH_SIZE> getGraph(){return graph;} diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 8fdebed..335e0e6 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -70,7 +70,7 @@ int main(int argc, char** argv){ ros::init(argc, argv, "path_planner_server"); ros::NodeHandle n; - ros::Rate rate(30.0); + ros::Rate rate(1); control::PlannerConfig::loadParams(); @@ -81,33 +81,35 @@ int main(int argc, char** argv){ ros::ServiceServer server = n.advertiseService("path_planner_service", boost::bind(&newPlanCB, _1, _2, &planner_state)); + ascend_msgs::PointArray points_in_plan; while(ros::ok()){ ros::spinOnce(); + //plan.refreshObstacles(obstacle_coordinates); + plan.removeOldPoints(planner_state.current_x, planner_state.current_y); // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed if(planner_state.make_plan && (!plan.isPlanSafe(planner_state.current_x,planner_state.current_y) || planner_state.new_goal)){ //ROS_INFO("Make new plan."); planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); - std::list points = plan.getSimplePlan(); + std::list simple_plan = plan.getSimplePlan(); geometry_msgs::Point point; // Iterate through the points in the plan and publish to pathplanner-action ascend_msgs::PathPlannerFeedback feedback; // For saving memory feedback.points_in_plan.reserve(20); - ascend_msgs::PointArray points_in_plan; points_in_plan.points.reserve(20); // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan - std::list::iterator second_point = ++(points.begin()); + std::list::iterator second_point = ++(simple_plan.begin()); std::cout << "Published points:\t"; - for(std::list::iterator it = second_point; it != points.end(); it++){ + for(std::list::iterator it = second_point; it != simple_plan.end(); it++){ point.x = it->getX(); point.y = it->getY(); @@ -120,9 +122,19 @@ int main(int argc, char** argv){ pub_plan.publish(points_in_plan); std::cout << std::endl; } - else{ - rate.sleep(); + + // Publish plan as long as a plan is requested + if(planner_state.make_plan){ + std::cout << "Published points:\t"; + for(auto it = points_in_plan.points.begin(); it != points_in_plan.points.end(); it++){ + std::cout << it->x << ", " << it->y << "\t" << std::endl; + } + pub_plan.publish(points_in_plan); + std::cout << std::endl; } + + rate.sleep(); + } ros::spin(); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index aec3e6e..3546740 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -110,8 +110,6 @@ void PathPlanner::relaxGraph(){ } void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent_y, float distance_to_parent) { - //std::cout << "Point: " << x << ", " << y << " Parent: " << parent_x - // << ", " << parent_y << std::endl; if(isValidCoordinate(x,y)){ int x_index = coordToIndex(x); int y_index = coordToIndex(y); @@ -145,7 +143,6 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent } void PathPlanner::handleAllSuccessors(float x, float y) { - // LOVER Å LØSE DETTE PÅ EN BEDRE MÅTE handleSuccessor(x+NODE_DISTANCE,y, x, y, NODE_DISTANCE); if(destination_reached){return;} handleSuccessor(x+NODE_DISTANCE, y+NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); @@ -286,8 +283,6 @@ void PathPlanner::simplifyPlan() { // If an obstacle is hit, the points are necessary and the search will start // at the end of the current search. if(!isSafeLine(x1,y1,x2,y2)){ - //std::cout << "Hit: first = (" << x1 << ", " << y1 << ") Third = (" - // << x2 << ", " << y2 << ")" << std::endl; first++; second++; third++; @@ -317,7 +312,7 @@ bool PathPlanner::isPlanSafe(float current_x, float current_y) { // Check if current point is the first point of the plan float margin = NODE_DISTANCE; - if(abs(current_x-simple_plan.begin()->getX()) < margin && abs(current_y-simple_plan.begin()->getY())getX()) < margin && abs(current_y-simple_plan.begin()->getY()) < margin){ std::cout << "Remove " << simple_plan.begin()->getX() << ", " << simple_plan.begin()->getY() << std::endl; simple_plan.pop_front(); @@ -349,4 +344,19 @@ void PathPlanner::resetParameters() { plan.clear(); simple_plan.clear(); destination_reached = false; +} + +void PathPlanner::removeOldPoints(float current_x, float current_y){ + // Check if current point is the first point of the plan + float margin = NODE_DISTANCE; + + if(simple_plan.empty()){ + return; + } + + if(abs(current_x-simple_plan.begin()->getX()) < margin && abs(current_y-simple_plan.begin()->getY())getX() << ", " + << simple_plan.begin()->getY() << std::endl; + simple_plan.pop_front(); + } } \ No newline at end of file From 1c37e3276443c016c08ab078a9838e6fd4578d3e Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 22 Feb 2018 11:51:19 +0100 Subject: [PATCH 064/200] Prototype drone speed based avoidance zone --- src/fsm/go_to_state.cpp | 86 +++++++++++++--------- src/tools/obstacle_avoidance.cpp | 121 +++++++++++++++++++------------ 2 files changed, 126 insertions(+), 81 deletions(-) diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 9ff439e..543e00c 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -13,6 +13,42 @@ GoToState::GoToState() : StateInterface::StateInterface() { setpoint_.type_mask = default_mask; } +/** + * @brief Returns a yaw that is a multiple of 90 degrees + * @details Drone should fly as straight forward as possible + * , but yaw should be a multiple of 90 degrees. + * This method assumes dx and dy != 0 at the same time + * @param dx difference in x + * @param dy difference in y + * @return Yaw angle in radians - not mavros corrected + */ +double calculatePathYaw(double dx, double dy) { + //Avoid fatal error if dx and dy is too small + //If method is used correctly this should NEVER be a problem + if(std::fabs(dx * dx + dy * dy) < 0.001) { + return 0; + } + /* + angle = acos(([dx, dy] dot [1,0]) / (norm([dx, dy]) * norm([1,0]))) = acos(dx / (norm([dx, dy]) * 1)) + */ + double angle = std::acos(dx / std::sqrt(dx * dx + dy * dy)); + + //Select closest multiple of 90 degrees + if(angle > 3 * PI / 4) { + angle = PI; + } else if(angle > PI / 4) { + angle = PI / 2.0; + } else { + angle = 0; + } + //Invert if dy is negative + if (dy < 0) { + angle = -angle; + } + + return angle; +} + void GoToState::handleEvent(ControlFSM& fsm, const EventData& event) { if(event.isValidRequest()) { if(event.request == RequestType::ABORT) { @@ -74,7 +110,21 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { using control::getMavrosCorrectedTargetYaw; using control::DroneHandler; auto quat = DroneHandler::getCurrentPose().pose.orientation; - setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(quat))); + auto pos = DroneHandler::getCurrentPose().pose.position; + + // calculate dx,dy + auto dx = setpoint_.position.x - pos.x; + auto dy = setpoint_.position.y - pos.y; + + setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(quat))); + /* + if (std::fabs(dx*dx + dy*dy) < 0.01){ + // assume drone is stationary and dont change orientation + } else{ + setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(calculatePathYaw(dx, dy))); + } + */ + } catch(const std::exception& e) { //Critical bug - no recovery //Transition to position hold if no pose available @@ -147,41 +197,7 @@ void GoToState::stateInit(ControlFSM& fsm) { control::handleInfoMsg("GoTo init completed!"); } -/** - * @brief Returns a yaw that is a multiple of 90 degrees - * @details Drone should fly as straight forward as possible - * , but yaw should be a multiple of 90 degrees. - * This method assumes dx and dy != 0 at the same time - * @param dx difference in x - * @param dy difference in y - * @return Yaw angle in radians - not mavros corrected - */ -double calculatePathYaw(double dx, double dy) { - //Avoid fatal error if dx and dy is too small - //If method is used correctly this should NEVER be a problem - if(std::fabs(dx * dx + dy * dy) < 0.001) { - return 0; - } - /* - angle = acos(([dx, dy] dot [1,0]) / (norm([dx, dy]) * norm([1,0]))) = acos(dx / (norm([dx, dy]) * 1)) - */ - double angle = std::acos(dx / std::sqrt(dx * dx + dy * dy)); - - //Select closest multiple of 90 degrees - if(angle > 3 * PI / 4) { - angle = PI; - } else if(angle > PI / 4) { - angle = PI / 2.0; - } else { - angle = 0; - } - //Invert if dy is negative - if (dy < 0) { - angle = -angle; - } - return angle; -} bool GoToState::stateIsReady(ControlFSM& fsm) { return true; diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 21e9e2e..6da6607 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -13,7 +13,7 @@ #include #include -#include +#include // not used? #include #include @@ -28,17 +28,25 @@ inline float angleWrapper(const float angle){ } template -inline float calcDistanceToObstacle(const T& point, const K& obstacle_position){ - geometry_msgs::Vector3 delta_to_obstacle; - delta_to_obstacle.x = point.x - obstacle_position.x; - delta_to_obstacle.y = point.y - obstacle_position.y; - delta_to_obstacle.z = point.z; +inline geometry_msgs::Vector3 calcVectorBetweenPoints(const T& point_A, const K& point_B){ + geometry_msgs::Vector3 vector_diff; + vector_diff.x = point_A.x - point_B.x; + vector_diff.y = point_A.y - point_B.y; + vector_diff.z = point_A.z - point_B.z; + + return vector_diff; +} - const float distance_to_obstacle = std::sqrt(std::pow(delta_to_obstacle.x, 2) + std::pow(delta_to_obstacle.y, 2)); +template +inline float calcDistanceToObstacle(const T& point, const K& obstacle_position){ + const auto vector_to_obstacle = calcVectorBetweenPoints(point, obstacle_position); + const float distance_to_obstacle = std::sqrt(std::pow(vector_to_obstacle.x, 2) + std::pow(vector_to_obstacle.y, 2)); return distance_to_obstacle; } + + template inline float calcAngleToObstacle(const T& point, const K& obstacle_position, const float obstacle_direction){ geometry_msgs::Vector3 delta_drone_obstacle; @@ -64,27 +72,27 @@ inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle){ /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle /// which is allowed, referenced from the obstacles coordinate system. x-axis is positive -/// direction of motion. Obstacle driving in with angle=0 would be driving in x-direction -inline geometry_msgs::Vector3 avoidZone(const float angle, +/// direction of motion. Obstacle driving in with obstacle_direction=0 would be driving in x-direction +inline geometry_msgs::Vector3 avoidZone(const float obstacle_direction, const float front_clearance, const float back_clearance, const float side_clearance){ - const auto angle_positive = angleWrapper(angle); + const auto angle_positive = angleWrapper(obstacle_direction); const bool drone_in_front_of_obstacle = angle_positive <= PI/2 || angle_positive >= 3*PI/2; geometry_msgs::Vector3 minimum_vector; minimum_vector.z = 0.0f; + minimum_vector.y = side_clearance * std::sin(obstacle_direction); if (drone_in_front_of_obstacle){ - //ROS_INFO_THROTTLE(1, "Drone in front of obstacle: %f", angle_positive); // Triangle shape - minimum_vector.y = side_clearance * std::sin(angle); - minimum_vector.x = (front_clearance/side_clearance)*(side_clearance - abs(minimum_vector.y)); + //minimum_vector.x = (front_clearance/side_clearance)*(side_clearance - std::fabs(minimum_vector.y)); + + // Ellipse + minimum_vector.x = front_clearance * std::cos(obstacle_direction); } else { // Ellipse - //ROS_INFO_THROTTLE(1, "Drone behind obstacle: %f", angle_positive); - minimum_vector.y = side_clearance*std::sin(angle); - minimum_vector.x = back_clearance*std::cos(angle); + minimum_vector.x = back_clearance * std::cos(obstacle_direction); } return minimum_vector; @@ -97,63 +105,84 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); // Format of isn't finalized yet const geometry_msgs::PoseStamped drone_pose = control::DroneHandler::getCurrentPose(); + + const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; + const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); + const float drone_speed_ratio = std::sqrt(drone_speed/3.f); + for (int i = 0; i < obstacles.count; i++){ const auto obstacle_position = obstacles.global_robot_position[i]; const float obstacle_direction = obstacles.direction[i]; const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_position); const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); - if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ + if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_position, obstacle_direction); - //ROS_INFO("Angle: %f", drone_angle_to_obstacle); - const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_position, obstacle_direction); + // True if setpoint is within a 120 deg cone away from the obstacle // TODO: turn this angle into a param once complete const bool setpoint_reachable = (drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); - - geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, - Config::obstacle_clearance_front, Config::obstacle_clearance_back, Config::obstacle_clearance_side); - ROS_INFO_THROTTLE(1, "Local minimum vector: %.3f\t%.3f", minimum_vector.x, minimum_vector.y); - // Rotate to global coordinate system - minimum_vector = rotateXY(minimum_vector, -obstacle_direction); + // magic numbers are minimum clearance no matter what. Turn into parameters and include + // this logic in the avoidZone function + const auto clearance_front = std::max((float)Config::obstacle_clearance_front*drone_speed_ratio, 1.f); + const auto clearance_back = std::max((float)Config::obstacle_clearance_back*drone_speed_ratio, 0.7f); + const auto clearance_side = std::max((float)Config::obstacle_clearance_side*drone_speed_ratio, 0.7f); + geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, + clearance_front, clearance_back, clearance_side); + // Rotate to global coordinate system const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); + // ROS_INFO_THROTTLE(1, "Minimum vector: %.3f\t%.3f\t(%.3f)\t(%.3f)", minimum_vector.x, minimum_vector.y, + // minimum_distance, drone_angle_to_obstacle); + minimum_vector = rotateXY(minimum_vector, -obstacle_direction); - if (setpoint_reachable - && setpoint_distance_to_obstacle > drone_distance_to_obstacle - && setpoint_distance_to_obstacle > minimum_distance - && drone_distance_to_obstacle < minimum_distance){ - // no action, maybe logging? - ROS_INFO_THROTTLE(1, "Going to setpoint instead"); - } - else if (drone_distance_to_obstacle < minimum_distance) { - if (setpoint_modified){ - ROS_WARN("[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); - setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) - } - // TODO: find out a better solution to this problem - // need to avoid obstacle - setpoint_modified = true; - setpoint->position.x = obstacle_position.x + minimum_vector.x; - setpoint->position.y = obstacle_position.y + minimum_vector.y; - if (setpoint->position.z < Config::min_in_air_alt){ - setpoint->position.z = Config::min_in_air_alt; - } - } + //ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_position.x + minimum_vector.x, obstacle_position.y + minimum_vector.y); + + + if (drone_distance_to_obstacle < minimum_distance){ + if (setpoint_reachable + && setpoint_distance_to_obstacle > drone_distance_to_obstacle + && setpoint_distance_to_obstacle > minimum_distance){ + // no action, maybe logging? + ROS_INFO_THROTTLE(1, "Going to setpoint instead"); + } + else { + const geometry_msgs::Vector3 drone_delta_to_obstacle + = calcVectorBetweenPoints(drone_pose.pose.position, obstacle_position); + + if (setpoint_modified){ + // TODO: find out a better solution to this problem + ROS_WARN("[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); + setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) + } + // need to avoid obstacle + setpoint_modified = true; + setpoint->position.x = obstacle_position.x + minimum_vector.x; + setpoint->position.y = obstacle_position.y + minimum_vector.y; + if (setpoint->position.z < Config::min_in_air_alt){ + setpoint->position.z = Config::min_in_air_alt; + } + + ROS_INFO_THROTTLE(0.25,"Avoiding: %.3f -> %.3f", drone_distance_to_obstacle, minimum_distance); + } + } } if (setpoint_modified){ const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); } + } + if (!setpoint_modified){ + ROS_INFO_THROTTLE(1,"[obstacle avoidance] no dangerous obstacles"); } return setpoint_modified; From 855dd87e547842ff24683d4487ba5bf4bd168e9b Mon Sep 17 00:00:00 2001 From: Marvin Reza Date: Fri, 23 Feb 2018 11:52:11 +0100 Subject: [PATCH 065/200] Fix some big bugs --- control_fsm_default_config.yaml | 32 +++++++------- src/tools/obstacle_avoidance.cpp | 74 ++++++++++++++++---------------- 2 files changed, 53 insertions(+), 53 deletions(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index 066a56f..c7bb0f7 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -6,45 +6,45 @@ #When is a destination reached dest_reached_margin: 0.3, #Topic for publishing error messages - fsm_error_topic: control/fsm/on_error, + fsm_error_topic: control/fsm/on_error, #Topic for publishing info messages - fsm_info_topic: control/fsm/on_info, + fsm_info_topic: control/fsm/on_info, #Topic for publishing current state fsm_state_changed_topic: control/fsm/state_changed, #Topic for publishing warning messages - fsm_warn_topic: control/fsm/on_warn, + fsm_warn_topic: control/fsm/on_warn, #Altitude for optimal gb detection - gb_search_altitude: 2.5, + gb_search_altitude: 2.5, #Time to wait before automatic landing goto_hold_dest_time: 0.5, #Topic for recieving land detector data - land_detector_topic: /mavros/extended_state, + land_detector_topic: /mavros/extended_state, #Topic for recieving lidar info lidar_topic: /perception/obstacles/lidar, #Topic for recieving local velocity estimates local_velocity_topic: /mavros/local_position/velocity, #Topic for recieving local position estimates - local_position_topic: /mavros/local_position/pose, + local_position_topic: /mavros/local_position/pose, #Topic for recieving mavros state mavros_state_topic: /mavros/state, #When is message considered old? - message_timeout: 2.0, + message_timeout: 2.0, #For what distances is it uneccesary to adjust yaw - no_yaw_correct_dist: 0.2, - #Safety distance - obstacle_too_close_dist: 2.0, + no_yaw_correct_dist: 0.2, + #Safety distance + obstacle_too_close_dist: 2.0, #Require all data streams to enable FSM require_all_streams: true, - #Require obstacle detection - require_obs_detection: true, + #Require obstacle detection + require_obs_detection: true, #Safe altitude for hovering - safe_hover_alt: 1.0, + safe_hover_alt: 1.0, #Margin to determine if we're close enough to setpoint - setp_reached_margin: 0.3, + setp_reached_margin: 0.3, #Buffersize for status messages status_msg_buffer_size: 10, #Takeoff altitude - takeoff_altitude: 1.0, + takeoff_altitude: 1.0, #Margin to determine if we're close enough to yaw setpoint yaw_reached_margin: 0.02, #Altitude for goto in landxy @@ -62,5 +62,5 @@ #Obstacle avoidance clearance in back obstacle_clearance_back: 2.0, #Obstacle avoidance checkradius - obstacle_clearance_checkradius: 100 + obstacle_clearance_checkradius: 5.0 } diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 6da6607..7f9fa85 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -30,7 +30,7 @@ inline float angleWrapper(const float angle){ template inline geometry_msgs::Vector3 calcVectorBetweenPoints(const T& point_A, const K& point_B){ geometry_msgs::Vector3 vector_diff; - vector_diff.x = point_A.x - point_B.x; + vector_diff.x = point_A.x - point_B.x; vector_diff.y = point_A.y - point_B.y; vector_diff.z = point_A.z - point_B.z; @@ -50,12 +50,12 @@ inline float calcDistanceToObstacle(const T& point, const K& obstacle_position){ template inline float calcAngleToObstacle(const T& point, const K& obstacle_position, const float obstacle_direction){ geometry_msgs::Vector3 delta_drone_obstacle; - delta_drone_obstacle.x = point.x - obstacle_position.x; + delta_drone_obstacle.x = point.x - obstacle_position.x; delta_drone_obstacle.y = point.y - obstacle_position.y; - delta_drone_obstacle.z = point.z; + delta_drone_obstacle.z = point.z; + + const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); - const float angle_to_obstacle = angleWrapper(-PI/2 + std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); - return angle_to_obstacle; } @@ -71,39 +71,39 @@ inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle){ } /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle -/// which is allowed, referenced from the obstacles coordinate system. x-axis is positive +/// which is allowed, referenced from the obstacles coordinate system. x-axis is positive /// direction of motion. Obstacle driving in with obstacle_direction=0 would be driving in x-direction -inline geometry_msgs::Vector3 avoidZone(const float obstacle_direction, +inline geometry_msgs::Vector3 avoidZone(const float obstacle_direction, const float front_clearance, const float back_clearance, const float side_clearance){ const auto angle_positive = angleWrapper(obstacle_direction); const bool drone_in_front_of_obstacle = angle_positive <= PI/2 || angle_positive >= 3*PI/2; geometry_msgs::Vector3 minimum_vector; - minimum_vector.z = 0.0f; + minimum_vector.z = 0.0f; - minimum_vector.y = side_clearance * std::sin(obstacle_direction); + minimum_vector.y = side_clearance * std::sin(angle_positive); if (drone_in_front_of_obstacle){ // Triangle shape //minimum_vector.x = (front_clearance/side_clearance)*(side_clearance - std::fabs(minimum_vector.y)); - - // Ellipse - minimum_vector.x = front_clearance * std::cos(obstacle_direction); + + // Ellipse + minimum_vector.x = front_clearance * std::cos(angle_positive); } else { // Ellipse - minimum_vector.x = back_clearance * std::cos(obstacle_direction); + minimum_vector.x = back_clearance * std::cos(angle_positive); } return minimum_vector; } bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { - using control::Config; + using control::Config; bool setpoint_modified{false}; - const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); // Format of isn't finalized yet - const geometry_msgs::PoseStamped drone_pose = control::DroneHandler::getCurrentPose(); + const auto& obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + const auto& drone_pose = control::DroneHandler::getCurrentPose(); const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; @@ -116,45 +116,48 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_position); const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); - if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ + if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ + ROS_INFO_THROTTLE(1, "Distance: %.2f", drone_distance_to_obstacle); // perform obstacle avoidance const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_position, obstacle_direction); const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_position, obstacle_direction); // True if setpoint is within a 120 deg cone away from the obstacle // TODO: turn this angle into a param once complete - const bool setpoint_reachable = - (drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && - setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); + const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; + //ROS_INFO_THROTTLE(2, "d_angle - s_angle: %.3f", fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI))); + //(drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && + //setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); - // magic numbers are minimum clearance no matter what. Turn into parameters and include - // this logic in the avoidZone function + + // magic numbers are minimum clearance no matter what. Turn into parameters and include + // this logic in the avoidZone function + ROS_INFO_THROTTLE("Drone speed ratio: %.3f", drone_speed_ratio); const auto clearance_front = std::max((float)Config::obstacle_clearance_front*drone_speed_ratio, 1.f); - const auto clearance_back = std::max((float)Config::obstacle_clearance_back*drone_speed_ratio, 0.7f); - const auto clearance_side = std::max((float)Config::obstacle_clearance_side*drone_speed_ratio, 0.7f); + const auto clearance_back = std::max((float)Config::obstacle_clearance_back*drone_speed_ratio, 1.f); + const auto clearance_side = std::max((float)Config::obstacle_clearance_side*drone_speed_ratio, 1.f); geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, clearance_front, clearance_back, clearance_side); // Rotate to global coordinate system const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); - // ROS_INFO_THROTTLE(1, "Minimum vector: %.3f\t%.3f\t(%.3f)\t(%.3f)", minimum_vector.x, minimum_vector.y, - // minimum_distance, drone_angle_to_obstacle); - minimum_vector = rotateXY(minimum_vector, -obstacle_direction); + //ROS_INFO("Local min vec: %.3f\t%.3f", minimum_vector.x, minimum_vector.y); + minimum_vector = rotateXY(minimum_vector, obstacle_direction); - //ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_position.x + minimum_vector.x, obstacle_position.y + minimum_vector.y); +//ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_position.x + minimum_vector.x, obstacle_position.y + minimum_vector.y); - if (drone_distance_to_obstacle < minimum_distance){ + if (drone_distance_to_obstacle < minimum_distance){ if (setpoint_reachable - && setpoint_distance_to_obstacle > drone_distance_to_obstacle + && setpoint_distance_to_obstacle > drone_distance_to_obstacle && setpoint_distance_to_obstacle > minimum_distance){ // no action, maybe logging? ROS_INFO_THROTTLE(1, "Going to setpoint instead"); } else { - const geometry_msgs::Vector3 drone_delta_to_obstacle + const geometry_msgs::Vector3 drone_delta_to_obstacle = calcVectorBetweenPoints(drone_pose.pose.position, obstacle_position); if (setpoint_modified){ @@ -170,21 +173,18 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget setpoint->position.z = Config::min_in_air_alt; } - ROS_INFO_THROTTLE(0.25,"Avoiding: %.3f -> %.3f", drone_distance_to_obstacle, minimum_distance); + //ROS_INFO_THROTTLE(0.25,"Avoiding: %.3f -> %.3f", drone_distance_to_obstacle, minimum_distance); + ROS_INFO_THROTTLE(1,"Avoid: [%.2f, %.2f]; %.2f %.2f", drone_delta_to_obstacle.x, drone_delta_to_obstacle.y, drone_angle_to_obstacle, setpoint_angle_to_obstacle); } } } if (setpoint_modified){ const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); - ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); + //ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); } } - if (!setpoint_modified){ - ROS_INFO_THROTTLE(1,"[obstacle avoidance] no dangerous obstacles"); - } - return setpoint_modified; } From 3f64f11f80e6c1917c435a0cdf1509f372eab4eb Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 23 Feb 2018 15:06:57 +0100 Subject: [PATCH 066/200] Prepare for unit testing --- include/control/tools/obstacle_avoidance.hpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index 5ce4901..5d594e6 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -37,6 +37,25 @@ class ObstacleAvoidance { bool isReady() { return true; } }; + +// Only do this for unit testing +#ifdef CONTROL_FSM_UNIT_TEST +#include + +float angleWrapper(const float angle); + +template +inline geometry_msgs::Vector3 calcVectorBetweenPoints(const T& point_A, const K& point_B); + +template +inline float calcDistanceToObstacle(const T& point, const K& obstacle_position); + +template +inline float calcAngleToObstacle(const T& point, const K& obstacle_position, const float obstacle_direction); + +template +inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle); +#endif } #endif From 7128652b89c79aa5c442c99d203986fc386dc520 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 15:42:32 +0100 Subject: [PATCH 067/200] Publish time along with plan --- src/nodes/path_planner_node.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 335e0e6..774d841 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -8,7 +8,7 @@ #include "ascend_msgs/GRStateArray.h" #include #include "control/tools/planner_config.hpp" -#include "ascend_msgs/PointArray.h" +#include "ascend_msgs/PointArrayStamped.h" #include "ascend_msgs/PathPlanner.h" using control::pathplanner::PathPlanner; @@ -81,7 +81,8 @@ int main(int argc, char** argv){ ros::ServiceServer server = n.advertiseService("path_planner_service", boost::bind(&newPlanCB, _1, _2, &planner_state)); - ascend_msgs::PointArray points_in_plan; + ascend_msgs::PointArrayStamped msg; + geometry_msgs::Point points_in_plan; while(ros::ok()){ @@ -119,7 +120,10 @@ int main(int argc, char** argv){ std::cout << point.x << ", " << point.y << "\t"; } - pub_plan.publish(points_in_plan); + msg.points = points_in_plan; + msg.header = ros::Time::now(); + pub_plan.publish(msg); + std::cout << std::endl; } From 3d2158090a13b0be387ff3b5c85f11f2aa1034cf Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 15:45:01 +0100 Subject: [PATCH 068/200] Fix small bug --- src/nodes/path_planner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 774d841..a787155 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -121,7 +121,7 @@ int main(int argc, char** argv){ } msg.points = points_in_plan; - msg.header = ros::Time::now(); + msg.header.stamp = ros::Time::now(); pub_plan.publish(msg); std::cout << std::endl; From c87179cc8b9c7caf65d935ce42e6d31d03f7386d Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 16:34:10 +0100 Subject: [PATCH 069/200] Move misplaced define to path planner file :mushroom: --- include/control/planner/node.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp index 9d79cf7..b246cc8 100644 --- a/include/control/planner/node.hpp +++ b/include/control/planner/node.hpp @@ -1,8 +1,6 @@ #ifndef NODE_HPP #define NODE_HPP -#define FIELD_LENGTH 20.0 - class Node{ private: float x; From 7d3ef231c6a5c44a4659dd148c64ddfc0597f94c Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 18:20:27 +0100 Subject: [PATCH 070/200] Change constexpr to member variables of class --- include/control/planner/path_planner.hpp | 24 +++++---- src/path_planner/path_planner.cpp | 69 +++++++++++++----------- 2 files changed, 51 insertions(+), 42 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index e06adce..d829ec7 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -18,20 +18,21 @@ namespace control{ namespace pathplanner{ -constexpr float OBSTACLE_RADIUS = 1.0; //meters -constexpr float NODE_DISTANCE = 0.4; // meters -constexpr float DIAGONAL_NODE_DISTANCE = sqrt(2*NODE_DISTANCE*NODE_DISTANCE); // meters - -constexpr int GRAPH_SIZE = FIELD_LENGTH/NODE_DISTANCE+1.5; // number of nodes in one direction +#define FIELD_LENGTH 20.0 class PathPlanner; class PathPlanner{ private: - std::array, GRAPH_SIZE> graph; + float obstacle_radius; //meters + float node_distance; //meters + float diagonal_node_distance; //meters + int graph_size; //number of nodes in one direction + + std::vector> graph; std::priority_queue open_list; - std::array, GRAPH_SIZE> closed_list; + //std::array, graph_size> closed_list; std::list plan; std::list simple_plan; @@ -49,8 +50,11 @@ class PathPlanner{ float destination_found_x; float destination_found_y; + PathPlanner() = delete; public: - PathPlanner(); + PathPlanner(float obstacle_radius, float node_distance); + + int coordToIndex(float coord); void initializeGraph(); @@ -64,7 +68,7 @@ class PathPlanner{ float calculateEuclidianHeuristic(float x, float y); // Print the f-value of each node - void printGraph(); + //void printGraph(); void relaxGraph(); // @@ -88,7 +92,7 @@ class PathPlanner{ void removeOldPoints(float current_x, float current_y); // These functions are mainly for the visualization - std::array, GRAPH_SIZE> getGraph(){return graph;} + std::vector> getGraph(){return graph;} std::list getPlan(){return plan;} std::list getSimplePlan(){return simple_plan;} // For the colors in the visualization diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 3546740..7431505 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -3,17 +3,24 @@ using namespace control::pathplanner; -int coordToIndex(float k) { - int index = round(k/NODE_DISTANCE); - return (index); -} +PathPlanner::PathPlanner(float obstacle_radius, float node_distance) + : obstacle_radius(obstacle_radius), node_distance(node_distance){ + diagonal_node_distance = sqrt(2*node_distance*node_distance); + // plus 1 to include both 0 and the last node + graph_size = round(FIELD_LENGTH/node_distance+1); + + graph.resize(graph_size, std::vector(graph_size)); +} -PathPlanner::PathPlanner(){} +int PathPlanner::coordToIndex(float coord) { + int index = round(coord/node_distance); + return (index); +} void PathPlanner::initializeGraph(){ - for (float x = 0; x < FIELD_LENGTH; x += NODE_DISTANCE){ - for (float y = 0; y < FIELD_LENGTH; y += NODE_DISTANCE){ + for (float x = 0; x < FIELD_LENGTH; x += node_distance){ + for (float y = 0; y < FIELD_LENGTH; y += node_distance){ if(isStart(x,y)){ graph[coordToIndex(x)][coordToIndex(y)] = start_node; // Push the start node to the open list so that the search starts from the start @@ -27,13 +34,12 @@ void PathPlanner::initializeGraph(){ } } - void PathPlanner::addObstacle(float center_x, float center_y) { // Find the y value for each x in the circle - for (float x = center_x-OBSTACLE_RADIUS; x <= center_x+OBSTACLE_RADIUS; x+=NODE_DISTANCE) { - float y = sqrt(OBSTACLE_RADIUS*OBSTACLE_RADIUS-(x-center_x)*(x-center_x)); + for (float x = center_x-obstacle_radius; x <= center_x+obstacle_radius; x+=node_distance) { + float y = sqrt(obstacle_radius*obstacle_radius-(x-center_x)*(x-center_x)); // Do this to fill the circle - for(float i = center_y-y; i<=center_y+y; i+= NODE_DISTANCE) { + for(float i = center_y-y; i<=center_y+y; i+= node_distance) { if (isValidCoordinate(x, i)) { graph[coordToIndex(x)][coordToIndex(i)].closed = true; graph[coordToIndex(x)][coordToIndex(i)].obstacle = true; @@ -42,9 +48,9 @@ void PathPlanner::addObstacle(float center_x, float center_y) { } // Do the same as over, just switch x and y - for (float y = center_y-OBSTACLE_RADIUS; y <= center_y+OBSTACLE_RADIUS; y+=NODE_DISTANCE) { - float x = sqrt(OBSTACLE_RADIUS*OBSTACLE_RADIUS-(y-center_y)*(y-center_y)); - for(float i = center_x-x; i<=center_x+x; i+= NODE_DISTANCE) { + for (float y = center_y-obstacle_radius; y <= center_y+obstacle_radius; y+=node_distance) { + float x = sqrt(obstacle_radius*obstacle_radius-(y-center_y)*(y-center_y)); + for(float i = center_x-x; i<=center_x+x; i+= node_distance) { if (isValidCoordinate(i, y)) { graph[coordToIndex(i)][coordToIndex(y)].closed = true; graph[coordToIndex(i)][coordToIndex(y)].obstacle = true; @@ -59,8 +65,7 @@ void PathPlanner::refreshObstacles(std::list obstacle_coordinates) { return; } std::list::iterator x = obstacle_coordinates.begin(); - std::list::iterator y = x; - y++; + std::list::iterator y = ++(obstacle_coordinates.begin()); while(y != obstacle_coordinates.end()){ std::cout << "adding obstacle" << std::endl; addObstacle(*x, *y); @@ -81,16 +86,16 @@ float PathPlanner::calculateEuclidianHeuristic(float x, float y) { return(sqrt((x-end_node.getX())*(x-end_node.getX())+(y-end_node.getY())*(y-end_node.getY()))); } -void PathPlanner::printGraph(){ - for(int i = 0; i < GRAPH_SIZE; i++){ - for(int j = 0; j < GRAPH_SIZE; j++){ +/*void PathPlanner::printGraph(){ + for(int i = 0; i < graph_size; i++){ + for(int j = 0; j < graph_size; j++){ std::cout << std::fixed; std::cout << std::setprecision(2); std::cout << graph[i][j].getF() << " "; } std::cout << std::endl; } -} +}*/ void PathPlanner::relaxGraph(){ Node current_node; @@ -143,31 +148,31 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent } void PathPlanner::handleAllSuccessors(float x, float y) { - handleSuccessor(x+NODE_DISTANCE,y, x, y, NODE_DISTANCE); + handleSuccessor(x+node_distance,y, x, y, node_distance); if(destination_reached){return;} - handleSuccessor(x+NODE_DISTANCE, y+NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); + handleSuccessor(x+node_distance, y+node_distance, x, y, diagonal_node_distance); if(destination_reached){return;} - handleSuccessor(x+NODE_DISTANCE, y-NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); + handleSuccessor(x+node_distance, y-node_distance, x, y, diagonal_node_distance); if(destination_reached){return;} - handleSuccessor(x, y+NODE_DISTANCE, x, y, NODE_DISTANCE); + handleSuccessor(x, y+node_distance, x, y, node_distance); if(destination_reached){return;} - handleSuccessor(x, y-NODE_DISTANCE, x, y, NODE_DISTANCE); + handleSuccessor(x, y-node_distance, x, y, node_distance); if(destination_reached){return;} - handleSuccessor(x-NODE_DISTANCE, y, x, y, NODE_DISTANCE); + handleSuccessor(x-node_distance, y, x, y, node_distance); if(destination_reached){return;} - handleSuccessor(x-NODE_DISTANCE, y+NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); + handleSuccessor(x-node_distance, y+node_distance, x, y, diagonal_node_distance); if(destination_reached){return;} - handleSuccessor(x-NODE_DISTANCE, y-NODE_DISTANCE, x, y, DIAGONAL_NODE_DISTANCE); + handleSuccessor(x-node_distance, y-node_distance, x, y, diagonal_node_distance); } bool PathPlanner::isDestination(float x, float y) { - float margin = NODE_DISTANCE/2; + float margin = node_distance/2; return ((x < end_node.getX()+margin && x>end_node.getX()-margin) && (y < end_node.getY()+margin && y > end_node.getY()-margin)); } bool PathPlanner::isStart(float x, float y) { - float margin = NODE_DISTANCE/1.5; + float margin = node_distance/1.5; return ((x < start_node.getX()+margin && x>start_node.getX()-margin) && (y < start_node.getY()+margin && y > start_node.getY()-margin)); } @@ -310,7 +315,7 @@ bool PathPlanner::isPlanSafe(float current_x, float current_y) { } // Check if current point is the first point of the plan - float margin = NODE_DISTANCE; + float margin = node_distance; if(abs(current_x-simple_plan.begin()->getX()) < margin && abs(current_y-simple_plan.begin()->getY()) < margin){ std::cout << "Remove " << simple_plan.begin()->getX() << ", " @@ -348,7 +353,7 @@ void PathPlanner::resetParameters() { void PathPlanner::removeOldPoints(float current_x, float current_y){ // Check if current point is the first point of the plan - float margin = NODE_DISTANCE; + float margin = node_distance; if(simple_plan.empty()){ return; From 0ef8d6d2ada3199b863b2cad77eb7f639f157e20 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 18:23:42 +0100 Subject: [PATCH 071/200] Fix bugs :snail: --- src/nodes/path_planner_node.cpp | 33 ++++++++++++++------------------- 1 file changed, 14 insertions(+), 19 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index a787155..2c9f243 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -4,7 +4,6 @@ #include "control/tools/drone_handler.hpp" #include #include -#include "ascend_msgs/PathPlannerAction.h" #include "ascend_msgs/GRStateArray.h" #include #include "control/tools/planner_config.hpp" @@ -13,16 +12,12 @@ using control::pathplanner::PathPlanner; -// typedefs - -using ActionServerType = actionlib::SimpleActionServer; - std::list obstacle_coordinates; struct PlannerState{ float current_x, current_y, goal_x, goal_y; - bool make_plan; - bool new_goal; + bool make_plan = false; + bool new_goal = false; }; @@ -66,7 +61,7 @@ int main(int argc, char** argv){ ROS_INFO("Path planner node started"); PlannerState planner_state; - PathPlanner plan; + PathPlanner plan(1.0, 0.4); ros::init(argc, argv, "path_planner_server"); ros::NodeHandle n; @@ -77,12 +72,14 @@ int main(int argc, char** argv){ //ros::Subscriber sub_obstacles = n.subscribe(control::PlannerConfig::obstacle_state_topic, 1, updateObstaclesCB); - ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); + ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); ros::ServiceServer server = n.advertiseService("path_planner_service", boost::bind(&newPlanCB, _1, _2, &planner_state)); ascend_msgs::PointArrayStamped msg; - geometry_msgs::Point points_in_plan; + auto& points_in_plan = msg.points; + // For saving memory + points_in_plan.reserve(20); while(ros::ok()){ @@ -100,11 +97,10 @@ int main(int argc, char** argv){ std::list simple_plan = plan.getSimplePlan(); geometry_msgs::Point point; - // Iterate through the points in the plan and publish to pathplanner-action - ascend_msgs::PathPlannerFeedback feedback; - // For saving memory - feedback.points_in_plan.reserve(20); - points_in_plan.points.reserve(20); + + // Removing old plan + points_in_plan.clear(); + // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan std::list::iterator second_point = ++(simple_plan.begin()); @@ -115,12 +111,11 @@ int main(int argc, char** argv){ point.x = it->getX(); point.y = it->getY(); - points_in_plan.points.push_back(point); + points_in_plan.push_back(point); std::cout << point.x << ", " << point.y << "\t"; } - msg.points = points_in_plan; msg.header.stamp = ros::Time::now(); pub_plan.publish(msg); @@ -130,10 +125,10 @@ int main(int argc, char** argv){ // Publish plan as long as a plan is requested if(planner_state.make_plan){ std::cout << "Published points:\t"; - for(auto it = points_in_plan.points.begin(); it != points_in_plan.points.end(); it++){ + for(auto it = points_in_plan.begin(); it != points_in_plan.end(); it++){ std::cout << it->x << ", " << it->y << "\t" << std::endl; } - pub_plan.publish(points_in_plan); + pub_plan.publish(msg); std::cout << std::endl; } From 438e1855dfb6d6ea85885d8cafc32c3054848d66 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Sun, 25 Feb 2018 18:59:46 +0100 Subject: [PATCH 072/200] Start work on goto state --- include/control/fsm/go_to_state.hpp | 16 ++++++- src/fsm/go_to_state.cpp | 74 +++++++++++++++++++++++++---- 2 files changed, 78 insertions(+), 12 deletions(-) diff --git a/include/control/fsm/go_to_state.hpp b/include/control/fsm/go_to_state.hpp index 6adda33..b91bce3 100644 --- a/include/control/fsm/go_to_state.hpp +++ b/include/control/fsm/go_to_state.hpp @@ -2,14 +2,24 @@ #define GO_TO_STATE_HPP #include "state_interface.hpp" #include -#include +#include #include #include ///Moves drone to XYZ class GoToState : public StateInterface { private: - + ///Get nodehandle, constructed on first use! + static ros::NodeHandle& getNodeHandler(); + ///Path planner ROS service client + ros::ServiceClient path_planner_client_; + ///Path planner plan sub + ros::Subscriber path_planner_sub_; + ///When was plan requested + ros::Time path_requested_stamp_; + ///Last recieved plan + ascend_msgs::PointArrayStamped::ConstPtr last_plan_; + struct { ros::Time started; bool enabled = false; @@ -21,6 +31,8 @@ class GoToState : public StateInterface { ///Is state active flag bool is_active_ = false; + ///Plan callback + void planCB(ascend_msgs::PointArrayStamped::ConstPtr msg_p); public: GoToState(); void stateInit(ControlFSM& fsm) override; diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 12c39f1..2581c0c 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -5,12 +5,19 @@ #include #include "control/tools/config.hpp" #include "control/tools/target_tools.hpp" +#include constexpr double PI = 3.14159265359; constexpr double MAVROS_YAW_CORRECTION_PI_HALF = 3.141592653589793 / 2.0; +//Gurantees construction on first use! +ros::NodeHandle& GoToState::getNodeHandler() { + static ros::NodeHandle n; + return n; +} -GoToState::GoToState() : StateInterface::StateInterface() { - setpoint_.type_mask = default_mask; +GoToState::GoToState() : StateInterface::StateInterface() { + last_plan_ = boost::make_shared(); + setpoint_.type_mask = default_mask; } void GoToState::handleEvent(ControlFSM& fsm, const EventData& event) { @@ -80,17 +87,33 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); return; } - // Set setpoint - setpoint_.position.x = cmd_.position_goal.x; - setpoint_.position.y = cmd_.position_goal.y; - setpoint_.position.z = cmd_.position_goal.z; + //Request new plan + using PathRequest = ascend_msgs::PathPlanner::Request; + Request req; + req.cmd = Request::MAKE_PLAN; + req.x = cmd_.position_goal.x; + req.y = cmd_.position_goal.y; + + if(!path_planner_client_.call(req)) { + control::handleErrorMsg("Couldn't request path plan"); + RequestEvent abort_event(RequestType::ABORT); + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + return; + } + //Change stamp + path_requested_stamp_ = ros::Time::now(); + try { ///Calculate yaw setpoint using control::pose::quat2yaw; using control::getMavrosCorrectedTargetYaw; using control::DroneHandler; - auto quat = DroneHandler::getCurrentPose().pose.orientation; - setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(quat))); + auto pose = DroneHandler::getCurrentPose().pose; + //Hold position while waiting for new plan! + setpoint_.position.x = pose.position.x; + setpoint_.position.y = pose.position.y; + setpoint_.position.z = pose.position.z; + setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(pose.orientation))); } catch(const std::exception& e) { //Critical bug - no recovery //Transition to position hold if no pose available @@ -101,6 +124,12 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { } void GoToState::stateEnd(ControlFSM& fsm, const EventData& event) { + using Request = ascend_msgs::PathPlanner::Request; + Request req; + req.cmd = Request::ABORT; + if(!path_planner_client_.call(req)) { + control::handleErrorMsg("Failed to call path planner service"); + } } void GoToState::loopState(ControlFSM& fsm) { @@ -109,7 +138,28 @@ void GoToState::loopState(ControlFSM& fsm) { if(!control::DroneHandler::isPoseValid()) { throw control::PoseNotValidException(); } + + bool last_plan_timeout = ros::Time::now() - last_plan_.header.stamp > ros::Duration(control::Config::path_plan_timeout); + bool plan_requested_timeout = ros::Time::now() - last_plan_.header.stamp > ros::Duration(control::Config::path_plan_timeout); + //No new valid plan recieved + if(last_plan_timeout || last_plan_.points.empty()) { + if(plan_requested_timeout) { + handleErrorMsg("Missing valid path plan!"); + RequestEvent abort_event(RequestType::ABORT); + if(cmd_.isValidCMD()) { + cmd_.eventError("ABORT"); + cmd_ = EventData(); + } + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + } + return; + } + + auto& target_point = last_plan_.points[0]; + setpoint_.position.x = target_point.x; + setpoint_.position.y = target_point.y; + using control::pose::quat2mavrosyaw; //Get pose auto pose_stamped = control::DroneHandler::getCurrentPose(); @@ -156,10 +206,14 @@ const mavros_msgs::PositionTarget* GoToState::getSetpointPtr() { //Initialize state void GoToState::stateInit(ControlFSM& fsm) { using control::Config; + using ascend_msgs::PointArrayStamped; //TODO Uneccesary variables - Config can be used directly //Set state variables + auto& pc_topic = Config::path_planner_client_topic; + auto& ps_topic = Config::path_planner_plan_topic; delay_transition_.delayTime = ros::Duration(Config::go_to_hold_dest_time); - + path_planner_client = getNodeHandler().serviceClienti(pc_topic); + path_planner_sub_ = getNodeHandler().subscribe(ps_topic, 1, GoToState::planCB, this); control::handleInfoMsg("GoTo init completed!"); } @@ -245,7 +299,7 @@ void GoToState::destinationReached(ControlFSM& fsm) { if(cmd_.isValidCMD()) { cmd_.sendFeedback("Destination reached, letting drone slow down before transitioning!"); - } + } //Delay transition if(ros::Time::now() - delay_transition_.started < delay_transition_.delayTime) { From 9f2ea1fbfda854facdab41b904e7af023d03fb97 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 19:05:09 +0100 Subject: [PATCH 073/200] Add tuning parameters to launch file --- include/control/tools/planner_config.hpp | 5 +++++ launch/path_planner.launch | 4 ++++ src/nodes/path_planner_node.cpp | 14 +++++++------- src/tools/planner_config.cpp | 6 ++++++ 4 files changed, 22 insertions(+), 7 deletions(-) diff --git a/include/control/tools/planner_config.hpp b/include/control/tools/planner_config.hpp index 2138e5a..c5f23fe 100644 --- a/include/control/tools/planner_config.hpp +++ b/include/control/tools/planner_config.hpp @@ -31,6 +31,11 @@ class PlannerConfig { static std::string obstacle_state_topic; static std::string mavros_setpoint_topic; static std::string plan_points_topic; + + // for path planner constructor + static double obstacle_radius; + static double node_distance; + /**Load paramaters * @throw control::ROSNotInitializedException */ diff --git a/launch/path_planner.launch b/launch/path_planner.launch index fb5114e..add34d8 100644 --- a/launch/path_planner.launch +++ b/launch/path_planner.launch @@ -3,10 +3,14 @@ + + + + diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 2c9f243..be1afc5 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -58,16 +58,16 @@ void updateObstaclesCB(ascend_msgs::GRStateArray::ConstPtr msg_p){ int main(int argc, char** argv){ + ros::init(argc, argv, "path_planner_server"); + ros::NodeHandle n; + ros::Rate rate(1); + ROS_INFO("Path planner node started"); - PlannerState planner_state; - PathPlanner plan(1.0, 0.4); + control::PlannerConfig::loadParams(); - ros::init(argc, argv, "path_planner_server"); - ros::NodeHandle n; - ros::Rate rate(1); - - control::PlannerConfig::loadParams(); + PlannerState planner_state; + PathPlanner plan(control::PlannerConfig::obstacle_radius, control::PlannerConfig::node_distance); //ros::Subscriber sub_obstacles = n.subscribe(control::PlannerConfig::obstacle_state_topic, 1, updateObstaclesCB); diff --git a/src/tools/planner_config.cpp b/src/tools/planner_config.cpp index f2641ac..27bc8a7 100644 --- a/src/tools/planner_config.cpp +++ b/src/tools/planner_config.cpp @@ -16,6 +16,9 @@ std::string PlannerConfig::obstacle_state_topic = "/perception_obstacle_states"; std::string PlannerConfig::mavros_setpoint_topic = "/mavros/setpoint_raw/local"; std::string PlannerConfig::plan_points_topic = "/control/planner/plan"; +double PlannerConfig::obstacle_radius = 1.0; +double PlannerConfig::node_distance = 0.4; + void PlannerConfig::loadParams() { if(!ros::isInitialized()) { throw ROSNotInitializedException(); @@ -87,6 +90,9 @@ void PlannerConfig::loadParams() { getStringParam("mavros_setpoint_topic", mavros_setpoint_topic); getStringParam("plan_points_topic", plan_points_topic); + getDoubleParam("obstacle_radius", obstacle_radius,0,3); + getDoubleParam("node_distance", node_distance,0,1); + } using Request = ascend_msgs::ReloadConfig::Request; From 9e13b033a010ac8ab14202642705d058529cdad9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Sun, 25 Feb 2018 20:11:09 +0100 Subject: [PATCH 074/200] Implements path following --- include/control/tools/config.hpp | 7 ++++ src/fsm/go_to_state.cpp | 55 +++++++++++++++++--------------- src/tools/config.cpp | 7 ++++ 3 files changed, 43 insertions(+), 26 deletions(-) diff --git a/include/control/tools/config.hpp b/include/control/tools/config.hpp index 24e760b..37b2314 100644 --- a/include/control/tools/config.hpp +++ b/include/control/tools/config.hpp @@ -83,6 +83,13 @@ class Config { static bool require_obstacle_detection; /// \fsmparam When is data considered old? static double valid_data_timeout; + /// \fsmparam How long to wait for path planner + static double path_plan_timeout; + /// \fsmparam Topic for planner plan sub + static std::string path_planner_plan_topic; + /// \fsmparam Topic for planner service client + static std::string path_planner_client_topic; + /**Load paramaters * @throw control::ROSNotInitializedException */ diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 2581c0c..09984bd 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -16,8 +16,9 @@ ros::NodeHandle& GoToState::getNodeHandler() { } GoToState::GoToState() : StateInterface::StateInterface() { - last_plan_ = boost::make_shared(); - setpoint_.type_mask = default_mask; + using ascend_msgs::PointArrayStamped; + last_plan_ = ascend_msgs::PointArrayStamped::ConstPtr(new ascend_msgs::PointArrayStamped);; + setpoint_.type_mask = default_mask; } void GoToState::handleEvent(ControlFSM& fsm, const EventData& event) { @@ -88,11 +89,11 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { return; } //Request new plan - using PathRequest = ascend_msgs::PathPlanner::Request; - Request req; - req.cmd = Request::MAKE_PLAN; - req.x = cmd_.position_goal.x; - req.y = cmd_.position_goal.y; + using PathService = ascend_msgs::PathPlanner; + PathService req; + req.request.cmd = PathService::Request::MAKE_PLAN; + req.request.goal_x = cmd_.position_goal.x; + req.request.goal_y = cmd_.position_goal.y; if(!path_planner_client_.call(req)) { control::handleErrorMsg("Couldn't request path plan"); @@ -124,9 +125,9 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { } void GoToState::stateEnd(ControlFSM& fsm, const EventData& event) { - using Request = ascend_msgs::PathPlanner::Request; + using Request = ascend_msgs::PathPlanner; Request req; - req.cmd = Request::ABORT; + req.request.cmd = Request::Request::ABORT; if(!path_planner_client_.call(req)) { control::handleErrorMsg("Failed to call path planner service"); } @@ -139,13 +140,13 @@ void GoToState::loopState(ControlFSM& fsm) { throw control::PoseNotValidException(); } - bool last_plan_timeout = ros::Time::now() - last_plan_.header.stamp > ros::Duration(control::Config::path_plan_timeout); - bool plan_requested_timeout = ros::Time::now() - last_plan_.header.stamp > ros::Duration(control::Config::path_plan_timeout); + bool last_plan_timeout = ros::Time::now() - last_plan_->header.stamp > ros::Duration(control::Config::path_plan_timeout); + bool plan_requested_timeout = ros::Time::now() - last_plan_->header.stamp > ros::Duration(control::Config::path_plan_timeout); //No new valid plan recieved - if(last_plan_timeout || last_plan_.points.empty()) { + if(last_plan_timeout || last_plan_->points.empty()) { if(plan_requested_timeout) { - handleErrorMsg("Missing valid path plan!"); + control::handleErrorMsg("Missing valid path plan!"); RequestEvent abort_event(RequestType::ABORT); if(cmd_.isValidCMD()) { cmd_.eventError("ABORT"); @@ -156,7 +157,7 @@ void GoToState::loopState(ControlFSM& fsm) { return; } - auto& target_point = last_plan_.points[0]; + auto& target_point = last_plan_->points[0]; setpoint_.position.x = target_point.x; setpoint_.position.y = target_point.y; @@ -203,17 +204,20 @@ const mavros_msgs::PositionTarget* GoToState::getSetpointPtr() { return &setpoint_; } +void GoToState::planCB(ascend_msgs::PointArrayStamped::ConstPtr msg_p) { + last_plan_ = msg_p; +} + //Initialize state void GoToState::stateInit(ControlFSM& fsm) { using control::Config; using ascend_msgs::PointArrayStamped; - //TODO Uneccesary variables - Config can be used directly - //Set state variables + using ascend_msgs::PathPlanner; auto& pc_topic = Config::path_planner_client_topic; auto& ps_topic = Config::path_planner_plan_topic; delay_transition_.delayTime = ros::Duration(Config::go_to_hold_dest_time); - path_planner_client = getNodeHandler().serviceClienti(pc_topic); - path_planner_sub_ = getNodeHandler().subscribe(ps_topic, 1, GoToState::planCB, this); + path_planner_client_ = getNodeHandler().serviceClient(pc_topic); + path_planner_sub_ = getNodeHandler().subscribe(ps_topic, 1, &GoToState::planCB, this); control::handleInfoMsg("GoTo init completed!"); } @@ -296,17 +300,16 @@ void GoToState::destinationReached(ControlFSM& fsm) { if(!delay_transition_.enabled) { delay_transition_.started = ros::Time::now(); delay_transition_.enabled = true; - if(cmd_.isValidCMD()) { cmd_.sendFeedback("Destination reached, letting drone slow down before transitioning!"); - - } - //Delay transition - if(ros::Time::now() - delay_transition_.started < delay_transition_.delayTime) { - return; + } + //Delay transition + if(ros::Time::now() - delay_transition_.started < delay_transition_.delayTime) { + return; + } + //If all checks passed - land! + fsm.transitionTo(ControlFSM::LAND_STATE, this, cmd_); } - //If all checks passed - land! - fsm.transitionTo(ControlFSM::LAND_STATE, this, cmd_); } else { //If drone is moving, reset delayed transition delay_transition_.enabled = false; diff --git a/src/tools/config.cpp b/src/tools/config.cpp index 8d0ae51..2fdb9cc 100644 --- a/src/tools/config.cpp +++ b/src/tools/config.cpp @@ -32,6 +32,8 @@ std::string Config::mavros_local_pos_topic = "mavros/local_position/pose"; std::string Config::mavros_state_changed_topic = "mavros/state"; std::string Config::land_detector_topic = "/landdetector"; std::string Config::obstacle_state_topic = "/perception_obstacle_states"; +std::string Config::path_planner_plan_topic = "/control/planner/plan"; +std::string Config::path_planner_client_topic = "/control/planner/client"; int Config::fsm_status_buffer_size = 10; double Config::go_to_hold_dest_time = 0.5; double Config::safe_hover_altitude = 2.0; @@ -39,6 +41,7 @@ double Config::obstacle_too_close_dist = 2.0; std::string Config::lidar_topic = "perception/obstacles/lidar"; bool Config::require_all_data_streams = true; bool Config::require_obstacle_detection = true; +double Config::path_plan_timeout = 1.0; void Config::loadParams() { if(!ros::isInitialized()) { @@ -138,6 +141,10 @@ void Config::loadParams() { getStringParam("land_detector_topic", land_detector_topic); //Obstacles getStringParam("obstacle_state_topic", obstacle_state_topic); + //Path planner + getDoubleParam("path_plan_timeout", path_plan_timeout, 0.0, 5.0); + getStringParam("path_planner_client_topic", path_planner_client_topic); + getStringParam("path_planner_plan_topic", path_planner_plan_topic); } From 275bb6028b985030b98c6a769cf5ba7e9b9f5ded Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Sun, 25 Feb 2018 20:18:35 +0100 Subject: [PATCH 075/200] Refactor type names --- src/fsm/go_to_state.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 09984bd..6f93e52 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -89,11 +89,11 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { return; } //Request new plan - using PathService = ascend_msgs::PathPlanner; - PathService req; - req.request.cmd = PathService::Request::MAKE_PLAN; - req.request.goal_x = cmd_.position_goal.x; - req.request.goal_y = cmd_.position_goal.y; + using PathServiceRequest = ascend_msgs::PathPlanner; + PathServiceRequest req; + req.request.cmd = PathServiceRequest::Request::MAKE_PLAN; + req.request.goal_x = static_cast(cmd_.position_goal.x); + req.request.goal_y = static_cast(cmd_.position_goal.y); if(!path_planner_client_.call(req)) { control::handleErrorMsg("Couldn't request path plan"); @@ -125,9 +125,9 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { } void GoToState::stateEnd(ControlFSM& fsm, const EventData& event) { - using Request = ascend_msgs::PathPlanner; - Request req; - req.request.cmd = Request::Request::ABORT; + using PathServiceRequest = ascend_msgs::PathPlanner; + PathServiceRequest req; + req.request.cmd = PathServiceRequest::Request::ABORT; if(!path_planner_client_.call(req)) { control::handleErrorMsg("Failed to call path planner service"); } From 798ac51e9ae3ce8c468d3b303b40fd2e7408df86 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 20:19:47 +0100 Subject: [PATCH 076/200] Add unit test for path planner indexing --- CMakeLists.txt | 9 ++++++--- test/utest.cpp | 21 +++++++++++++++++++-- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2068d4e..15f0f65 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,9 @@ target_link_libraries(control_fsm_lib add_executable(control_fsm_main src/nodes/control_fsm_main) add_executable(action_interface_client src/nodes/action_interface_client.cpp) -add_executable(path_planner_node src/nodes/path_planner_node src/path_planner/path_planner src/path_planner/node) +add_library(path_planner_lib src/path_planner/path_planner src/path_planner/node) + +add_executable(path_planner_node src/nodes/path_planner_node) add_executable(test_path_planner_node src/nodes/test_path_planner_node) @@ -77,7 +79,8 @@ target_link_libraries(action_interface_client ${catkin_LIBRARIES}) target_link_libraries(path_planner_node ${catkin_LIBRARIES} - control_tools_lib) + control_tools_lib + path_planner_lib) target_link_libraries(test_path_planner_node ${catkin_LIBRARIES} @@ -92,6 +95,6 @@ add_dependencies(action_interface_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${c if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(utest test/launch_test.test test/utest.cpp) - target_link_libraries(utest control_fsm_lib) + target_link_libraries(utest control_fsm_lib path_planner_lib) roslaunch_add_file_check(launch) endif() diff --git a/test/utest.cpp b/test/utest.cpp index 50ecdb9..c5dbff7 100644 --- a/test/utest.cpp +++ b/test/utest.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include "gtest/gtest.h" #include @@ -75,9 +76,25 @@ TEST(ControlTest, quatConversionTest) { } +TEST(PathPlannerTest, coordToIndexTest) { + control::pathplanner::PathPlanner obj1(1,0.4); + EXPECT_EQ(obj1.coordToIndex(0.0), 0); + EXPECT_EQ(obj1.coordToIndex(0.9), 2); + EXPECT_EQ(obj1.coordToIndex(0.7999), 2); + EXPECT_EQ(obj1.coordToIndex(0.5999), 1); + EXPECT_EQ(obj1.coordToIndex(0.6), 2); + + control::pathplanner::PathPlanner obj2(1, 0.5); + EXPECT_EQ(obj2.coordToIndex(0.0), 0); + EXPECT_EQ(obj2.coordToIndex(0.5), 1); + EXPECT_EQ(obj2.coordToIndex(0.2499), 0); + EXPECT_EQ(obj2.coordToIndex(10), 20); + EXPECT_EQ(obj2.coordToIndex(0.4999), 1); + +} + int main(int argc, char** argv) { ros::init(argc, argv, "control_fsm_unit_test"); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} - +} \ No newline at end of file From 121db3430d248d14592ca68aa966b30af986bab3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Sun, 25 Feb 2018 20:23:14 +0100 Subject: [PATCH 077/200] Adds ros params --- control_fsm_default_config.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index 9f7d5dd..d2d4b10 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -54,5 +54,11 @@ #Topic for recieving obstacle state data obstacle_state_topic: perception/obstacles, #Margin to determine if velocity is reached - velocity_reached_margin: 0.2 + velocity_reached_margin: 0.2, + #Path planner client topic + path_planner_client_topic: /control/planner/client, + #Path planner plan topic + path_planner_plan_topic: /control/planner/plan, + #Path planner timeout + path_plan_timeout: 1.0 } From 572c5b931147d8b854831c568c96bdf87b38dc2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Sun, 25 Feb 2018 20:24:49 +0100 Subject: [PATCH 078/200] Remove uneccesary semicolon --- src/fsm/go_to_state.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 6f93e52..05f26de 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -17,7 +17,7 @@ ros::NodeHandle& GoToState::getNodeHandler() { GoToState::GoToState() : StateInterface::StateInterface() { using ascend_msgs::PointArrayStamped; - last_plan_ = ascend_msgs::PointArrayStamped::ConstPtr(new ascend_msgs::PointArrayStamped);; + last_plan_ = ascend_msgs::PointArrayStamped::ConstPtr(new ascend_msgs::PointArrayStamped); setpoint_.type_mask = default_mask; } From d1c0dc1b3c69d7c26f15fa4ffb35bea85a3c394a Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 22:15:10 +0100 Subject: [PATCH 079/200] Send request type in client call --- src/nodes/test_path_planner_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/nodes/test_path_planner_node.cpp b/src/nodes/test_path_planner_node.cpp index e83ea0e..dbad261 100644 --- a/src/nodes/test_path_planner_node.cpp +++ b/src/nodes/test_path_planner_node.cpp @@ -5,6 +5,8 @@ int main(int argc, char** argv){ ROS_INFO("Test path planner node started"); + + using Request = ascend_msgs::PathPlanner::Request; ros::init(argc, argv, "path_planner_test"); ros::NodeHandle n; @@ -14,6 +16,7 @@ int main(int argc, char** argv){ ascend_msgs::PathPlanner clientCall; + clientCall.request.cmd = Request::MAKE_PLAN; clientCall.request.goal_x = 1; clientCall.request.goal_y = 1; From 394411c4babe2bfc80829f7356c2ddf9a20616a9 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 22:21:22 +0100 Subject: [PATCH 080/200] Add request types in callback function --- src/nodes/path_planner_node.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index be1afc5..846772a 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -27,22 +27,30 @@ using Response = ascend_msgs::PathPlanner::Response; bool newPlanCB(Request &req, Response &res, PlannerState* planner_state){ - ROS_INFO("newPlanCB"); - - planner_state->make_plan = true; + if(req.cmd == Request::ABORT){ + ROS_INFO("Service callback: abort"); + planner_state->make_plan = false; + } + else if(req.cmd == Request::MAKE_PLAN){ + ROS_INFO("Service callback: make new plan"); - // Update planner state - geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); - auto& position = current_pose.pose.position; - planner_state->current_x = position.x; - planner_state->current_y = position.y; + planner_state->make_plan = true; - //Accept the new goal - planner_state->new_goal = true; + // Update planner state + geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); + auto& position = current_pose.pose.position; + planner_state->current_x = position.x; + planner_state->current_y = position.y; - planner_state->goal_x = req.goal_x; - planner_state->goal_y = req.goal_y; + //Accept the new goal + planner_state->new_goal = true; + planner_state->goal_x = req.goal_x; + planner_state->goal_y = req.goal_y; + } + else{ + ROS_INFO("Service callback: not relevant"); + } return true; } From ab196a53832fb9e7c21eda957d260926b09dc831 Mon Sep 17 00:00:00 2001 From: atussa Date: Sun, 25 Feb 2018 22:22:39 +0100 Subject: [PATCH 081/200] Remove unneccesary code --- src/path_planner/path_planner.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 7431505..239de79 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -228,11 +228,6 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo int x_index = coordToIndex(x); int y_index = coordToIndex(y); - if(graph[x_index][y_index].obstacle){ - plan.push_front(start_node); - return; - } - // Calculate all f values and set the parents relaxGraph(); @@ -314,15 +309,6 @@ bool PathPlanner::isPlanSafe(float current_x, float current_y) { return false; } - // Check if current point is the first point of the plan - float margin = node_distance; - - if(abs(current_x-simple_plan.begin()->getX()) < margin && abs(current_y-simple_plan.begin()->getY()) < margin){ - std::cout << "Remove " << simple_plan.begin()->getX() << ", " - << simple_plan.begin()->getY() << std::endl; - simple_plan.pop_front(); - } - std::list::iterator current = simple_plan.begin(); std::list::iterator next = ++(simple_plan.begin()); From a745ee39b6573cfa298ac371d6e88ca6f92b0683 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Tue, 27 Feb 2018 18:22:28 +0100 Subject: [PATCH 082/200] Adds on warn callback --- include/control/tools/obstacle_avoidance.hpp | 21 +++++++++++++++----- src/tools/obstacle_avoidance.cpp | 17 +++++++++++++++- 2 files changed, 32 insertions(+), 6 deletions(-) diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index 5d594e6..b6f76ba 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -11,14 +11,22 @@ namespace control { class ObstacleAvoidance; class ObstacleAvoidance { private: - /**Set of callback method pointers registered by states + /**Set of modified callback method pointers registered by states * Pointers are used to allow comparison as std::function can't be compared */ std::set< std::shared_ptr< std::function > > on_modified_cb_set_; + + /**Set of warn callback method pointers registered by states + * Pointers are used to allow comparison as std::function can't be compared + */ + std::set< std::shared_ptr< std::function > > on_warn_cb_set_; ///Notify states the setpoint has been changed void onModified(); - + + ///Notify all states setpoint is unsafe + void onWarn(); + ///Runs obstacle avoidance, and modifies setpoint (and notifies) virtual bool doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint); @@ -27,15 +35,18 @@ class ObstacleAvoidance { ObstacleAvoidance() = default; ///Default copy constructor ObstacleAvoidance(const ObstacleAvoidance&) = default; - ///Add new callback + ///Add new on warn callback + void registerOnWarnCBPtr(const std::shared_ptr >& cb_p) { on_warn_cb_set_.insert(cb_p); } + ///Remove on warn callback + void removeOnWarnCBPtr(const std::shared_ptr >& cb_p); + ///Add new on modified callback void registerOnModifiedCBPtr(const std::shared_ptr >& cb_p) { on_modified_cb_set_.insert(cb_p); } - ///Remove a callback + ///Remove on modified callback void removeOnModifiedCBPtr(const std::shared_ptr >& cb_p); ///Modifies setpoint if obstacle is too close mavros_msgs::PositionTarget run(mavros_msgs::PositionTarget setpoint); ///Returns true when obstacle avoidance is running bool isReady() { return true; } - }; // Only do this for unit testing diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 7f9fa85..42081a1 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -133,7 +133,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget // magic numbers are minimum clearance no matter what. Turn into parameters and include // this logic in the avoidZone function - ROS_INFO_THROTTLE("Drone speed ratio: %.3f", drone_speed_ratio); + ROS_INFO_THROTTLE(1, "Drone speed ratio: %.3f", drone_speed_ratio); const auto clearance_front = std::max((float)Config::obstacle_clearance_front*drone_speed_ratio, 1.f); const auto clearance_back = std::max((float)Config::obstacle_clearance_back*drone_speed_ratio, 1.f); const auto clearance_side = std::max((float)Config::obstacle_clearance_side*drone_speed_ratio, 1.f); @@ -195,11 +195,19 @@ void control::ObstacleAvoidance::onModified() { } } +void control::ObstacleAvoidance::onWarn() { + //Run all callbacks + for(auto& cb_p : on_warn_cb_set_ ) { + (*cb_p)(); + } +} + mavros_msgs::PositionTarget control::ObstacleAvoidance::run(mavros_msgs::PositionTarget setpoint) { //If obstacle avoidance has altered the setpoint if(doObstacleAvoidance(&setpoint)) { //Notify states onModified(); + onWarn(); } return setpoint; } @@ -211,3 +219,10 @@ void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr >& cb_p) { + const auto it = on_warn_cb_set_.find(cb_p); + if (it != on_warn_cb_set_.cend()) { + on_warn_cb_set_.erase(it); + } +} + From 6f0ece68b8608fae5d76ce101bca8b439dff2a2d Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 12:57:50 +0100 Subject: [PATCH 083/200] Add initializing loop --- src/nodes/path_planner_node.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 846772a..6af4641 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -89,6 +89,11 @@ int main(int argc, char** argv){ // For saving memory points_in_plan.reserve(20); + while(!control::DroneHandler::isPoseValid() && ros::ok()){ + ros::spinOnce(); + ros::Duration(1.0).sleep(); + } + while(ros::ok()){ ros::spinOnce(); From 66e4c7384987626662f1f74accf1647f588f8886 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 12:58:41 +0100 Subject: [PATCH 084/200] Simplify iterator incrementing --- src/path_planner/path_planner.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 239de79..0e4dfc1 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -291,8 +291,7 @@ void PathPlanner::simplifyPlan() { // (no use in going through it when we can go in a straight line) // The search continues with same starting point, but the second and third point is changed else{ - simple_plan.erase(second); - second = third; + second = simple_plan.erase(second); third++; } } From 440762916098f03ed5859d214a14c2c2c3ac2fd9 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 1 Mar 2018 14:04:11 +0100 Subject: [PATCH 085/200] Start unit testing --- CMakeLists.txt | 2 + include/control/tools/obstacle_avoidance.hpp | 2 +- test/utest.cpp | 53 ++++++++++++++++++++ 3 files changed, 56 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3a10d34..0522583 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,5 +81,7 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(utest test/launch_test.test test/utest.cpp) target_link_libraries(utest control_fsm_lib) + target_link_libraries(utest ${catkin_LIBRARIS}) roslaunch_add_file_check(launch) endif() + diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index 5d594e6..05ee91d 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -37,6 +37,7 @@ class ObstacleAvoidance { bool isReady() { return true; } }; +} // Only do this for unit testing #ifdef CONTROL_FSM_UNIT_TEST @@ -56,6 +57,5 @@ inline float calcAngleToObstacle(const T& point, const K& obstacle_position, con template inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle); #endif -} #endif diff --git a/test/utest.cpp b/test/utest.cpp index 50ecdb9..555e962 100644 --- a/test/utest.cpp +++ b/test/utest.cpp @@ -7,9 +7,12 @@ #define CONTROL_FSM_UNIT_TEST #endif +#include + #include #include #include +#include #include "gtest/gtest.h" #include @@ -72,9 +75,59 @@ TEST(ControlTest, quatConversionTest) { EXPECT_NEAR(PI, quat2yaw(pi), 0.001); EXPECT_NEAR(PI / 3.0, quat2yaw(pi3), 0.001); EXPECT_NEAR(0.0 - PI_HALF, quat2mavrosyaw(zero), 0.001); +} + +TEST(ControlTest, obstacleAvoidanceHelpers) { + + // angleWrapper + EXPECT_NEAR(0.f, angleWrapper(0.f), 0.001); + EXPECT_NEAR(2.f, angleWrapper(2.f), 0.001); + EXPECT_NEAR(4.f, angleWrapper(4.f), 0.001); + EXPECT_NEAR(0.7168f, angleWrapper(7.f), 0.001); + EXPECT_NEAR(2.8673f, angleWrapper(28.f), 0.001); + EXPECT_NEAR(5.2832f, angleWrapper(-1.f), 0.001); + EXPECT_NEAR(0.8496f, angleWrapper(-18.f), 0.001); + + geometry_msgs::Vector3 vec = {}; + geometry_msgs::Vector3 zero = {}; + geometry_msgs::Vector3 unitx; unitx.x=1.0; unitx.y=0.0; unitx.z=0.0; + geometry_msgs::Vector3 unity; unity.x=0.0; unity.y=1.0; unity.z=0.0; + geometry_msgs::Vector3 unitz; unitz.x=0.0; unitz.y=0.0; unitz.z=1.0; + + // rotateXY + geometry_msgs::Vector3 result = rotateXY(unitx, PI/2); + EXPECT_NEAR(unity.x, result.x, 0.001); + EXPECT_NEAR(unity.y, result.y, 0.001); + EXPECT_NEAR(unity.z, result.z, 0.001); + + result = rotateXY(unitx, PI); + EXPECT_NEAR(-unitx.x, result.x, 0.001); + EXPECT_NEAR(-unitx.y, result.y, 0.001); + EXPECT_NEAR(-unitx.z, result.z, 0.001); + + result = rotateXY(unity, 3*PI/2); + EXPECT_NEAR(unitx.x, result.x, 0.001); + EXPECT_NEAR(unitx.y, result.y, 0.001); + EXPECT_NEAR(unitx.z, result.z, 0.001); + + result = rotateXY(unitz, 10.0); + EXPECT_NEAR(unitz.x, result.x, 0.001); + EXPECT_NEAR(unitz.y, result.y, 0.001); + EXPECT_NEAR(unitz.z, result.z, 0.001); + + vec.x=2.0; vec.y=4.0; vec.z=3.0; + result = rotateXY(vec, 12.3); + EXPECT_NEAR(2.9824, result.x, 0.001); + EXPECT_NEAR(3.3325, result.y, 0.001); + EXPECT_NEAR(3.0, result.z, 0.001); + + // calcVectorBetweenPoints + } + + int main(int argc, char** argv) { ros::init(argc, argv, "control_fsm_unit_test"); testing::InitGoogleTest(&argc, argv); From bd688d6dd18b543369fe9030952ca607d920d89c Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 17:22:28 +0100 Subject: [PATCH 086/200] Add safety check for iterator --- src/nodes/path_planner_node.cpp | 38 +++++++++++++++++---------------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 6af4641..13d89b3 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -114,25 +114,27 @@ int main(int argc, char** argv){ // Removing old plan points_in_plan.clear(); - - // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan - std::list::iterator second_point = ++(simple_plan.begin()); - - std::cout << "Published points:\t"; - for(std::list::iterator it = second_point; it != simple_plan.end(); it++){ - - point.x = it->getX(); - point.y = it->getY(); - - points_in_plan.push_back(point); - - std::cout << point.x << ", " << point.y << "\t"; - + if(!simple_plan.empty()){ + + // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan + std::list::iterator second_point = ++(simple_plan.begin()); + + std::cout << "Published points:\t"; + for(std::list::iterator it = second_point; it != simple_plan.end(); it++){ + + point.x = it->getX(); + point.y = it->getY(); + + points_in_plan.push_back(point); + + std::cout << point.x << ", " << point.y << "\t"; + + } + msg.header.stamp = ros::Time::now(); + pub_plan.publish(msg); + + std::cout << std::endl; } - msg.header.stamp = ros::Time::now(); - pub_plan.publish(msg); - - std::cout << std::endl; } // Publish plan as long as a plan is requested From 46750799fa7c34ab36aeffcdbc582b4efa309e72 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 17:22:52 +0100 Subject: [PATCH 087/200] Simplify iterator code --- src/path_planner/path_planner.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 0e4dfc1..61f05c5 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -126,7 +126,8 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent graph[x_index][y_index].setParentY(parent_y); destination_reached = true; std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; - std::cout << "Destination parent: x=" << graph[x_index][y_index].getParentX() << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; + std::cout << "Destination parent: x=" << graph[x_index][y_index].getParentX() + << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; } // If node is not destination and hasn't been searched yet else if(!graph[x_index][y_index].closed){ @@ -265,12 +266,16 @@ void PathPlanner::simplifyPlan() { simple_plan = plan; // Pointing at the three first elements std::list::iterator first = simple_plan.begin(); - std::list::iterator second = simple_plan.begin(); - std::list::iterator third = simple_plan.begin(); - second++; - third++; + std::list::iterator second = ++(simple_plan.begin()); + std::list::iterator third = ++(simple_plan.begin()); third++; + if(first == simple_plan.end()){ + return; + } else if(second == simple_plan.end()){ + return; + } + float x1, x2, y1, y2; while(third != simple_plan.end()){ From 5d73a8974fffbd4a23bbd1811ed48f9d2222cf8f Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 17:41:38 +0100 Subject: [PATCH 088/200] Remove unused inlcude --- include/control/planner/path_planner.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index d829ec7..bc0e171 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -2,7 +2,6 @@ #define PATH_PLANNER_HPP #include -#include #include "control/planner/node.hpp" #include "control/tools/control_pose.hpp" #include From 5a1982372b2a4b4bfb1819bb4f305fe93962c642 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 17:42:08 +0100 Subject: [PATCH 089/200] Print error messages --- src/nodes/path_planner_node.cpp | 2 +- src/path_planner/path_planner.cpp | 9 +++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 13d89b3..d1b4329 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -68,7 +68,7 @@ int main(int argc, char** argv){ ros::init(argc, argv, "path_planner_server"); ros::NodeHandle n; - ros::Rate rate(1); + ros::Rate rate(5); ROS_INFO("Path planner node started"); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 61f05c5..459d756 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -205,11 +205,11 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // If start or end point is not valid, a plan is not created if(!isValidCoordinate(current_x,current_y)){ - // send error to fsm + ROS_INFO("Current point invalid!"); return; } if(!isValidCoordinate(target_x,target_y)){ - // send error to fsm + ROS_INFO("Target point invalid!"); return; } @@ -229,6 +229,11 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo int x_index = coordToIndex(x); int y_index = coordToIndex(y); + if(graph[x_index][y_index].obstacle || graph[coordToIndex(current_x)][coordToIndex(current_y)].obstacle){ + std::cout << "Start or end on obstacle, invalid!" << std::endl; + return; + } + // Calculate all f values and set the parents relaxGraph(); From a135a867a9585370bc28302d32d204f238da97cd Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 21:20:13 +0100 Subject: [PATCH 090/200] Improve isdestination and isstart --- src/path_planner/path_planner.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 459d756..baad54f 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -8,7 +8,7 @@ PathPlanner::PathPlanner(float obstacle_radius, float node_distance) : obstacle_radius(obstacle_radius), node_distance(node_distance){ diagonal_node_distance = sqrt(2*node_distance*node_distance); // plus 1 to include both 0 and the last node - graph_size = round(FIELD_LENGTH/node_distance+1); + graph_size = ceil(FIELD_LENGTH/node_distance+1); graph.resize(graph_size, std::vector(graph_size)); } @@ -167,15 +167,13 @@ void PathPlanner::handleAllSuccessors(float x, float y) { } bool PathPlanner::isDestination(float x, float y) { - float margin = node_distance/2; - return ((x < end_node.getX()+margin && x>end_node.getX()-margin) - && (y < end_node.getY()+margin && y > end_node.getY()-margin)); + return (coordToIndex(x) == coordToIndex(end_node.getX()) && coordToIndex(y) == coordToIndex(end_node.getY())); + } bool PathPlanner::isStart(float x, float y) { - float margin = node_distance/1.5; - return ((x < start_node.getX()+margin && x>start_node.getX()-margin) - && (y < start_node.getY()+margin && y > start_node.getY()-margin)); + return (coordToIndex(x) == coordToIndex(start_node.getX()) && coordToIndex(y) == coordToIndex(start_node.getY())); + } bool PathPlanner::isValidCoordinate(float x, float y) { From 14f1d1174ec6406c7aade80d7c9d6ee22654d306 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 21:49:20 +0100 Subject: [PATCH 091/200] Stamp even though new plan is not made --- src/nodes/path_planner_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index d1b4329..1f6549b 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -143,6 +143,7 @@ int main(int argc, char** argv){ for(auto it = points_in_plan.begin(); it != points_in_plan.end(); it++){ std::cout << it->x << ", " << it->y << "\t" << std::endl; } + msg.header.stamp = ros::Time::now(); pub_plan.publish(msg); std::cout << std::endl; } From e754f2092183f6d4c923c84772577e2488c7ef5e Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 1 Mar 2018 21:58:17 +0100 Subject: [PATCH 092/200] dont publish plan more thannecessary --- src/nodes/path_planner_node.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 1f6549b..fd1e926 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -130,9 +130,6 @@ int main(int argc, char** argv){ std::cout << point.x << ", " << point.y << "\t"; } - msg.header.stamp = ros::Time::now(); - pub_plan.publish(msg); - std::cout << std::endl; } } From 8a97d394624929f935816f2e3611bd24d094a8b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 1 Mar 2018 22:25:22 +0100 Subject: [PATCH 093/200] Fix timeout bug --- src/fsm/go_to_state.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 7684cef..e1455ca 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -141,7 +141,7 @@ void GoToState::loopState(ControlFSM& fsm) { } bool last_plan_timeout = ros::Time::now() - last_plan_->header.stamp > ros::Duration(control::Config::path_plan_timeout); - bool plan_requested_timeout = ros::Time::now() - last_plan_->header.stamp > ros::Duration(control::Config::path_plan_timeout); + bool plan_requested_timeout = ros::Time::now() - path_requested_stamp_ > ros::Duration(control::Config::path_plan_timeout); //No new valid plan recieved if(last_plan_timeout || last_plan_->points.empty()) { @@ -160,6 +160,7 @@ void GoToState::loopState(ControlFSM& fsm) { auto& target_point = last_plan_->points[0]; setpoint_.position.x = target_point.x; setpoint_.position.y = target_point.y; + setpoint_.position.z = cmd_.position_goal.z; using control::pose::quat2mavrosyaw; //Get pose From 10c14cd1b88ffd3ad0a0ce7b0e50668f4cbf09fd Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 10:21:20 +0100 Subject: [PATCH 094/200] Return plan with specific end coordinate --- src/path_planner/path_planner.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index baad54f..43a2178 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -240,8 +240,12 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // Go through the graph from end to start through the parents of each node // Add nodes to the plan + // Actual endpoint + plan.push_front(end_node); + while(!isStart(x, y)){ - plan.push_front(graph[x_index][y_index]); + // parent is calculated from the endpoint index, this node might have a different + // coordinate (depending on the grid size) float x_temp = graph[x_index][y_index].getParentX(); float y_temp = graph[x_index][y_index].getParentY(); x = x_temp; @@ -252,9 +256,9 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo counter ++; if(counter >200){break;} + plan.push_front(graph[x_index][y_index]); } - // Add start node to the plan (might not be necessary) - plan.push_front(graph[x_index][y_index]); + // Print the plan - can be removed /*std::cout << std::endl << "Whole plan:" << std::endl; for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ From 95ca69028cf3c41a3201c5a58b9b0ca9a0a48245 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Wed, 7 Mar 2018 10:54:40 +0100 Subject: [PATCH 095/200] Adds sim test launch --- control_fsm_default_config.yaml | 2 +- launch/test_control_fsm_sim_planner.launch | 13 +++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) create mode 100644 launch/test_control_fsm_sim_planner.launch diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index d2d4b10..0e3de40 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -56,7 +56,7 @@ #Margin to determine if velocity is reached velocity_reached_margin: 0.2, #Path planner client topic - path_planner_client_topic: /control/planner/client, + path_planner_client_topic: /path_planner_service, #Path planner plan topic path_planner_plan_topic: /control/planner/plan, #Path planner timeout diff --git a/launch/test_control_fsm_sim_planner.launch b/launch/test_control_fsm_sim_planner.launch new file mode 100644 index 0000000..5cd6482 --- /dev/null +++ b/launch/test_control_fsm_sim_planner.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + From ad04307cc35765953f32341e002852140a3b7b67 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 11:30:28 +0100 Subject: [PATCH 096/200] Fix define --- include/control/tools/planner_config.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control/tools/planner_config.hpp b/include/control/tools/planner_config.hpp index c5f23fe..6fc43d2 100644 --- a/include/control/tools/planner_config.hpp +++ b/include/control/tools/planner_config.hpp @@ -1,5 +1,5 @@ -#ifndef FSM_CONFIG_HPP -#define FSM_CONFIG_HPP +#ifndef FSM_PLANNER_CONFIG_HPP +#define FSM_PLANNER_CONFIG_HPP #include #include #include From 1c6ca20303837b8247d53dda7eb10f53c5d25447 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 11:31:00 +0100 Subject: [PATCH 097/200] Get obstacles from obstacle state handler --- src/nodes/path_planner_node.cpp | 28 +++++++++++----------------- 1 file changed, 11 insertions(+), 17 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index fd1e926..b3f61a1 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -9,11 +9,10 @@ #include "control/tools/planner_config.hpp" #include "ascend_msgs/PointArrayStamped.h" #include "ascend_msgs/PathPlanner.h" +#include "control/tools/obstacle_state_handler.hpp" using control::pathplanner::PathPlanner; -std::list obstacle_coordinates; - struct PlannerState{ float current_x, current_y, goal_x, goal_y; bool make_plan = false; @@ -55,15 +54,6 @@ bool newPlanCB(Request &req, Response &res, PlannerState* planner_state){ } -void updateObstaclesCB(ascend_msgs::GRStateArray::ConstPtr msg_p){ - obstacle_coordinates.clear(); - for(auto it = msg_p->states.begin(); it != msg_p->states.end(); ++it) { - obstacle_coordinates.push_back(it->x); - obstacle_coordinates.push_back(it->y); - } -} - - int main(int argc, char** argv){ ros::init(argc, argv, "path_planner_server"); @@ -77,11 +67,9 @@ int main(int argc, char** argv){ PlannerState planner_state; PathPlanner plan(control::PlannerConfig::obstacle_radius, control::PlannerConfig::node_distance); - - //ros::Subscriber sub_obstacles = n.subscribe(control::PlannerConfig::obstacle_state_topic, 1, updateObstaclesCB); + std::list obstacle_coordinates; ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); - ros::ServiceServer server = n.advertiseService("path_planner_service", boost::bind(&newPlanCB, _1, _2, &planner_state)); ascend_msgs::PointArrayStamped msg; @@ -89,7 +77,7 @@ int main(int argc, char** argv){ // For saving memory points_in_plan.reserve(20); - while(!control::DroneHandler::isPoseValid() && ros::ok()){ + while(!control::DroneHandler::isPoseValid() && control::ObstacleStateHandler::isInstanceReady() && ros::ok()){ ros::spinOnce(); ros::Duration(1.0).sleep(); } @@ -97,9 +85,15 @@ int main(int argc, char** argv){ while(ros::ok()){ ros::spinOnce(); - - //plan.refreshObstacles(obstacle_coordinates); + auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + obstacle_coordinates.clear(); + for(auto it = obstacles.global_robot_position.begin(); it != obstacles.global_robot_position.end(); ++it) { + obstacle_coordinates.push_back(it->x); + obstacle_coordinates.push_back(it->y); + } + plan.refreshObstacles(obstacle_coordinates); + plan.removeOldPoints(planner_state.current_x, planner_state.current_y); // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed From 46a5be61a63b23f508bb5e30e04abc543f54f106 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 11:31:24 +0100 Subject: [PATCH 098/200] Remove old obstacles in refresh obstacles function --- src/path_planner/path_planner.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 43a2178..6696240 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -64,6 +64,14 @@ void PathPlanner::refreshObstacles(std::list obstacle_coordinates) { if(no_plan){ return; } + + // delete all old obstacles + for (float x = 0; x < FIELD_LENGTH; x += node_distance){ + for (float y = 0; y < FIELD_LENGTH; y += node_distance){ + graph[coordToIndex(x)][coordToIndex(y)].obstacle = false; + } + } + std::list::iterator x = obstacle_coordinates.begin(); std::list::iterator y = ++(obstacle_coordinates.begin()); while(y != obstacle_coordinates.end()){ From 6aed9792a391ebddbb9aec057d60f97c12d73b59 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 11:44:51 +0100 Subject: [PATCH 099/200] Only get obstacles if a plan is requested --- src/nodes/path_planner_node.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index b3f61a1..34f5e77 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -85,14 +85,15 @@ int main(int argc, char** argv){ while(ros::ok()){ ros::spinOnce(); - - auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); - obstacle_coordinates.clear(); - for(auto it = obstacles.global_robot_position.begin(); it != obstacles.global_robot_position.end(); ++it) { - obstacle_coordinates.push_back(it->x); - obstacle_coordinates.push_back(it->y); + if(planner_state.make_plan){ + auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + obstacle_coordinates.clear(); + for(auto it = obstacles.global_robot_position.begin(); it != obstacles.global_robot_position.end(); ++it) { + obstacle_coordinates.push_back(it->x); + obstacle_coordinates.push_back(it->y); + } + plan.refreshObstacles(obstacle_coordinates); } - plan.refreshObstacles(obstacle_coordinates); plan.removeOldPoints(planner_state.current_x, planner_state.current_y); From dfad3300f2120d39fb36bcc28abfffe835346ba7 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 11:56:53 +0100 Subject: [PATCH 100/200] Update current point as long as plan is requested --- src/nodes/path_planner_node.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 34f5e77..834ffbc 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -35,12 +35,6 @@ bool newPlanCB(Request &req, Response &res, PlannerState* planner_state){ planner_state->make_plan = true; - // Update planner state - geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); - auto& position = current_pose.pose.position; - planner_state->current_x = position.x; - planner_state->current_y = position.y; - //Accept the new goal planner_state->new_goal = true; @@ -93,6 +87,12 @@ int main(int argc, char** argv){ obstacle_coordinates.push_back(it->y); } plan.refreshObstacles(obstacle_coordinates); + + // Update planner state + geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); + auto& position = current_pose.pose.position; + planner_state->current_x = position.x; + planner_state->current_y = position.y; } plan.removeOldPoints(planner_state.current_x, planner_state.current_y); From 00527bed88f088be3c2109e21f30a7ceabcdf8c7 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 11:57:15 +0100 Subject: [PATCH 101/200] Remove print --- src/path_planner/path_planner.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 6696240..41aeaec 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -71,11 +71,10 @@ void PathPlanner::refreshObstacles(std::list obstacle_coordinates) { graph[coordToIndex(x)][coordToIndex(y)].obstacle = false; } } - + std::list::iterator x = obstacle_coordinates.begin(); std::list::iterator y = ++(obstacle_coordinates.begin()); while(y != obstacle_coordinates.end()){ - std::cout << "adding obstacle" << std::endl; addObstacle(*x, *y); x++; y++; From db9c5ac0cf6f8a6892fa464d11da3f1612cd877f Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 11:59:26 +0100 Subject: [PATCH 102/200] Fix bug --- src/nodes/path_planner_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 834ffbc..ca3f821 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -91,8 +91,8 @@ int main(int argc, char** argv){ // Update planner state geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); auto& position = current_pose.pose.position; - planner_state->current_x = position.x; - planner_state->current_y = position.y; + planner_state.current_x = position.x; + planner_state.current_y = position.y; } plan.removeOldPoints(planner_state.current_x, planner_state.current_y); From 0cc77651a2c70c9dfc8677370b1d45182794bac2 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 12:05:37 +0100 Subject: [PATCH 103/200] Do not remove endpoint from plan --- src/path_planner/path_planner.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 41aeaec..83b374b 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -363,6 +363,11 @@ void PathPlanner::removeOldPoints(float current_x, float current_y){ return; } + // Do not remove end point from plan + if(++simple_plan.begin() == simple_plan.end()){ + return; + } + if(abs(current_x-simple_plan.begin()->getX()) < margin && abs(current_y-simple_plan.begin()->getY())getX() << ", " << simple_plan.begin()->getY() << std::endl; From 682298f0959167e9273629556018e29f16fec9dc Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 12:17:55 +0100 Subject: [PATCH 104/200] Add ros info --- src/nodes/path_planner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index ca3f821..08312e6 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -99,7 +99,7 @@ int main(int argc, char** argv){ // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed if(planner_state.make_plan && (!plan.isPlanSafe(planner_state.current_x,planner_state.current_y) || planner_state.new_goal)){ - //ROS_INFO("Make new plan."); + ROS_INFO("Make new plan."); planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); std::list simple_plan = plan.getSimplePlan(); From 180e50cbd87da5de80893a908227f1e5eb092d01 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 12:24:56 +0100 Subject: [PATCH 105/200] Publish all points in simple plan --- src/nodes/path_planner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 08312e6..f02027f 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -115,7 +115,7 @@ int main(int argc, char** argv){ std::list::iterator second_point = ++(simple_plan.begin()); std::cout << "Published points:\t"; - for(std::list::iterator it = second_point; it != simple_plan.end(); it++){ + for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ point.x = it->getX(); point.y = it->getY(); From 268ca39402e0e9789d38a48bb9fd87b7bef370cc Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 12:29:57 +0100 Subject: [PATCH 106/200] Remove printing --- src/path_planner/path_planner.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 83b374b..721c44e 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -132,9 +132,9 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent graph[x_index][y_index].setParentX(parent_x); graph[x_index][y_index].setParentY(parent_y); destination_reached = true; - std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; + /*std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; std::cout << "Destination parent: x=" << graph[x_index][y_index].getParentX() - << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl; + << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl;*/ } // If node is not destination and hasn't been searched yet else if(!graph[x_index][y_index].closed){ @@ -316,10 +316,10 @@ void PathPlanner::simplifyPlan() { } // Print the remaining points - std::cout << "Simple plan: " << std::endl; + /*std::cout << "Simple plan: " << std::endl; for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; - } + }*/ } bool PathPlanner::isPlanSafe(float current_x, float current_y) { From a6281b1c041d2e3d2dd2e3e951759ba583cae023 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 12:33:43 +0100 Subject: [PATCH 107/200] Remove printing --- src/nodes/path_planner_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index f02027f..76172c5 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -114,7 +114,7 @@ int main(int argc, char** argv){ // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan std::list::iterator second_point = ++(simple_plan.begin()); - std::cout << "Published points:\t"; + //std::cout << "Published points:\t"; for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ point.x = it->getX(); @@ -122,7 +122,7 @@ int main(int argc, char** argv){ points_in_plan.push_back(point); - std::cout << point.x << ", " << point.y << "\t"; + //std::cout << point.x << ", " << point.y << "\t"; } std::cout << std::endl; From 43b6c58b4ceca20d5d9998539263dc5d4748abe9 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 12:39:18 +0100 Subject: [PATCH 108/200] Make printing smarter --- src/nodes/path_planner_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 76172c5..7cccc1f 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -114,7 +114,7 @@ int main(int argc, char** argv){ // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan std::list::iterator second_point = ++(simple_plan.begin()); - //std::cout << "Published points:\t"; + std::cout << "Published points:\t"; for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ point.x = it->getX(); @@ -122,7 +122,7 @@ int main(int argc, char** argv){ points_in_plan.push_back(point); - //std::cout << point.x << ", " << point.y << "\t"; + std::cout << point.x << ", " << point.y << "\t"; } std::cout << std::endl; @@ -131,10 +131,10 @@ int main(int argc, char** argv){ // Publish plan as long as a plan is requested if(planner_state.make_plan){ - std::cout << "Published points:\t"; + /*std::cout << "Published points:\t"; for(auto it = points_in_plan.begin(); it != points_in_plan.end(); it++){ std::cout << it->x << ", " << it->y << "\t" << std::endl; - } + }*/ msg.header.stamp = ros::Time::now(); pub_plan.publish(msg); std::cout << std::endl; From 149b9a431ebf6a726ce17cfd78a2dfcb319fcee3 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 12:41:47 +0100 Subject: [PATCH 109/200] Fix stupid print.. --- src/nodes/path_planner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 7cccc1f..8fbb288 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -137,7 +137,7 @@ int main(int argc, char** argv){ }*/ msg.header.stamp = ros::Time::now(); pub_plan.publish(msg); - std::cout << std::endl; + //std::cout << std::endl; } rate.sleep(); From dc343aebecbdaaa0148049c5b714e7773ffa7948 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 12:45:31 +0100 Subject: [PATCH 110/200] Do not send start --- src/nodes/path_planner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 8fbb288..5bbb160 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -115,7 +115,7 @@ int main(int argc, char** argv){ std::list::iterator second_point = ++(simple_plan.begin()); std::cout << "Published points:\t"; - for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ + for(std::list::iterator it = second_point; it != simple_plan.end(); it++){ point.x = it->getX(); point.y = it->getY(); From 3d9684f152bb2ed7397238ff666006144bfb5a55 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 13:23:19 +0100 Subject: [PATCH 111/200] Update published points when plan is updated --- include/control/planner/path_planner.hpp | 2 +- src/nodes/path_planner_node.cpp | 32 +++++++++++++++++++++--- src/path_planner/path_planner.cpp | 9 ++++--- 3 files changed, 35 insertions(+), 8 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index bc0e171..62c8583 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -88,7 +88,7 @@ class PathPlanner{ // Verify the plan from where the drone is to the end point bool isPlanSafe(float current_x, float current_y); void resetParameters(); - void removeOldPoints(float current_x, float current_y); + bool removeOldPoints(float current_x, float current_y); // These functions are mainly for the visualization std::vector> getGraph(){return graph;} diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 5bbb160..395d840 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -60,7 +60,8 @@ int main(int argc, char** argv){ PlannerState planner_state; PathPlanner plan(control::PlannerConfig::obstacle_radius, control::PlannerConfig::node_distance); - + std::list simple_plan; + geometry_msgs::Point point; std::list obstacle_coordinates; ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); @@ -95,16 +96,17 @@ int main(int argc, char** argv){ planner_state.current_y = position.y; } - plan.removeOldPoints(planner_state.current_x, planner_state.current_y); + bool is_plan_updated = plan.removeOldPoints(planner_state.current_x, planner_state.current_y); + // !!!!! points blir fjernet fra planen men publishes fortsatt!!!!!!!!!!!!!!!!!!!!! // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed if(planner_state.make_plan && (!plan.isPlanSafe(planner_state.current_x,planner_state.current_y) || planner_state.new_goal)){ ROS_INFO("Make new plan."); planner_state.new_goal = false; plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); - std::list simple_plan = plan.getSimplePlan(); + simple_plan = plan.getSimplePlan(); - geometry_msgs::Point point; + // Removing old plan points_in_plan.clear(); @@ -131,6 +133,28 @@ int main(int argc, char** argv){ // Publish plan as long as a plan is requested if(planner_state.make_plan){ + if(is_plan_updated){ + // Removing old plan + points_in_plan.clear(); + if(!simple_plan.empty()){ + + // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan + std::list::iterator second_point = ++(simple_plan.begin()); + + std::cout << "Published points:\t"; + for(std::list::iterator it = second_point; it != simple_plan.end(); it++){ + + point.x = it->getX(); + point.y = it->getY(); + + points_in_plan.push_back(point); + + std::cout << point.x << ", " << point.y << "\t"; + + } + std::cout << std::endl; + } + } /*std::cout << "Published points:\t"; for(auto it = points_in_plan.begin(); it != points_in_plan.end(); it++){ std::cout << it->x << ", " << it->y << "\t" << std::endl; diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 721c44e..f8ba02b 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -355,22 +355,25 @@ void PathPlanner::resetParameters() { destination_reached = false; } -void PathPlanner::removeOldPoints(float current_x, float current_y){ +bool PathPlanner::removeOldPoints(float current_x, float current_y){ // Check if current point is the first point of the plan float margin = node_distance; if(simple_plan.empty()){ - return; + return false; } // Do not remove end point from plan if(++simple_plan.begin() == simple_plan.end()){ - return; + return false; } + bool removed = false; if(abs(current_x-simple_plan.begin()->getX()) < margin && abs(current_y-simple_plan.begin()->getY())getX() << ", " << simple_plan.begin()->getY() << std::endl; simple_plan.pop_front(); + removed = true; } + return removed; } \ No newline at end of file From 596e57891a84256be99f6f11c631182458ac15db Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 14:23:09 +0100 Subject: [PATCH 112/200] Change print layout --- src/nodes/path_planner_node.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 395d840..c3b3fcd 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -97,7 +97,6 @@ int main(int argc, char** argv){ } bool is_plan_updated = plan.removeOldPoints(planner_state.current_x, planner_state.current_y); - // !!!!! points blir fjernet fra planen men publishes fortsatt!!!!!!!!!!!!!!!!!!!!! // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed if(planner_state.make_plan && (!plan.isPlanSafe(planner_state.current_x,planner_state.current_y) || planner_state.new_goal)){ @@ -124,7 +123,7 @@ int main(int argc, char** argv){ points_in_plan.push_back(point); - std::cout << point.x << ", " << point.y << "\t"; + std::cout << point.x << ", " << point.y << " "; } std::cout << std::endl; @@ -137,19 +136,16 @@ int main(int argc, char** argv){ // Removing old plan points_in_plan.clear(); if(!simple_plan.empty()){ - - // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan - std::list::iterator second_point = ++(simple_plan.begin()); std::cout << "Published points:\t"; - for(std::list::iterator it = second_point; it != simple_plan.end(); it++){ + for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ point.x = it->getX(); point.y = it->getY(); points_in_plan.push_back(point); - std::cout << point.x << ", " << point.y << "\t"; + std::cout << point.x << ", " << point.y << " "; } std::cout << std::endl; From 9045bf33c252bdbc72e141658d7bc40ba7919efe Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 14:23:35 +0100 Subject: [PATCH 113/200] Print simple plan after point is removed --- src/path_planner/path_planner.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index f8ba02b..3f1ec58 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -374,6 +374,12 @@ bool PathPlanner::removeOldPoints(float current_x, float current_y){ << simple_plan.begin()->getY() << std::endl; simple_plan.pop_front(); removed = true; + + std::cout << "Updated plan: "; + for(auto it = simple_plan.begin(); it != simple_plan.end(); it++){ + std::cout << it->getX() << ", " << it->getY() << " "; + } + std::cout << std::endl; } return removed; } \ No newline at end of file From f9a3f6c404f09b4998d93929293fa1f60cb22c80 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 14:24:48 +0100 Subject: [PATCH 114/200] Fix bug and remove debug printing --- src/nodes/path_planner_node.cpp | 1 + src/path_planner/path_planner.cpp | 6 ------ 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index c3b3fcd..170d65d 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -135,6 +135,7 @@ int main(int argc, char** argv){ if(is_plan_updated){ // Removing old plan points_in_plan.clear(); + simple_plan = plan.getSimplePlan(); if(!simple_plan.empty()){ std::cout << "Published points:\t"; diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 3f1ec58..f8ba02b 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -374,12 +374,6 @@ bool PathPlanner::removeOldPoints(float current_x, float current_y){ << simple_plan.begin()->getY() << std::endl; simple_plan.pop_front(); removed = true; - - std::cout << "Updated plan: "; - for(auto it = simple_plan.begin(); it != simple_plan.end(); it++){ - std::cout << it->getX() << ", " << it->getY() << " "; - } - std::cout << std::endl; } return removed; } \ No newline at end of file From c89e5c455fbba29d258855fb16bab17acab41fcb Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 7 Mar 2018 14:35:51 +0100 Subject: [PATCH 115/200] Not needed to check if starting point is on obstacle --- src/path_planner/path_planner.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index f8ba02b..1225c4b 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -234,8 +234,8 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo int x_index = coordToIndex(x); int y_index = coordToIndex(y); - if(graph[x_index][y_index].obstacle || graph[coordToIndex(current_x)][coordToIndex(current_y)].obstacle){ - std::cout << "Start or end on obstacle, invalid!" << std::endl; + if(graph[x_index][y_index].obstacle){ + std::cout << "End point on obstacle, invalid!" << std::endl; return; } From 5045fec0a40f6d56486d778f8e267351b06ce2e5 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Wed, 7 Mar 2018 21:35:29 +0100 Subject: [PATCH 116/200] Setup for publishing to mission_debugger --- control_fsm_default_config.yaml | 2 +- include/control/fsm/control_fsm.hpp | 3 +- include/control/fsm/go_to_state.hpp | 6 ++ include/control/tools/obstacle_avoidance.hpp | 15 +++++ src/fsm/go_to_state.cpp | 30 +++++++-- src/tools/obstacle_avoidance.cpp | 66 +++++++++++++++++--- 6 files changed, 108 insertions(+), 14 deletions(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index c7bb0f7..5d00925 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -62,5 +62,5 @@ #Obstacle avoidance clearance in back obstacle_clearance_back: 2.0, #Obstacle avoidance checkradius - obstacle_clearance_checkradius: 5.0 + obstacle_clearance_checkradius: 10.0 } diff --git a/include/control/fsm/control_fsm.hpp b/include/control/fsm/control_fsm.hpp index 5f34cd7..eff562c 100644 --- a/include/control/fsm/control_fsm.hpp +++ b/include/control/fsm/control_fsm.hpp @@ -118,7 +118,6 @@ class ControlFSM { ///All states needs access to obstacle avoidance control::ObstacleAvoidance obstacle_avoidance_; - protected: /** * @brief Changes the current running state @@ -128,6 +127,8 @@ class ControlFSM { * @param event Which event triggered the transition request */ void transitionTo(StateInterface& state, StateInterface* caller_p, const EventData& event); + + public: ///Constructor sets default/starting state diff --git a/include/control/fsm/go_to_state.hpp b/include/control/fsm/go_to_state.hpp index 6adda33..9c6f79a 100644 --- a/include/control/fsm/go_to_state.hpp +++ b/include/control/fsm/go_to_state.hpp @@ -21,6 +21,9 @@ class GoToState : public StateInterface { ///Is state active flag bool is_active_ = false; + + ///Obstacle avoidance kicked in flag + bool obstacle_avoidance_kicked_in_ = false; public: GoToState(); void stateInit(ControlFSM& fsm) override; @@ -38,6 +41,9 @@ class GoToState : public StateInterface { ///Handles delayed transition when position is reached void destinationReached(ControlFSM &fsm); + + ///Callback for obstacle avoidance + void obstacleAvoidanceCB(); }; //Only make these available for unit testing diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index 05ee91d..9b829e5 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -16,17 +16,32 @@ class ObstacleAvoidance { */ std::set< std::shared_ptr< std::function > > on_modified_cb_set_; + /**Set of callback method pointers registered by states + * Pointers are used to allow comparison as std::function can't be compared + */ + std::set< std::shared_ptr< std::function > > on_warn_cb_set_; + ///Notify states the setpoint has been changed void onModified(); + ///Notify states the setpoint should be changed + void onWarn(); + ///Runs obstacle avoidance, and modifies setpoint (and notifies) virtual bool doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint); + ///Publisher for avoid zone information + //ros::Publisher zone_pub_; + public: ///Default constructor ObstacleAvoidance() = default; ///Default copy constructor ObstacleAvoidance(const ObstacleAvoidance&) = default; + ///Add new callback to warning set + void registerOnWarnCBPtr(const std::shared_ptr >& cb_p) { on_warn_cb_set_.insert(cb_p); } + ///Remove a callback from warning set + void removeOnWarnCBPtr(const std::shared_ptr >& cb_p); ///Add new callback void registerOnModifiedCBPtr(const std::shared_ptr >& cb_p) { on_modified_cb_set_.insert(cb_p); } ///Remove a callback diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index edc96aa..41dbfc9 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -1,18 +1,27 @@ #include "control/fsm/go_to_state.hpp" #include "control/fsm/control_fsm.hpp" #include +#include #include #include #include "control/tools/config.hpp" #include "control/tools/target_tools.hpp" constexpr double PI = 3.14159265359; -constexpr double MAVROS_YAW_CORRECTION_PI_HALF = 3.141592653589793 / 2.0; +constexpr double MAVROS_YAW_CORRECTION_PI_HALF = PI / 2.0; GoToState::GoToState() : StateInterface::StateInterface() { setpoint_.type_mask = default_mask; } + +/** + * @brief Callback for obstacle avoidance + */ +void GoToState::obstacleAvoidanceCB(){ + obstacle_avoidance_kicked_in_ = true; +} + /** * @brief Returns a yaw that is a multiple of 90 degrees * @details Drone should fly as straight forward as possible @@ -116,6 +125,10 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); return; } + + // Set obstacle avoidance flag + obstacle_avoidance_kicked_in_ = false; + // Set setpoint setpoint_.position.x = cmd_.position_goal.x; setpoint_.position.y = cmd_.position_goal.y; @@ -132,8 +145,8 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { auto dx = setpoint_.position.x - pos.x; auto dy = setpoint_.position.y - pos.y; - setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(quat))); - /* + setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(quat))); + /* if (std::fabs(dx*dx + dy*dy) < 0.01){ // assume drone is stationary and dont change orientation } else{ @@ -151,6 +164,8 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { } void GoToState::stateEnd(ControlFSM& fsm, const EventData& event) { + // Set obstacle avoidance flag + obstacle_avoidance_kicked_in_ = false; } void GoToState::loopState(ControlFSM& fsm) { @@ -160,6 +175,11 @@ void GoToState::loopState(ControlFSM& fsm) { throw control::PoseNotValidException(); } + if (obstacle_avoidance_kicked_in_){ + RequestEvent abort_event(RequestType::ABORT); + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + } + using control::pose::quat2mavrosyaw; //Get pose auto pose_stamped = control::DroneHandler::getCurrentPose(); @@ -177,7 +197,7 @@ void GoToState::loopState(ControlFSM& fsm) { using control::Config; bool xy_reached = (pow(delta_x, 2) + pow(delta_y, 2)) <= pow(Config::dest_reached_margin, 2); bool z_reached = (fabs(delta_z) <= Config::altitude_reached_margin); - bool yaw_reached = (fabs(quat2mavrosyaw(quat) - setpoint_.yaw) <= Config::yaw_reached_margin); + bool yaw_reached = fabs(quat2mavrosyaw(quat) - setpoint_.yaw) <= Config::yaw_reached_margin; //If destination is reached, begin transition to another state if(xy_reached && z_reached && yaw_reached) { destinationReached(fsm); @@ -210,6 +230,8 @@ void GoToState::stateInit(ControlFSM& fsm) { //Set state variables delay_transition_.delayTime = ros::Duration(Config::go_to_hold_dest_time); + //fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(this->obstacleAvoidanceCB)); + control::handleInfoMsg("GoTo init completed!"); } diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 7f9fa85..7879de2 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -1,10 +1,11 @@ +#include + #include "control/tools/obstacle_avoidance.hpp" #include "control/tools/obstacle_state_handler.hpp" #include "control/tools/drone_handler.hpp" #include "control/tools/config.hpp" -#include -#include +#include #include #include @@ -37,6 +38,17 @@ inline geometry_msgs::Vector3 calcVectorBetweenPoints(const T& point_A, const K& return vector_diff; } +template +inline geometry_msgs::Vector3 vectorSum(const T& point_A, const K& point_B){ + geometry_msgs::Vector3 vector_sum; + vector_sum.x = point_A.x + point_B.x; + vector_sum.y = point_A.y + point_B.y; + vector_sum.z = point_A.z + point_B.z; + + return vector_sum; +} + + template inline float calcDistanceToObstacle(const T& point, const K& obstacle_position){ const auto vector_to_obstacle = calcVectorBetweenPoints(point, obstacle_position); @@ -60,7 +72,8 @@ inline float calcAngleToObstacle(const T& point, const K& obstacle_position, con } template -inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle){ +//inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle){ +inline T rotateXY(const T& point, const float angle){ // Apply 2d transformation matrix T new_point; new_point.x = point.x * std::cos(angle) - point.y * std::sin(angle); @@ -105,10 +118,12 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto& obstacles = control::ObstacleStateHandler::getCurrentObstacles(); const auto& drone_pose = control::DroneHandler::getCurrentPose(); - const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); - const float drone_speed_ratio = std::sqrt(drone_speed/3.f); + const float drone_speed_ratio = std::min((float)std::sqrt(drone_speed/3.f), 1.f); // 3.f m/s is assumed drone max speed + + ascend_msgs::PolygonArray polygon_array; + for (int i = 0; i < obstacles.count; i++){ const auto obstacle_position = obstacles.global_robot_position[i]; @@ -133,7 +148,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget // magic numbers are minimum clearance no matter what. Turn into parameters and include // this logic in the avoidZone function - ROS_INFO_THROTTLE("Drone speed ratio: %.3f", drone_speed_ratio); + //ROS_INFO_THROTTLE("Drone speed ratio: %.3f", drone_speed_ratio); const auto clearance_front = std::max((float)Config::obstacle_clearance_front*drone_speed_ratio, 1.f); const auto clearance_back = std::max((float)Config::obstacle_clearance_back*drone_speed_ratio, 1.f); const auto clearance_side = std::max((float)Config::obstacle_clearance_side*drone_speed_ratio, 1.f); @@ -147,7 +162,26 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget minimum_vector = rotateXY(minimum_vector, obstacle_direction); //ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_position.x + minimum_vector.x, obstacle_position.y + minimum_vector.y); - + + // generate points for zone_pub_ + geometry_msgs::Point32 point_front; point_front.x = clearance_front; + geometry_msgs::Point32 point_back; point_back.x = -clearance_back; + geometry_msgs::Point32 point_right; point_right.y = -clearance_side; + geometry_msgs::Point32 point_left; point_left.y = clearance_side; + point_front = rotateXY(point_front, obstacle_direction); + point_back = rotateXY(point_back, obstacle_direction); + point_left = rotateXY(point_left, obstacle_direction); + point_right = rotateXY(point_right, obstacle_direction); + + //ROS_INFO("Point front: %.2f\t%.2f", obstacle_position.x + point_front.x, obstacle_position.y + point_front.y); + //ROS_INFO("Point left: %.2f\t%.2f", obstacle_position.x + point_left.x, obstacle_position.y + point_left.y); + //ROS_INFO("Point right: %.2f\t%.2f", obstacle_position.x + point_right.x, obstacle_position.y + point_right.y); + //ROS_INFO("Point back: %.2f\t%.2f", obstacle_position.x + point_back.x, obstacle_position.y + point_back.y); + std::vector polygon; + polygon.push_back(point_front); + polygon.push_back(point_left); + polygon.push_back(point_back); + polygon.push_back(point_right); if (drone_distance_to_obstacle < minimum_distance){ if (setpoint_reachable @@ -174,7 +208,8 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } //ROS_INFO_THROTTLE(0.25,"Avoiding: %.3f -> %.3f", drone_distance_to_obstacle, minimum_distance); - ROS_INFO_THROTTLE(1,"Avoid: [%.2f, %.2f]; %.2f %.2f", drone_delta_to_obstacle.x, drone_delta_to_obstacle.y, drone_angle_to_obstacle, setpoint_angle_to_obstacle); + ROS_INFO_THROTTLE(1,"Avoid: [%.2f, %.2f]; %.2f %.2f", drone_delta_to_obstacle.x, drone_delta_to_obstacle.y, + drone_angle_to_obstacle, setpoint_angle_to_obstacle); } } } @@ -195,6 +230,13 @@ void control::ObstacleAvoidance::onModified() { } } +void control::ObstacleAvoidance::onWarn() { + //Run all callbacks + for(auto& cb_p : on_warn_cb_set_ ) { + (*cb_p)(); + } +} + mavros_msgs::PositionTarget control::ObstacleAvoidance::run(mavros_msgs::PositionTarget setpoint) { //If obstacle avoidance has altered the setpoint if(doObstacleAvoidance(&setpoint)) { @@ -211,3 +253,11 @@ void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr >& cb_p) { + const auto it = on_warn_cb_set_.find(cb_p); + if (it != on_warn_cb_set_.cend()){ + on_warn_cb_set_.erase(it); + } +} + From d1987dc555a6c7b9ad3dcfeb1a29ffe437bff07e Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 11:20:48 +0100 Subject: [PATCH 117/200] Add unsafe zones for planning a path --- include/control/planner/node.hpp | 1 + include/control/planner/path_planner.hpp | 4 ++ src/path_planner/path_planner.cpp | 50 ++++++++++++++++++++++++ 3 files changed, 55 insertions(+) diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp index b246cc8..dd48e6b 100644 --- a/include/control/planner/node.hpp +++ b/include/control/planner/node.hpp @@ -32,6 +32,7 @@ class Node{ bool obstacle = false; bool closed = false; + bool unsafe = false; // Implemented only for the closed list priority queue friend bool operator< (const Node &lhs, const Node &rhs); diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 62c8583..f2b9f80 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -61,6 +61,10 @@ class PathPlanner{ void addObstacle(float center_x, float center_y); void refreshObstacles(std::list obstacle_coordinates); + // Adds unsafe zone around obstacles, the plan cannot be made here + void addUnsafeZone(float center_x, float center_y); + void refreshUnsafeZones(); + // Diagonal heuristic - can move in 8 directions from current point float calculateDiagonalHeuristic(float x, float y); float calculateManhattanHeuristic(float x, float y); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 1225c4b..e9fa3cf 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -81,6 +81,55 @@ void PathPlanner::refreshObstacles(std::list obstacle_coordinates) { } } +void PathPlanner::addUnsafeZone(float center_x, float center_y) { + // Same logic as adding obstacles, only difference is the radius + float unsafe_radius = obstacle_radius*2; + // Find the y value for each x in the circle + for (float x = center_x-unsafe_radius; x <= center_x+unsafe_radius; x+=node_distance) { + float y = sqrt(unsafe_radius*unsafe_radius-(x-center_x)*(x-center_x)); + // Do this to fill the circle + for(float i = center_y-y; i<=center_y+y; i+= node_distance) { + if (isValidCoordinate(x, i)) { + graph[coordToIndex(x)][coordToIndex(i)].closed = true; + graph[coordToIndex(x)][coordToIndex(i)].unsafe = true; + } + } + + } + // Do the same as over, just switch x and y + for (float y = center_y-unsafe_radius; y <= center_y+unsafe_radius; y+=node_distance) { + float x = sqrt(unsafe_radius*unsafe_radius-(y-center_y)*(y-center_y)); + for(float i = center_x-x; i<=center_x+x; i+= node_distance) { + if (isValidCoordinate(i, y)) { + graph[coordToIndex(i)][coordToIndex(y)].closed = true; + graph[coordToIndex(i)][coordToIndex(y)].unsafe = true; + } + } + } + +} +void PathPlanner::refreshUnsafeZones(){ + if(no_plan){ + return; + } + // delete all old unsafe zones + for (float x = 0; x < FIELD_LENGTH; x += node_distance){ + for (float y = 0; y < FIELD_LENGTH; y += node_distance){ + graph[coordToIndex(x)][coordToIndex(y)].unsafe = false; + } + } + + + std::list::iterator x = obstacle_coordinates.begin(); + std::list::iterator y = ++(obstacle_coordinates.begin()); + while(y != obstacle_coordinates.end()){ + std::cout << "adding unsafe zone" << std::endl; + addUnsafeZone(*x, *y); + x++; + y++; + } +} + float PathPlanner::calculateDiagonalHeuristic(float x, float y) { return (std::max(abs(x-end_node.getX()), abs(y-end_node.getY()))); } @@ -227,6 +276,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo initializeGraph(); refreshObstacles(obstacle_coordinates); + refreshUnsafeZones(); float x = end_node.getX(); float y = end_node.getY(); From 2977e9f8fee5d90bbadb77e3e534c66c6de5f61d Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 12:50:11 +0100 Subject: [PATCH 118/200] Add separate function for update the obstacles list --- include/control/planner/path_planner.hpp | 4 +++- src/nodes/path_planner_node.cpp | 4 +++- src/path_planner/path_planner.cpp | 7 +++++-- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index f2b9f80..240365c 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -57,9 +57,11 @@ class PathPlanner{ void initializeGraph(); + void setObstacleCoordinates(std::list &obstacle_coordinates); + // Add a circular obstacle void addObstacle(float center_x, float center_y); - void refreshObstacles(std::list obstacle_coordinates); + void refreshObstacles(); // Adds unsafe zone around obstacles, the plan cannot be made here void addUnsafeZone(float center_x, float center_y); diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 170d65d..e12bea9 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -87,7 +87,9 @@ int main(int argc, char** argv){ obstacle_coordinates.push_back(it->x); obstacle_coordinates.push_back(it->y); } - plan.refreshObstacles(obstacle_coordinates); + plan.setObstacleCoordinates(obstacle_coordinates); + plan.refreshObstacles(); + plan.refreshUnsafeZones(); // Update planner state geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index e9fa3cf..50afe9a 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -34,6 +34,10 @@ void PathPlanner::initializeGraph(){ } } +void PathPlanner::setObstacleCoordinates(std::list &obstacle_coordinates) { + this->obstacle_coordinates = obstacle_coordinates; +} + void PathPlanner::addObstacle(float center_x, float center_y) { // Find the y value for each x in the circle for (float x = center_x-obstacle_radius; x <= center_x+obstacle_radius; x+=node_distance) { @@ -59,8 +63,7 @@ void PathPlanner::addObstacle(float center_x, float center_y) { } } -void PathPlanner::refreshObstacles(std::list obstacle_coordinates) { - this->obstacle_coordinates = obstacle_coordinates; +void PathPlanner::refreshObstacles() { if(no_plan){ return; } From e32aba2c7fef1a12e06081a86f97a900be9a4ab2 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 12:51:06 +0100 Subject: [PATCH 119/200] Make sure iterator doesnt go outside list --- src/path_planner/path_planner.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 50afe9a..e56d638 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -77,7 +77,7 @@ void PathPlanner::refreshObstacles() { std::list::iterator x = obstacle_coordinates.begin(); std::list::iterator y = ++(obstacle_coordinates.begin()); - while(y != obstacle_coordinates.end()){ + while(y != obstacle_coordinates.end() && x != obstacle_coordinates.end()){ addObstacle(*x, *y); x++; y++; @@ -125,7 +125,7 @@ void PathPlanner::refreshUnsafeZones(){ std::list::iterator x = obstacle_coordinates.begin(); std::list::iterator y = ++(obstacle_coordinates.begin()); - while(y != obstacle_coordinates.end()){ + while(y != obstacle_coordinates.end()&& x != obstacle_coordinates.end()){ std::cout << "adding unsafe zone" << std::endl; addUnsafeZone(*x, *y); x++; From ec420e4cfe0d2e4809d1f5156373de5a383e1f7e Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 12:52:13 +0100 Subject: [PATCH 120/200] Dont make plan if end or start point is unsafe for planning --- src/path_planner/path_planner.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index e56d638..ae50740 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -278,7 +278,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo end_node = Node(target_x,target_y, std::numeric_limits::infinity(), 0); initializeGraph(); - refreshObstacles(obstacle_coordinates); + refreshObstacles(); refreshUnsafeZones(); float x = end_node.getX(); @@ -287,8 +287,12 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo int x_index = coordToIndex(x); int y_index = coordToIndex(y); - if(graph[x_index][y_index].obstacle){ - std::cout << "End point on obstacle, invalid!" << std::endl; + if(graph[x_index][y_index].unsafe){ + std::cout << "End point unsafe, invalid!" << std::endl; + return; + } + if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { + std::cout << "\n GET OOOOOUT!!! \n\n"; return; } From d55023f91d8745dc5399050ecfeea0aa4926cb45 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 12:54:49 +0100 Subject: [PATCH 121/200] Add function for checking if line can be simplified --- include/control/planner/path_planner.hpp | 1 + src/path_planner/path_planner.cpp | 19 ++++++++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 240365c..6aee247 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -86,6 +86,7 @@ class PathPlanner{ bool isStart(float x, float y); bool isValidCoordinate(float x, float y); bool isSafeLine(float x1, float y1, float x2, float y2); + bool canSimplifyLine(float x1, float y1, float x2, float y2); // Use A* to calculate the path void makePlan(float current_x, float current_y, float target_x, float target_y); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index ae50740..3c8c33d 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -256,6 +256,23 @@ bool PathPlanner::isSafeLine(float x1, float y1, float x2, float y2) { return true; } +bool PathPlanner::canSimplifyLine(float x1, float y1, float x2, float y2) { + float delta_x = x2-x1; + float delta_y = y2-y1; + float num_points = 20*std::max(abs(delta_x),abs(delta_y)); + for(int i = 0; i < num_points; i++){ + x1 += delta_x/num_points; + y1 += delta_y/num_points; + // checking if the line goes through an obstacle at this point + if(isValidCoordinate(x1,y1)){ + if (graph[coordToIndex(x1)][coordToIndex(y1)].unsafe) { + return false; + } + } + } + return true; +} + void PathPlanner::makePlan(float current_x, float current_y, float target_x, float target_y) { no_plan = false; @@ -358,7 +375,7 @@ void PathPlanner::simplifyPlan() { // If an obstacle is hit, the points are necessary and the search will start // at the end of the current search. - if(!isSafeLine(x1,y1,x2,y2)){ + if(!canSimplifyLine(x1,y1,x2,y2)){ first++; second++; third++; From 6e111cf65bad662cd15ac3eeb9ac834b94d4afe8 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 13:10:26 +0100 Subject: [PATCH 122/200] Remove print --- src/path_planner/path_planner.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 3c8c33d..e5f0a9d 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -126,7 +126,6 @@ void PathPlanner::refreshUnsafeZones(){ std::list::iterator x = obstacle_coordinates.begin(); std::list::iterator y = ++(obstacle_coordinates.begin()); while(y != obstacle_coordinates.end()&& x != obstacle_coordinates.end()){ - std::cout << "adding unsafe zone" << std::endl; addUnsafeZone(*x, *y); x++; y++; From 5e96ca14b49e448bb5d4d017368aba1a87dfe1f5 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 14:53:30 +0100 Subject: [PATCH 123/200] Add struct for obstacle data --- include/control/planner/path_planner.hpp | 10 ++++++++-- src/nodes/path_planner_node.cpp | 9 ++++----- src/path_planner/path_planner.cpp | 18 +++++------------- 3 files changed, 17 insertions(+), 20 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 6aee247..76f8d10 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -20,6 +20,12 @@ namespace pathplanner{ #define FIELD_LENGTH 20.0 +struct Obstacle { + float x; + float y; + float dir; +}; + class PathPlanner; class PathPlanner{ private: @@ -36,7 +42,7 @@ class PathPlanner{ std::list plan; std::list simple_plan; - std::list obstacle_coordinates; + std::list obstacle_coordinates; Node end_node; Node start_node; @@ -57,7 +63,7 @@ class PathPlanner{ void initializeGraph(); - void setObstacleCoordinates(std::list &obstacle_coordinates); + void setObstacleCoordinates(std::list &obstacle_coordinates); // Add a circular obstacle void addObstacle(float center_x, float center_y); diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index e12bea9..1994dea 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -62,7 +62,7 @@ int main(int argc, char** argv){ PathPlanner plan(control::PlannerConfig::obstacle_radius, control::PlannerConfig::node_distance); std::list simple_plan; geometry_msgs::Point point; - std::list obstacle_coordinates; + std::list obstacle_coordinates; ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); ros::ServiceServer server = n.advertiseService("path_planner_service", boost::bind(&newPlanCB, _1, _2, &planner_state)); @@ -81,11 +81,10 @@ int main(int argc, char** argv){ ros::spinOnce(); if(planner_state.make_plan){ - auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + auto obstacles_msg = control::ObstacleStateHandler::getCurrentObstacles(); obstacle_coordinates.clear(); - for(auto it = obstacles.global_robot_position.begin(); it != obstacles.global_robot_position.end(); ++it) { - obstacle_coordinates.push_back(it->x); - obstacle_coordinates.push_back(it->y); + for(auto it = obstacles_msg.global_robot_position.begin(); it != obstacles_msg.global_robot_position.end(); ++it) { + obstacle_coordinates.push_back(control::pathplanner::Obstacle{it->x, it->y,0}); } plan.setObstacleCoordinates(obstacle_coordinates); plan.refreshObstacles(); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index e5f0a9d..8466693 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -34,7 +34,7 @@ void PathPlanner::initializeGraph(){ } } -void PathPlanner::setObstacleCoordinates(std::list &obstacle_coordinates) { +void PathPlanner::setObstacleCoordinates(std::list &obstacle_coordinates) { this->obstacle_coordinates = obstacle_coordinates; } @@ -75,12 +75,8 @@ void PathPlanner::refreshObstacles() { } } - std::list::iterator x = obstacle_coordinates.begin(); - std::list::iterator y = ++(obstacle_coordinates.begin()); - while(y != obstacle_coordinates.end() && x != obstacle_coordinates.end()){ - addObstacle(*x, *y); - x++; - y++; + for(auto current = obstacle_coordinates.begin(); current != obstacle_coordinates.end(); current++){ + addObstacle(current->x, current->y); } } @@ -123,12 +119,8 @@ void PathPlanner::refreshUnsafeZones(){ } - std::list::iterator x = obstacle_coordinates.begin(); - std::list::iterator y = ++(obstacle_coordinates.begin()); - while(y != obstacle_coordinates.end()&& x != obstacle_coordinates.end()){ - addUnsafeZone(*x, *y); - x++; - y++; + for(auto current = obstacle_coordinates.begin(); current != obstacle_coordinates.end(); current++){ + addObstacle(current->x, current->y); } } From 65edcca92d589c1490f1a7b81f2a74ebb1bb1d5d Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 14:53:49 +0100 Subject: [PATCH 124/200] Remoce unused include --- src/nodes/path_planner_node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 1994dea..622c657 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -4,7 +4,6 @@ #include "control/tools/drone_handler.hpp" #include #include -#include "ascend_msgs/GRStateArray.h" #include #include "control/tools/planner_config.hpp" #include "ascend_msgs/PointArrayStamped.h" From fa0e27e123aad9a8c335a0a868b1153c92c387a9 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 14:59:40 +0100 Subject: [PATCH 125/200] Change to from obstacle_coordinates name to obstacles --- include/control/planner/path_planner.hpp | 4 ++-- src/nodes/path_planner_node.cpp | 8 ++++---- src/path_planner/path_planner.cpp | 8 ++++---- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 76f8d10..2563887 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -42,7 +42,7 @@ class PathPlanner{ std::list plan; std::list simple_plan; - std::list obstacle_coordinates; + std::list obstacles; Node end_node; Node start_node; @@ -63,7 +63,7 @@ class PathPlanner{ void initializeGraph(); - void setObstacleCoordinates(std::list &obstacle_coordinates); + void setObstacles(std::list &obstacles); // Add a circular obstacle void addObstacle(float center_x, float center_y); diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 622c657..ddc8cae 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -61,7 +61,7 @@ int main(int argc, char** argv){ PathPlanner plan(control::PlannerConfig::obstacle_radius, control::PlannerConfig::node_distance); std::list simple_plan; geometry_msgs::Point point; - std::list obstacle_coordinates; + std::list obstacles; ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); ros::ServiceServer server = n.advertiseService("path_planner_service", boost::bind(&newPlanCB, _1, _2, &planner_state)); @@ -81,11 +81,11 @@ int main(int argc, char** argv){ ros::spinOnce(); if(planner_state.make_plan){ auto obstacles_msg = control::ObstacleStateHandler::getCurrentObstacles(); - obstacle_coordinates.clear(); + obstacles.clear(); for(auto it = obstacles_msg.global_robot_position.begin(); it != obstacles_msg.global_robot_position.end(); ++it) { - obstacle_coordinates.push_back(control::pathplanner::Obstacle{it->x, it->y,0}); + obstacles.push_back(control::pathplanner::Obstacle{it->x, it->y,0}); } - plan.setObstacleCoordinates(obstacle_coordinates); + plan.setObstacles(obstacles); plan.refreshObstacles(); plan.refreshUnsafeZones(); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 8466693..88c3fb8 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -34,8 +34,8 @@ void PathPlanner::initializeGraph(){ } } -void PathPlanner::setObstacleCoordinates(std::list &obstacle_coordinates) { - this->obstacle_coordinates = obstacle_coordinates; +void PathPlanner::setObstacles(std::list &obstacles) { + this->obstacles = obstacles; } void PathPlanner::addObstacle(float center_x, float center_y) { @@ -75,7 +75,7 @@ void PathPlanner::refreshObstacles() { } } - for(auto current = obstacle_coordinates.begin(); current != obstacle_coordinates.end(); current++){ + for(auto current = obstacles.begin(); current != obstacles.end(); current++){ addObstacle(current->x, current->y); } } @@ -119,7 +119,7 @@ void PathPlanner::refreshUnsafeZones(){ } - for(auto current = obstacle_coordinates.begin(); current != obstacle_coordinates.end(); current++){ + for(auto current = obstacles.begin(); current != obstacles.end(); current++){ addObstacle(current->x, current->y); } } From 90f59d3f9ea2a6619cb0db34ed7e0ca82b4fa2c3 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 15:11:02 +0100 Subject: [PATCH 126/200] Add direction to the obstacles list --- src/nodes/path_planner_node.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index ddc8cae..c384700 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -82,9 +82,12 @@ int main(int argc, char** argv){ if(planner_state.make_plan){ auto obstacles_msg = control::ObstacleStateHandler::getCurrentObstacles(); obstacles.clear(); - for(auto it = obstacles_msg.global_robot_position.begin(); it != obstacles_msg.global_robot_position.end(); ++it) { - obstacles.push_back(control::pathplanner::Obstacle{it->x, it->y,0}); - } + auto current_coord = obstacles_msg.global_robot_position.begin(); + auto current_dir = obstacles_msg.direction.begin(); + while(current_coord != obstacles_msg.global_robot_position.end() && current_dir != obstacles_msg.direction.end()){ + obstacles.push_back(control::pathplanner::Obstacle{current_coord->x, current_coord->y,*current_dir}); + } + plan.setObstacles(obstacles); plan.refreshObstacles(); plan.refreshUnsafeZones(); From a6809fddc7ddd4e05a87ffb754e0f0a0b6cb9ac3 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 8 Mar 2018 15:35:13 +0100 Subject: [PATCH 127/200] integrate with go_to_state --- src/fsm/go_to_state.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 41dbfc9..a45da9f 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -230,7 +230,11 @@ void GoToState::stateInit(ControlFSM& fsm) { //Set state variables delay_transition_.delayTime = ros::Duration(Config::go_to_hold_dest_time); - //fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(this->obstacleAvoidanceCB)); + std::function obstacleAvoidanceCB = [this]()->void { + this->obstacle_avoidance_kicked_in_ = true; + }; + + fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); control::handleInfoMsg("GoTo init completed!"); } From 72f980cb767f2fc8f2bbb0b7a95926cfc268b491 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 8 Mar 2018 15:37:16 +0100 Subject: [PATCH 128/200] integrate with go_to_state --- src/fsm/go_to_state.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 53f59f5..73a3c99 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -126,7 +126,7 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { return; } - // Set obstacle avoidance flag + // Set obstacle avoidance flag on entry obstacle_avoidance_kicked_in_ = false; // Set setpoint @@ -164,7 +164,7 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { } void GoToState::stateEnd(ControlFSM& fsm, const EventData& event) { - // Set obstacle avoidance flag + // Set obstacle avoidance flag on exit obstacle_avoidance_kicked_in_ = false; } From d01a35a472babb3e05f62ed09e8996fa36f5a8ac Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 8 Mar 2018 16:12:52 +0100 Subject: [PATCH 129/200] Readd polygon publisher --- control_fsm_default_config.yaml | 2 +- include/control/tools/obstacle_avoidance.hpp | 8 ++++-- src/tools/obstacle_avoidance.cpp | 26 +++++++++++--------- 3 files changed, 22 insertions(+), 14 deletions(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index 5d00925..f3a126c 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -62,5 +62,5 @@ #Obstacle avoidance clearance in back obstacle_clearance_back: 2.0, #Obstacle avoidance checkradius - obstacle_clearance_checkradius: 10.0 + obstacle_clearance_checkradius: 100.0 } diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index 746d4ad..0b55c28 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace control { @@ -30,12 +31,15 @@ class ObstacleAvoidance { ///Runs obstacle avoidance, and modifies setpoint (and notifies) virtual bool doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint); + ///Node handle + ros::NodeHandle nh_; + ///Publisher for avoid zone information - //ros::Publisher zone_pub_; + ros::Publisher zone_pub_; public: ///Default constructor - ObstacleAvoidance() = default; + ObstacleAvoidance(); ///Default copy constructor ObstacleAvoidance(const ObstacleAvoidance&) = default; diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index fd741b3..6e81d84 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -122,8 +122,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); const float drone_speed_ratio = std::min((float)std::sqrt(drone_speed/3.f), 1.f); // 3.f m/s is assumed drone max speed - ascend_msgs::PolygonArray polygon_array; - + ascend_msgs::PolygonArray zone_msg; for (int i = 0; i < obstacles.count; i++){ const auto obstacle_position = obstacles.global_robot_position[i]; @@ -173,15 +172,13 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget point_left = rotateXY(point_left, obstacle_direction); point_right = rotateXY(point_right, obstacle_direction); - //ROS_INFO("Point front: %.2f\t%.2f", obstacle_position.x + point_front.x, obstacle_position.y + point_front.y); - //ROS_INFO("Point left: %.2f\t%.2f", obstacle_position.x + point_left.x, obstacle_position.y + point_left.y); - //ROS_INFO("Point right: %.2f\t%.2f", obstacle_position.x + point_right.x, obstacle_position.y + point_right.y); - //ROS_INFO("Point back: %.2f\t%.2f", obstacle_position.x + point_back.x, obstacle_position.y + point_back.y); - std::vector polygon; - polygon.push_back(point_front); - polygon.push_back(point_left); - polygon.push_back(point_back); - polygon.push_back(point_right); + geometry_msgs::Polygon polygon; + polygon.points.push_back(point_front); + polygon.points.push_back(point_left); + polygon.points.push_back(point_back); + polygon.points.push_back(point_right); + + zone_msg.polygons.push_back(polygon); if (drone_distance_to_obstacle < minimum_distance){ if (setpoint_reachable @@ -220,6 +217,8 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } } + zone_pub_.publish(zone_msg); + return setpoint_modified; } @@ -261,3 +260,8 @@ void control::ObstacleAvoidance::removeOnWarnCBPtr(const std::shared_ptr("/control/obstacle_avoidance/polygons", 10); +} + From c49be247331e2aa2d0f2fcb6b42d3ecb44316328 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 8 Mar 2018 16:18:59 +0100 Subject: [PATCH 130/200] Fix polygon bug --- src/tools/obstacle_avoidance.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 6e81d84..8c3f428 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -39,8 +39,8 @@ inline geometry_msgs::Vector3 calcVectorBetweenPoints(const T& point_A, const K& } template -inline geometry_msgs::Vector3 vectorSum(const T& point_A, const K& point_B){ - geometry_msgs::Vector3 vector_sum; +inline T vectorSum(const T& point_A, const K& point_B){ + T vector_sum; vector_sum.x = point_A.x + point_B.x; vector_sum.y = point_A.y + point_B.y; vector_sum.z = point_A.z + point_B.z; @@ -171,6 +171,10 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget point_back = rotateXY(point_back, obstacle_direction); point_left = rotateXY(point_left, obstacle_direction); point_right = rotateXY(point_right, obstacle_direction); + point_front = vectorSum(point_front, obstacle_position); + point_back = vectorSum(point_back, obstacle_position); + point_left = vectorSum(point_left, obstacle_position); + point_right = vectorSum(point_right, obstacle_position); geometry_msgs::Polygon polygon; polygon.points.push_back(point_front); From 5db27c7f709bfa9929025013c9e54e168345117a Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 17:41:31 +0100 Subject: [PATCH 131/200] Start functionality for escaping an unsafe zone --- include/control/planner/path_planner.hpp | 1 + src/path_planner/path_planner.cpp | 67 ++++++++++++++++++++++-- 2 files changed, 63 insertions(+), 5 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 2563887..4b6c007 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -68,6 +68,7 @@ class PathPlanner{ // Add a circular obstacle void addObstacle(float center_x, float center_y); void refreshObstacles(); + Obstacle* findBlockingObstacle(float current_x, float current_y); // Adds unsafe zone around obstacles, the plan cannot be made here void addUnsafeZone(float center_x, float center_y); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 88c3fb8..eb36077 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -264,9 +264,21 @@ bool PathPlanner::canSimplifyLine(float x1, float y1, float x2, float y2) { return true; } +Obstacle* PathPlanner::findBlockingObstacle(float current_x, float current_y) { + for (auto it = obstacles.begin(); it != obstacles.end(); it++) { + if(abs(current_x-it->x) <= obstacle_radius*2 && abs(current_y-it->y) <= obstacle_radius*2) { + std::cout << "Found blocking obstacle" << std::endl; + return &(*it); + } + } + std::cout << "Could not find blocking obstacle!!" << std::endl; + return nullptr; +} + void PathPlanner::makePlan(float current_x, float current_y, float target_x, float target_y) { no_plan = false; + bool escape_from_obstacle = false; // If start or end point is not valid, a plan is not created if(!isValidCoordinate(current_x,current_y)){ @@ -289,7 +301,51 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo refreshObstacles(); refreshUnsafeZones(); - float x = end_node.getX(); + + if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { + std::cout << "\n GET OOOOOUT!!! \n\n"; + escape_from_obstacle = true; + + // ---- make this a function ---- + + // Figure out which obstacle is blocking + Obstacle* blocking_obstacle = findBlockingObstacle(current_x, current_y); + if (!blocking_obstacle) { + std::cout << "Unable to solve problem!" << std::endl; + return; + } + + // Find the direction of the obstacle + float obstacle_dir = blocking_obstacle->dir; + + + // Figure out what direction drone should move in + float drone_dir /* = Rendell sin funksjon :) */; + + // Find setpoint in right direction outside unsafe zone + // The safe zone is obstacle_radius*2, but to ensure that + // the new point is outside the safe zone, 2.5 is used here + float safe_radius = obstacle_radius*2.5; + float safe_x = safe_radius*cos(drone_dir)+blocking_obstacle->x; + float safe_y = safe_radius*sin(drone_dir)+blocking_obstacle->y; + + if(!isValidCoordinate(safe_x, safe_y)) { + std::cout << "Cannot find safe point!" << std::endl; + return; + } + + // Add starting point to plan and plan a new plan from the + // setpoint to target + resetParameters(); + start_node = Node(safe_x, safe_y, 0, 0); + start_node.setParentX(0); + start_node.setParentY(0); + initializeGraph(); + refreshObstacles(); + refreshUnsafeZones(); + } + + float x = end_node.getX(); float y = end_node.getY(); int x_index = coordToIndex(x); @@ -299,10 +355,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo std::cout << "End point unsafe, invalid!" << std::endl; return; } - if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { - std::cout << "\n GET OOOOOUT!!! \n\n"; - return; - } + // Calculate all f values and set the parents relaxGraph(); @@ -331,6 +384,10 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo plan.push_front(graph[x_index][y_index]); } + if(escape_from_obstacle) { + plan.push_front(graph[current_x][current_y]); + } + // Print the plan - can be removed /*std::cout << std::endl << "Whole plan:" << std::endl; for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ From c5758423032aa89df06793008a0cdef101a99c17 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 18:58:19 +0100 Subject: [PATCH 132/200] Use obstacle math for finding escape direction for drone --- src/path_planner/path_planner.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index eb36077..5339a83 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -1,4 +1,5 @@ #include "control/planner/path_planner.hpp" +#include "control/tools/obstacle_math.hpp" using namespace control::pathplanner; @@ -318,10 +319,11 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // Find the direction of the obstacle float obstacle_dir = blocking_obstacle->dir; - - // Figure out what direction drone should move in - float drone_dir /* = Rendell sin funksjon :) */; - + // Find desired direction of the drone + struct Point {float x,y;}; + Point current_point{current_x, current_y}; + float drone_dir = obstacle_math::calcAngleToObstacle(current_point, *blocking_obstacle, obstacle_dir); + // Find setpoint in right direction outside unsafe zone // The safe zone is obstacle_radius*2, but to ensure that // the new point is outside the safe zone, 2.5 is used here From 0a2f9557b56e5e3b1a9e2520fb374232944aa768 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 8 Mar 2018 19:31:39 +0100 Subject: [PATCH 133/200] Integrate with obstacle_math lib --- include/control/tools/obstacle_math.hpp | 66 ----------------- src/tools/obstacle_avoidance.cpp | 94 +++++-------------------- 2 files changed, 17 insertions(+), 143 deletions(-) delete mode 100644 include/control/tools/obstacle_math.hpp diff --git a/include/control/tools/obstacle_math.hpp b/include/control/tools/obstacle_math.hpp deleted file mode 100644 index 6f80ae9..0000000 --- a/include/control/tools/obstacle_math.hpp +++ /dev/null @@ -1,66 +0,0 @@ -#include - -namespace obstacle_math { - -constexpr double PI{3.14159265358979323846264338327950288419}; - -// wrap any angle to range [0, 2pi) -template::value, T>::type> -T angleWrapper(const T angle){ - return angle - 2*PI*floor(angle/(2*PI)); -} - -/// Calculate vector between two points, end - start -template -inline T vectorDifference(const T& end, const K& start){ - T vector_diff; - vector_diff.x = end.x - start.x; - vector_diff.y = end.y - start.y; - vector_diff.z = end.z - start.z; - - return vector_diff; -} - - -template -inline T vectorSum(const T& a, const K& b){ - T vector_sum; - vector_sum.x = a.x + b.x; - vector_sum.y = a.y + b.y; - vector_sum.z = a.z + b.z; - - return vector_sum; -} - -template -inline double calcDistanceToObstacle(const T& point, const K& obstacle_position){ - const auto vector_to_obstacle = vectorDifference(point, obstacle_position); - const double distance_to_obstacle = std::sqrt(std::pow(vector_to_obstacle.x, 2) + std::pow(vector_to_obstacle.y, 2)); - - return distance_to_obstacle; -} - - -template -inline N calcAngleToObstacle(const T& point, const K& obstacle_position, const N obstacle_direction){ - T delta_drone_obstacle; - delta_drone_obstacle.x = point.x - obstacle_position.x; - delta_drone_obstacle.y = point.y - obstacle_position.y; - - const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); - - return angle_to_obstacle; -} - -// Apply 2d transformation matrix -template -inline T rotateXY(const T& point, const float angle){ - T new_point; - new_point.x = point.x * std::cos(angle) - point.y * std::sin(angle); - new_point.y = point.x * std::sin(angle) + point.y * std::cos(angle); - new_point.z = point.z; - - return new_point; -} - -} diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 8c3f428..7ab7f81 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -1,6 +1,7 @@ #include #include "control/tools/obstacle_avoidance.hpp" +#include "control/tools/obstacle_math.hpp" #include "control/tools/obstacle_state_handler.hpp" #include "control/tools/drone_handler.hpp" #include "control/tools/config.hpp" @@ -21,67 +22,13 @@ #include #include -constexpr float PI{3.14159265359f}; - -// wrap any angle to range [0, 2pi) -inline float angleWrapper(const float angle){ - return angle - 2*PI*floor(angle/(2*PI)); -} - -template -inline geometry_msgs::Vector3 calcVectorBetweenPoints(const T& point_A, const K& point_B){ - geometry_msgs::Vector3 vector_diff; - vector_diff.x = point_A.x - point_B.x; - vector_diff.y = point_A.y - point_B.y; - vector_diff.z = point_A.z - point_B.z; - - return vector_diff; -} - -template -inline T vectorSum(const T& point_A, const K& point_B){ - T vector_sum; - vector_sum.x = point_A.x + point_B.x; - vector_sum.y = point_A.y + point_B.y; - vector_sum.z = point_A.z + point_B.z; - - return vector_sum; -} - - -template -inline float calcDistanceToObstacle(const T& point, const K& obstacle_position){ - const auto vector_to_obstacle = calcVectorBetweenPoints(point, obstacle_position); - const float distance_to_obstacle = std::sqrt(std::pow(vector_to_obstacle.x, 2) + std::pow(vector_to_obstacle.y, 2)); - - return distance_to_obstacle; -} - - - -template -inline float calcAngleToObstacle(const T& point, const K& obstacle_position, const float obstacle_direction){ - geometry_msgs::Vector3 delta_drone_obstacle; - delta_drone_obstacle.x = point.x - obstacle_position.x; - delta_drone_obstacle.y = point.y - obstacle_position.y; - delta_drone_obstacle.z = point.z; - - const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); - - return angle_to_obstacle; -} - -template -//inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle){ -inline T rotateXY(const T& point, const float angle){ - // Apply 2d transformation matrix - T new_point; - new_point.x = point.x * std::cos(angle) - point.y * std::sin(angle); - new_point.y = point.x * std::sin(angle) + point.y * std::cos(angle); - new_point.z = point.z; - - return new_point; -} +using obstacle_math::PI; +using obstacle_math::rotateXY; +using obstacle_math::angleWrapper; +using obstacle_math::vectorSum; +using obstacle_math::vectorDifference; +using obstacle_math::calcAngleToObstacle; +using obstacle_math::calcDistanceToObstacle; /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle /// which is allowed, referenced from the obstacles coordinate system. x-axis is positive @@ -113,6 +60,7 @@ inline geometry_msgs::Vector3 avoidZone(const float obstacle_direction, bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { using control::Config; + bool setpoint_modified{false}; const auto& obstacles = control::ObstacleStateHandler::getCurrentObstacles(); @@ -131,7 +79,6 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ - ROS_INFO_THROTTLE(1, "Distance: %.2f", drone_distance_to_obstacle); // perform obstacle avoidance const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_position, obstacle_direction); const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_position, obstacle_direction); @@ -141,14 +88,14 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; //ROS_INFO_THROTTLE(2, "d_angle - s_angle: %.3f", fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI))); - //(drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && - //setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); + //(drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && + //setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); - - // magic numbers are minimum clearance no matter what. Turn into parameters and include + // magic numbers are minimum clearance no matter what. + // TODO: Turn into parameters and include // this logic in the avoidZone function //ROS_INFO_THROTTLE("Drone speed ratio: %.3f", drone_speed_ratio); - const auto clearance_front = std::max((float)Config::obstacle_clearance_front*drone_speed_ratio, 1.f); + const auto clearance_front = std::max((float)Config::obstacle_clearance_front*drone_speed_ratio, 1.3f); const auto clearance_back = std::max((float)Config::obstacle_clearance_back*drone_speed_ratio, 1.f); const auto clearance_side = std::max((float)Config::obstacle_clearance_side*drone_speed_ratio, 1.f); geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, @@ -160,7 +107,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget //ROS_INFO("Local min vec: %.3f\t%.3f", minimum_vector.x, minimum_vector.y); minimum_vector = rotateXY(minimum_vector, obstacle_direction); -//ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_position.x + minimum_vector.x, obstacle_position.y + minimum_vector.y); + //ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_position.x + minimum_vector.x, obstacle_position.y + minimum_vector.y); // generate points for zone_pub_ geometry_msgs::Point32 point_front; point_front.x = clearance_front; @@ -189,12 +136,9 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget && setpoint_distance_to_obstacle > drone_distance_to_obstacle && setpoint_distance_to_obstacle > minimum_distance){ // no action, maybe logging? - ROS_INFO_THROTTLE(1, "Going to setpoint instead"); + ROS_INFO_THROTTLE(1, "[obstacle avoidance]: Prefer setpoint over avoid algorithm"); } else { - const geometry_msgs::Vector3 drone_delta_to_obstacle - = calcVectorBetweenPoints(drone_pose.pose.position, obstacle_position); - if (setpoint_modified){ // TODO: find out a better solution to this problem ROS_WARN("[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); @@ -207,16 +151,12 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget if (setpoint->position.z < Config::min_in_air_alt){ setpoint->position.z = Config::min_in_air_alt; } - - //ROS_INFO_THROTTLE(0.25,"Avoiding: %.3f -> %.3f", drone_distance_to_obstacle, minimum_distance); - ROS_INFO_THROTTLE(1,"Avoid: [%.2f, %.2f]; %.2f %.2f", drone_delta_to_obstacle.x, drone_delta_to_obstacle.y, - drone_angle_to_obstacle, setpoint_angle_to_obstacle); } } } if (setpoint_modified){ - const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); + //const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); //ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); } } From a3fd413d60bc03ef48201085c11cb01a3bc1af94 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 20:08:25 +0100 Subject: [PATCH 134/200] Debugging --- src/nodes/path_planner_node.cpp | 2 +- src/path_planner/path_planner.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index c384700..bb1b816 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -51,7 +51,7 @@ int main(int argc, char** argv){ ros::init(argc, argv, "path_planner_server"); ros::NodeHandle n; - ros::Rate rate(5); + ros::Rate rate(1); ROS_INFO("Path planner node started"); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 5339a83..c822f1f 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -278,6 +278,8 @@ Obstacle* PathPlanner::findBlockingObstacle(float current_x, float current_y) { void PathPlanner::makePlan(float current_x, float current_y, float target_x, float target_y) { + std::cout << "Run makePlan" << std::endl; + no_plan = false; bool escape_from_obstacle = false; From 532b872d3077cf0b743bac46b5912c2252617b6e Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 20:36:05 +0100 Subject: [PATCH 135/200] Debug printing --- src/path_planner/path_planner.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index c822f1f..e219bfc 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -20,6 +20,7 @@ int PathPlanner::coordToIndex(float coord) { } void PathPlanner::initializeGraph(){ + std::cout << "Init" << std::endl; for (float x = 0; x < FIELD_LENGTH; x += node_distance){ for (float y = 0; y < FIELD_LENGTH; y += node_distance){ if(isStart(x,y)){ @@ -65,6 +66,7 @@ void PathPlanner::addObstacle(float center_x, float center_y) { } void PathPlanner::refreshObstacles() { + std::cout << "refreshObstacles" << std::endl; if(no_plan){ return; } @@ -109,6 +111,7 @@ void PathPlanner::addUnsafeZone(float center_x, float center_y) { } void PathPlanner::refreshUnsafeZones(){ + std::cout << "refreshUnsafeZones" << std::endl; if(no_plan){ return; } @@ -149,6 +152,7 @@ float PathPlanner::calculateEuclidianHeuristic(float x, float y) { }*/ void PathPlanner::relaxGraph(){ + std::cout << "relaxGraph" << std::endl; Node current_node; while(!open_list.empty()){ // Always search from the node with lowest f value @@ -266,6 +270,7 @@ bool PathPlanner::canSimplifyLine(float x1, float y1, float x2, float y2) { } Obstacle* PathPlanner::findBlockingObstacle(float current_x, float current_y) { + std::cout << "Search for blocking obstacle" << std::endl; for (auto it = obstacles.begin(); it != obstacles.end(); it++) { if(abs(current_x-it->x) <= obstacle_radius*2 && abs(current_y-it->y) <= obstacle_radius*2) { std::cout << "Found blocking obstacle" << std::endl; @@ -473,6 +478,7 @@ bool PathPlanner::isPlanSafe(float current_x, float current_y) { } void PathPlanner::resetParameters() { + std::cout << "Reset Parameters" << std::endl; while(!open_list.empty()){ open_list.pop(); } From f89eb219ca551dbb98ff8d3bee4570ffc417bd24 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 20:36:54 +0100 Subject: [PATCH 136/200] Change simple plan when escaping --- src/path_planner/path_planner.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index e219bfc..85b7c90 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -393,9 +393,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo plan.push_front(graph[x_index][y_index]); } - if(escape_from_obstacle) { - plan.push_front(graph[current_x][current_y]); - } + // Print the plan - can be removed /*std::cout << std::endl << "Whole plan:" << std::endl; @@ -405,6 +403,9 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // Delete unnecessary points simplifyPlan(); + if(escape_from_obstacle) { + simple_plan.push_front(graph[current_x][current_y]); + } } void PathPlanner::simplifyPlan() { From 72790d3473ce0f2a4595ba3f5bcf819321d00fc0 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 8 Mar 2018 20:37:56 +0100 Subject: [PATCH 137/200] Fix comments --- src/path_planner/path_planner.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 85b7c90..46ba39f 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -431,14 +431,14 @@ void PathPlanner::simplifyPlan() { y1 = first->getY(); y2 = third->getY(); - // If an obstacle is hit, the points are necessary and the search will start + // If an unsafe zone is hit, the points are necessary and the search will start // at the end of the current search. if(!canSimplifyLine(x1,y1,x2,y2)){ first++; second++; third++; } - // If the straight line does not go through an obstacle, delete the second point + // If the straight line does not go through an unsafe zone, delete the second point // (no use in going through it when we can go in a straight line) // The search continues with same starting point, but the second and third point is changed else{ From 402683e442030b89b0631aaa890cef1877eafdb6 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 8 Mar 2018 21:23:30 +0100 Subject: [PATCH 138/200] Start integration with posholdstate --- include/control/fsm/go_to_state.hpp | 2 -- include/control/fsm/position_hold_state.hpp | 3 +++ src/fsm/go_to_state.cpp | 7 ------ src/fsm/position_hold_state.cpp | 26 ++++++++++++++++++++- 4 files changed, 28 insertions(+), 10 deletions(-) diff --git a/include/control/fsm/go_to_state.hpp b/include/control/fsm/go_to_state.hpp index 5d20177..c3b0a37 100644 --- a/include/control/fsm/go_to_state.hpp +++ b/include/control/fsm/go_to_state.hpp @@ -43,8 +43,6 @@ class GoToState : public StateInterface { ///Handles delayed transition when position is reached void destinationReached(ControlFSM &fsm); - ///Callback for obstacle avoidance - void obstacleAvoidanceCB(); }; //Only make these available for unit testing diff --git a/include/control/fsm/position_hold_state.hpp b/include/control/fsm/position_hold_state.hpp index 399a6d7..c844d5c 100644 --- a/include/control/fsm/position_hold_state.hpp +++ b/include/control/fsm/position_hold_state.hpp @@ -8,10 +8,13 @@ ///Holds the current position class PositionHoldState : public StateInterface { + bool obstacle_avoidance_kicked_in_ = false; public: PositionHoldState(); void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; + void stateEnd(ControlFSM& fsm, const EventData& event) override; + void stateInit(ControlFSM& fsm) override; std::string getStateName() const override { return "Position hold"; } ascend_msgs::ControlFSMState getStateMsg() override; const mavros_msgs::PositionTarget* getSetpointPtr() override; diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index 73a3c99..36a753b 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -15,13 +15,6 @@ GoToState::GoToState() : StateInterface::StateInterface() { } -/** - * @brief Callback for obstacle avoidance - */ -void GoToState::obstacleAvoidanceCB(){ - obstacle_avoidance_kicked_in_ = true; -} - /** * @brief Returns a yaw that is a multiple of 90 degrees * @details Drone should fly as straight forward as possible diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index f76de2a..b0599ba 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -14,6 +14,7 @@ //Constructor sets default setpoint type mask PositionHoldState::PositionHoldState() { setpoint_.type_mask = default_mask; + } //Handles incoming events @@ -56,6 +57,11 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { //Error if pose is not valid throw control::PoseNotValidException(); } + + // Set flag for obstacle avoidance + obstacle_avoidance_kicked_in_ = false; + + //Get pose - and position auto pose_stamped = control::DroneHandler::getCurrentPose(); auto& position = pose_stamped.pose.position; @@ -103,6 +109,25 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { } } + + +void PositionHoldState::stateInit(ControlFSM& fsm) { + std::function obstacleAvoidanceCB = [this]()->void { + this->obstacle_avoidance_kicked_in_ = true; + }; + fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); + + + // Set flag for obstacle avoidance + obstacle_avoidance_kicked_in_ = false; +} + +void PositionHoldState::stateEnd(ControlFSM& fsm, const EventData& event) { + // Set flag for obstacle avoidance + obstacle_avoidance_kicked_in_ = false; +} + + //Returns setpoint const mavros_msgs::PositionTarget* PositionHoldState::getSetpointPtr() { setpoint_.header.stamp = ros::Time::now(); @@ -124,4 +149,3 @@ ascend_msgs::ControlFSMState PositionHoldState::getStateMsg() { } - From 5c7b0b26a333aa5a319a13e36378c16bee2b4e8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 8 Mar 2018 21:31:03 +0100 Subject: [PATCH 139/200] Update unit test --- test/utest.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/test/utest.cpp b/test/utest.cpp index 555e962..2f8846f 100644 --- a/test/utest.cpp +++ b/test/utest.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include "gtest/gtest.h" #include @@ -78,7 +79,9 @@ TEST(ControlTest, quatConversionTest) { } TEST(ControlTest, obstacleAvoidanceHelpers) { - + + using obstacle_math::angleWrapper; + using obstacle_math::rotateXY; // angleWrapper EXPECT_NEAR(0.f, angleWrapper(0.f), 0.001); EXPECT_NEAR(2.f, angleWrapper(2.f), 0.001); @@ -92,6 +95,7 @@ TEST(ControlTest, obstacleAvoidanceHelpers) { geometry_msgs::Vector3 zero = {}; geometry_msgs::Vector3 unitx; unitx.x=1.0; unitx.y=0.0; unitx.z=0.0; geometry_msgs::Vector3 unity; unity.x=0.0; unity.y=1.0; unity.z=0.0; + geometry_msgs::Vector3 unitz; unitz.x=0.0; unitz.y=0.0; unitz.z=1.0; // rotateXY From f1396c96a3110639a37a34392ac056980b1ad49f Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 8 Mar 2018 21:53:44 +0100 Subject: [PATCH 140/200] Add loopState to positionhold --- include/control/fsm/position_hold_state.hpp | 1 + src/fsm/position_hold_state.cpp | 25 +++++++++++++-------- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/include/control/fsm/position_hold_state.hpp b/include/control/fsm/position_hold_state.hpp index c844d5c..ec07af3 100644 --- a/include/control/fsm/position_hold_state.hpp +++ b/include/control/fsm/position_hold_state.hpp @@ -13,6 +13,7 @@ class PositionHoldState : public StateInterface { PositionHoldState(); void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; + void loopState(ControlFSM& fsm) override; void stateEnd(ControlFSM& fsm, const EventData& event) override; void stateInit(ControlFSM& fsm) override; std::string getStateName() const override { return "Position hold"; } diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index b0599ba..28ab62a 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -45,6 +45,18 @@ void PositionHoldState::handleEvent(ControlFSM& fsm, const EventData& event) { } } +void PositionHoldState::stateInit(ControlFSM& fsm) { + std::function obstacleAvoidanceCB = [this]()->void { + ROS_WARN_THROTTLE(1, "[control_fsm/positionholdstate] Obstacle avoidance kicked in"); + this->obstacle_avoidance_kicked_in_ = true; + }; + + fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); + + // Set flag for obstacle avoidance + obstacle_avoidance_kicked_in_ = false; +} + void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { //No need to check other commands if(event.isValidCMD()) { @@ -109,17 +121,12 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { } } +void PositionHoldState::loopState(ControlFSM& fsm){ + if (obstacle_avoidance_kicked_in_){ + // keep new setpoint after obstacle avoidance -void PositionHoldState::stateInit(ControlFSM& fsm) { - std::function obstacleAvoidanceCB = [this]()->void { - this->obstacle_avoidance_kicked_in_ = true; - }; - fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); - - - // Set flag for obstacle avoidance - obstacle_avoidance_kicked_in_ = false; + } } void PositionHoldState::stateEnd(ControlFSM& fsm, const EventData& event) { From e7fc569340b16c0e75b75e14e8d052ebd3a8c5f4 Mon Sep 17 00:00:00 2001 From: Marvin Reza Date: Thu, 8 Mar 2018 22:16:22 +0100 Subject: [PATCH 141/200] Fix position hold init and loop --- include/control/fsm/position_hold_state.hpp | 1 + include/control/fsm/state_interface.hpp | 36 ++++++++++----------- src/fsm/control_fsm.cpp | 2 +- src/fsm/position_hold_state.cpp | 9 ++++-- 4 files changed, 27 insertions(+), 21 deletions(-) diff --git a/include/control/fsm/position_hold_state.hpp b/include/control/fsm/position_hold_state.hpp index ec07af3..4681c97 100644 --- a/include/control/fsm/position_hold_state.hpp +++ b/include/control/fsm/position_hold_state.hpp @@ -20,6 +20,7 @@ class PositionHoldState : public StateInterface { ascend_msgs::ControlFSMState getStateMsg() override; const mavros_msgs::PositionTarget* getSetpointPtr() override; void handleManual(ControlFSM &fsm) override; + bool stateIsReady(ControlFSM &fsm) override; }; #endif diff --git a/include/control/fsm/state_interface.hpp b/include/control/fsm/state_interface.hpp index d91ac03..e746892 100644 --- a/include/control/fsm/state_interface.hpp +++ b/include/control/fsm/state_interface.hpp @@ -3,17 +3,17 @@ /** @page fsm_description FSM design * @brief Describes the design pattern - * + * * All states inherits the StateInterface class, and the - * FSM use polymorphism to switch between different states. - * - * On transition between states, the StateInterface::stateBegin method will be called in the new state. + * FSM use polymorphism to switch between different states. + * + * On transition between states, the StateInterface::stateBegin method will be called in the new state. * The active states StateInterface::loopState method will always run in a loop. * The StateInterface::stateEnd method will be called before the fsm transition to another state. - * - * Transitioning between states is done with the ControlFSM::transtionTo method. - * - * FSM is not async so do not run any blocking code + * + * Transitioning between states is done with the ControlFSM::transtionTo method. + * + * FSM is not async so do not run any blocking code * in any of these methods. * EventData is passed by reference and is NOT guaranteed to remain in scope. * DO NOT store event data by reference. @@ -34,7 +34,7 @@ class ControlFSM; ///Abstract interface class inherited by all states /* NOTE: -FSM is not async so do not run any blocking code +FSM is not async so do not run any blocking code in any of these methods. EventData is passed by reference and is NOT guaranteed to remain in scope. DO NOT store event data by reference. @@ -82,7 +82,7 @@ class StateInterface { ///Handles loss of offboard mode - must be implemented by state virtual void handleManual(ControlFSM& fsm) = 0; - + ///Runs on current state AFTER transition /**stateBegin is only implemented if needed by state.*/ virtual void stateBegin(ControlFSM& fsm, const EventData& event) {} @@ -90,27 +90,27 @@ class StateInterface { ///Runs on current state BEFORE transition /**stateEnd is only implemented if needed by state*/ virtual void stateEnd(ControlFSM& fsm, const EventData& event) {} - + ///Runs state specific code /**loopState is only implemented if needed by state*/ virtual void loopState(ControlFSM& fsm) {} - + ///Should return name of the state - used for debugging purposes virtual std::string getStateName() const = 0; - + ///Get state message - virtual ascend_msgs::ControlFSMState getStateMsg() = 0; - - ///Returning a valid setpoint from state + virtual ascend_msgs::ControlFSMState getStateMsg() = 0; + + ///Returning a valid setpoint from state /**Be aware - it's returned by const pointer - only return address of _setpoint.*/ virtual const mavros_msgs::PositionTarget* getSetpointPtr() = 0; ///Static interface returning iterator to first state static std::list::const_iterator cbegin() { return getAllStatesVector()->cbegin(); } - + ///Static interface returning iterator to last + 1 state static std::list::const_iterator cend() { return getAllStatesVector()->cend(); } - + ///Returns number of instanciated states static size_t getNumStates() { return getAllStatesVector()->size(); } }; diff --git a/src/fsm/control_fsm.cpp b/src/fsm/control_fsm.cpp index 4ccfcfe..1adccc4 100644 --- a/src/fsm/control_fsm.cpp +++ b/src/fsm/control_fsm.cpp @@ -118,7 +118,7 @@ bool ControlFSM::isReady() { if (subscribers_.mavros_state_changed_sub.getNumPublishers() <= 0) { control::handleWarnMsg("Preflight Check: No valid mavros state data!"); return false; - } + } //Land detector must be ready if (!LandDetector::getSharedInstancePtr()->isReady()) { control::handleWarnMsg("Preflight Check: No valid land detector data!"); diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index 28ab62a..f2044a1 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -47,7 +47,6 @@ void PositionHoldState::handleEvent(ControlFSM& fsm, const EventData& event) { void PositionHoldState::stateInit(ControlFSM& fsm) { std::function obstacleAvoidanceCB = [this]()->void { - ROS_WARN_THROTTLE(1, "[control_fsm/positionholdstate] Obstacle avoidance kicked in"); this->obstacle_avoidance_kicked_in_ = true; }; @@ -55,6 +54,7 @@ void PositionHoldState::stateInit(ControlFSM& fsm) { // Set flag for obstacle avoidance obstacle_avoidance_kicked_in_ = false; + } void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { @@ -124,6 +124,9 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { void PositionHoldState::loopState(ControlFSM& fsm){ if (obstacle_avoidance_kicked_in_){ // keep new setpoint after obstacle avoidance + ROS_WARN_THROTTLE(1, "[control_fsm/positionholdstate] Obstacle avoidance kicked in"); + + } @@ -155,4 +158,6 @@ ascend_msgs::ControlFSMState PositionHoldState::getStateMsg() { return msg; } - +bool PositionHoldState::stateIsReady(ControlFSM& fsm){ + return true; +} From 8ff4e7e196ebd33f212230e0d8fc4bcb4596a0f2 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 9 Mar 2018 11:52:37 +0100 Subject: [PATCH 142/200] Change position on obstacle avoidance in poshold --- src/fsm/position_hold_state.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index 28ab62a..03cb60a 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -123,9 +123,14 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { void PositionHoldState::loopState(ControlFSM& fsm){ if (obstacle_avoidance_kicked_in_){ - // keep new setpoint after obstacle avoidance - + //TODO: logic to avoid being pushed around + // keep new setpoint after obstacle avoidance + auto pose_stamped = control::DroneHandler::getCurrentPose(); + auto& position = pose_stamped.pose.position; + //Set setpoint to current position + setpoint_.position.x = position.x; + setpoint_.position.y = position.y; } } From 9a7b10f48a537c8a8acbdaa78c710baa5355abe7 Mon Sep 17 00:00:00 2001 From: atussa Date: Fri, 9 Mar 2018 12:40:06 +0100 Subject: [PATCH 143/200] Add variable for turning debug printing on and off --- src/path_planner/path_planner.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 46ba39f..b2b03d9 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -4,6 +4,8 @@ using namespace control::pathplanner; +bool debug = true; + PathPlanner::PathPlanner(float obstacle_radius, float node_distance) : obstacle_radius(obstacle_radius), node_distance(node_distance){ @@ -20,7 +22,7 @@ int PathPlanner::coordToIndex(float coord) { } void PathPlanner::initializeGraph(){ - std::cout << "Init" << std::endl; + if (debug) {std::cout << "Init" << std::endl;} for (float x = 0; x < FIELD_LENGTH; x += node_distance){ for (float y = 0; y < FIELD_LENGTH; y += node_distance){ if(isStart(x,y)){ @@ -66,7 +68,7 @@ void PathPlanner::addObstacle(float center_x, float center_y) { } void PathPlanner::refreshObstacles() { - std::cout << "refreshObstacles" << std::endl; + if (debug) {std::cout << "refreshObstacles" << std::endl;} if(no_plan){ return; } @@ -111,7 +113,7 @@ void PathPlanner::addUnsafeZone(float center_x, float center_y) { } void PathPlanner::refreshUnsafeZones(){ - std::cout << "refreshUnsafeZones" << std::endl; + if (debug) {std::cout << "refreshUnsafeZones" << std::endl;} if(no_plan){ return; } @@ -152,7 +154,7 @@ float PathPlanner::calculateEuclidianHeuristic(float x, float y) { }*/ void PathPlanner::relaxGraph(){ - std::cout << "relaxGraph" << std::endl; + if (debug) {std::cout << "relaxGraph" << std::endl;} Node current_node; while(!open_list.empty()){ // Always search from the node with lowest f value @@ -270,7 +272,7 @@ bool PathPlanner::canSimplifyLine(float x1, float y1, float x2, float y2) { } Obstacle* PathPlanner::findBlockingObstacle(float current_x, float current_y) { - std::cout << "Search for blocking obstacle" << std::endl; + if (debug) {std::cout << "Search for blocking obstacle" << std::endl;} for (auto it = obstacles.begin(); it != obstacles.end(); it++) { if(abs(current_x-it->x) <= obstacle_radius*2 && abs(current_y-it->y) <= obstacle_radius*2) { std::cout << "Found blocking obstacle" << std::endl; @@ -283,7 +285,7 @@ Obstacle* PathPlanner::findBlockingObstacle(float current_x, float current_y) { void PathPlanner::makePlan(float current_x, float current_y, float target_x, float target_y) { - std::cout << "Run makePlan" << std::endl; + if (debug) {std::cout << "Run makePlan" << std::endl;} no_plan = false; bool escape_from_obstacle = false; From 26619d5f6604579fd2c56c7ad70b277a89a5ec7e Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Sun, 11 Mar 2018 17:19:13 +0100 Subject: [PATCH 144/200] Implement responsibility flag, take 1 --- include/control/tools/obstacle_avoidance.hpp | 30 +++++++------------- src/tools/obstacle_avoidance.cpp | 15 +++++++--- 2 files changed, 21 insertions(+), 24 deletions(-) diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index 0b55c28..b5fe222 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -31,12 +31,15 @@ class ObstacleAvoidance { ///Runs obstacle avoidance, and modifies setpoint (and notifies) virtual bool doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint); - ///Node handle + ///Nodehandle ros::NodeHandle nh_; ///Publisher for avoid zone information ros::Publisher zone_pub_; + ///Responsibility flag + bool has_setpoint_responsibility_ = true; + public: ///Default constructor ObstacleAvoidance(); @@ -52,6 +55,12 @@ class ObstacleAvoidance { ///Remove on modified callback void removeOnModifiedCBPtr(const std::shared_ptr >& cb_p); + ///Obstacle avoidance has reduced responsibility. + ///It won't change setpoints and only calls warning level callbacks + void relaxResponsibility() { has_setpoint_responsibility_ = false; } + ///Obstacle avoidance takes full responsibility over setpoints and calls all callbacks + void takeResponsibility() { has_setpoint_responsibility_ = true; } + ///Modifies setpoint if obstacle is too close mavros_msgs::PositionTarget run(mavros_msgs::PositionTarget setpoint); ///Returns true when obstacle avoidance is running @@ -59,23 +68,4 @@ class ObstacleAvoidance { }; } -// Only do this for unit testing -#ifdef CONTROL_FSM_UNIT_TEST -#include - -float angleWrapper(const float angle); - -template -inline geometry_msgs::Vector3 calcVectorBetweenPoints(const T& point_A, const K& point_B); - -template -inline float calcDistanceToObstacle(const T& point, const K& obstacle_position); - -template -inline float calcAngleToObstacle(const T& point, const K& obstacle_position, const float obstacle_direction); - -template -inline geometry_msgs::Vector3 rotateXY(const T& point, const float angle); -#endif - #endif diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 7ab7f81..321ac7d 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -181,13 +181,20 @@ void control::ObstacleAvoidance::onWarn() { } mavros_msgs::PositionTarget control::ObstacleAvoidance::run(mavros_msgs::PositionTarget setpoint) { - //If obstacle avoidance has altered the setpoint - if(doObstacleAvoidance(&setpoint)) { + mavros_msgs::PositionTarget setpoint_unchanged = setpoint; + + const bool setpoint_modified = doObstacleAvoidance(&setpoint); + + if (setpoint_modified){ //Notify states - onModified(); onWarn(); + if (has_setpoint_responsibility_){ + onModified(); + return setpoint; + } } - return setpoint; + + return setpoint_unchanged; } void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr >& cb_p) { From d6aafd6191cb8a80866ee4238f91554066cc117e Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 12 Mar 2018 21:12:01 +0100 Subject: [PATCH 145/200] Add debug printing --- src/path_planner/path_planner.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index b2b03d9..2c006cf 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -332,6 +332,8 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo struct Point {float x,y;}; Point current_point{current_x, current_y}; float drone_dir = obstacle_math::calcAngleToObstacle(current_point, *blocking_obstacle, obstacle_dir); + + std::cout << "Drone direction: " << drone_dir << std::endl; // Find setpoint in right direction outside unsafe zone // The safe zone is obstacle_radius*2, but to ensure that @@ -344,6 +346,8 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo std::cout << "Cannot find safe point!" << std::endl; return; } + std::cout << "Safe point: " << safe_x << ", " << safe_y << std::endl; + // Add starting point to plan and plan a new plan from the // setpoint to target From bb97002b2e7a21c260f4a561edac8091c123b7d7 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 12 Mar 2018 21:28:05 +0100 Subject: [PATCH 146/200] FIX FATAL BUG --- src/nodes/path_planner_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index bb1b816..8c0401a 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -86,6 +86,8 @@ int main(int argc, char** argv){ auto current_dir = obstacles_msg.direction.begin(); while(current_coord != obstacles_msg.global_robot_position.end() && current_dir != obstacles_msg.direction.end()){ obstacles.push_back(control::pathplanner::Obstacle{current_coord->x, current_coord->y,*current_dir}); + current_coord++; + current_dir++; } plan.setObstacles(obstacles); From a1a5e412cd975bb5cad0a12cc24ce3de782570dc Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 12 Mar 2018 21:48:23 +0100 Subject: [PATCH 147/200] Use at instead of [] --- src/path_planner/path_planner.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 2c006cf..0d0fdb6 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -386,8 +386,8 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo while(!isStart(x, y)){ // parent is calculated from the endpoint index, this node might have a different // coordinate (depending on the grid size) - float x_temp = graph[x_index][y_index].getParentX(); - float y_temp = graph[x_index][y_index].getParentY(); + float x_temp = graph.at(x_index).at(y_index).getParentX(); + float y_temp = graph.at(x_index).at(y_index).getParentY(); x = x_temp; y = y_temp; @@ -396,7 +396,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo counter ++; if(counter >200){break;} - plan.push_front(graph[x_index][y_index]); + plan.push_front(graph.at(x_index).at(y_index)); } From a97488bb6177ebbbb0148c723b6e4b2766fe6e10 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 12 Mar 2018 22:21:24 +0100 Subject: [PATCH 148/200] Debug printing --- src/path_planner/path_planner.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 0d0fdb6..14addf8 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -182,8 +182,8 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent graph[x_index][y_index].setParentX(parent_x); graph[x_index][y_index].setParentY(parent_y); destination_reached = true; - /*std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; - std::cout << "Destination parent: x=" << graph[x_index][y_index].getParentX() + std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; + /*std::cout << "Destination parent: x=" << graph[x_index][y_index].getParentX() << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl;*/ } // If node is not destination and hasn't been searched yet @@ -311,7 +311,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo refreshObstacles(); refreshUnsafeZones(); - + /* if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { std::cout << "\n GET OOOOOUT!!! \n\n"; escape_from_obstacle = true; @@ -358,7 +358,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo initializeGraph(); refreshObstacles(); refreshUnsafeZones(); - } + }*/ float x = end_node.getX(); float y = end_node.getY(); @@ -386,14 +386,18 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo while(!isStart(x, y)){ // parent is calculated from the endpoint index, this node might have a different // coordinate (depending on the grid size) - float x_temp = graph.at(x_index).at(y_index).getParentX(); - float y_temp = graph.at(x_index).at(y_index).getParentY(); - x = x_temp; - y = y_temp; - + x = graph.at(x_index).at(y_index).getParentX(); + y = graph.at(x_index).at(y_index).getParentY(); + x_index = coordToIndex(x); y_index = coordToIndex(y); + if(!isValidCoordinate(x,y)) { + std::cout << "Invalid parent: " << x << ", " << y << std::endl; + std::cout << "Index: " << x_index << ", " << y_index << std::endl; + } + + counter ++; if(counter >200){break;} plan.push_front(graph.at(x_index).at(y_index)); From 18ef1debf42853b97b11120db713d5b47d002336 Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 12 Mar 2018 22:33:53 +0100 Subject: [PATCH 149/200] Debug printing --- src/path_planner/path_planner.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 14addf8..a5ccc16 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -172,6 +172,7 @@ void PathPlanner::relaxGraph(){ } void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent_y, float distance_to_parent) { + std::cout << "Successor = " << x << ", " << y << std::endl; if(isValidCoordinate(x,y)){ int x_index = coordToIndex(x); int y_index = coordToIndex(y); @@ -206,6 +207,7 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent } void PathPlanner::handleAllSuccessors(float x, float y) { + std::cout << "q = " << x << ", " << y << std::endl; handleSuccessor(x+node_distance,y, x, y, node_distance); if(destination_reached){return;} handleSuccessor(x+node_distance, y+node_distance, x, y, diagonal_node_distance); @@ -389,14 +391,14 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo x = graph.at(x_index).at(y_index).getParentX(); y = graph.at(x_index).at(y_index).getParentY(); - x_index = coordToIndex(x); - y_index = coordToIndex(y); if(!isValidCoordinate(x,y)) { std::cout << "Invalid parent: " << x << ", " << y << std::endl; std::cout << "Index: " << x_index << ", " << y_index << std::endl; } + x_index = coordToIndex(x); + y_index = coordToIndex(y); counter ++; if(counter >200){break;} From 6217aa24b671afca298da51b6e59667dd843223f Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 12 Mar 2018 23:22:05 +0100 Subject: [PATCH 150/200] Debug printing + return from fct if point in unsafe zone --- include/control/planner/path_planner.hpp | 1 + src/nodes/path_planner_node.cpp | 8 ++++---- src/path_planner/path_planner.cpp | 7 ++++++- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 4b6c007..fc0af93 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -24,6 +24,7 @@ struct Obstacle { float x; float y; float dir; + Obstacle(float x, float y, float dir) : x(x), y(y), dir(dir){} }; class PathPlanner; diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 8c0401a..812b004 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -51,7 +51,7 @@ int main(int argc, char** argv){ ros::init(argc, argv, "path_planner_server"); ros::NodeHandle n; - ros::Rate rate(1); + ros::Rate rate(5); ROS_INFO("Path planner node started"); @@ -85,7 +85,7 @@ int main(int argc, char** argv){ auto current_coord = obstacles_msg.global_robot_position.begin(); auto current_dir = obstacles_msg.direction.begin(); while(current_coord != obstacles_msg.global_robot_position.end() && current_dir != obstacles_msg.direction.end()){ - obstacles.push_back(control::pathplanner::Obstacle{current_coord->x, current_coord->y,*current_dir}); + obstacles.push_back(control::pathplanner::Obstacle(current_coord->x, current_coord->y,*current_dir)); current_coord++; current_dir++; } @@ -101,8 +101,6 @@ int main(int argc, char** argv){ planner_state.current_y = position.y; } - bool is_plan_updated = plan.removeOldPoints(planner_state.current_x, planner_state.current_y); - // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed if(planner_state.make_plan && (!plan.isPlanSafe(planner_state.current_x,planner_state.current_y) || planner_state.new_goal)){ ROS_INFO("Make new plan."); @@ -135,6 +133,8 @@ int main(int argc, char** argv){ } } + bool is_plan_updated = plan.removeOldPoints(planner_state.current_x, planner_state.current_y); + // Publish plan as long as a plan is requested if(planner_state.make_plan){ if(is_plan_updated){ diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index a5ccc16..4e48da7 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -313,6 +313,11 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo refreshObstacles(); refreshUnsafeZones(); + if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { + std::cout << "\n GET OOOOOUT!!! \n\n"; + return; + } + /* if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { std::cout << "\n GET OOOOOUT!!! \n\n"; @@ -377,7 +382,7 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo // Calculate all f values and set the parents relaxGraph(); - // Dummy safety for avoiding infinite loop, will remove later + // Dummy safety for avoiding infinite loop, will remove never int counter = 0; // Go through the graph from end to start through the parents of each node // Add nodes to the plan From 1414ee3b81714adca2b9c6764f8494488c1e5ce7 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Mon, 12 Mar 2018 23:26:01 +0100 Subject: [PATCH 151/200] Change land detector --- launch/test_control_fsm_sim_planner.launch | 2 ++ src/nodes/path_planner_node.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/launch/test_control_fsm_sim_planner.launch b/launch/test_control_fsm_sim_planner.launch index 5cd6482..0b21423 100644 --- a/launch/test_control_fsm_sim_planner.launch +++ b/launch/test_control_fsm_sim_planner.launch @@ -9,5 +9,7 @@ + + diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 8c0401a..6788744 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -172,4 +172,4 @@ int main(int argc, char** argv){ ros::spin(); -} \ No newline at end of file +} From 338faedb1417de27b8209ad5ddf946ccfca0a93e Mon Sep 17 00:00:00 2001 From: atussa Date: Mon, 12 Mar 2018 23:34:24 +0100 Subject: [PATCH 152/200] Fix bug --- src/path_planner/path_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 4e48da7..76c924b 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -126,7 +126,7 @@ void PathPlanner::refreshUnsafeZones(){ for(auto current = obstacles.begin(); current != obstacles.end(); current++){ - addObstacle(current->x, current->y); + addUnsafeZone(current->x, current->y); } } From 61d313b6ad4d4d7da49febf6362d15a7336c38c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Tue, 13 Mar 2018 20:18:37 +0100 Subject: [PATCH 153/200] Always take responsibility on state end, fix poshold --- src/fsm/control_fsm.cpp | 3 ++- src/fsm/position_hold_state.cpp | 5 ++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/fsm/control_fsm.cpp b/src/fsm/control_fsm.cpp index ca86f27..1eaafb2 100644 --- a/src/fsm/control_fsm.cpp +++ b/src/fsm/control_fsm.cpp @@ -42,7 +42,8 @@ void ControlFSM::transitionTo(StateInterface& state, StateInterface* caller_p, c } void ControlFSM::releaseCommonStateResources() { - //Add resources here + //Force return of responsibility + obstacle_avoidance_.takeResponsibility(); } //Send external event to current state and to "next" state diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index d0b2061..cf87b69 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -47,6 +47,7 @@ void PositionHoldState::handleEvent(ControlFSM& fsm, const EventData& event) { void PositionHoldState::stateInit(ControlFSM& fsm) { std::function obstacleAvoidanceCB = [this]()->void { + //TODO Why not set position setpoint to current position here?? this->obstacle_avoidance_kicked_in_ = true; }; @@ -126,12 +127,14 @@ void PositionHoldState::loopState(ControlFSM& fsm){ //TODO: logic to avoid being pushed around // keep new setpoint after obstacle avoidance - auto pose_stamped = control::DroneHandler::getCurrentPose(); + auto pose_stamped = control::DroneHandler::getCurrentPose(); auto& position = pose_stamped.pose.position; //Set setpoint to current position setpoint_.position.x = position.x; setpoint_.position.y = position.y; } + //Only react once + obstacle_avoidance_kicked_in_ = false; } void PositionHoldState::stateEnd(ControlFSM& fsm, const EventData& event) { From a40cc0c84c1fcf82665f03caca2428fe7fae9226 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Wed, 14 Mar 2018 09:18:48 +0100 Subject: [PATCH 154/200] implement obstacle avoidance in land_state --- include/control/fsm/land_state.hpp | 3 +++ src/fsm/land_state.cpp | 23 +++++++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/include/control/fsm/land_state.hpp b/include/control/fsm/land_state.hpp index 74e51d5..215ae11 100644 --- a/include/control/fsm/land_state.hpp +++ b/include/control/fsm/land_state.hpp @@ -6,10 +6,13 @@ class LandState : public StateInterface { private: EventData cmd_; + + bool obstacle_avoidance_kicked_in_ = false; public: LandState(); void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; + void stateInit(ControlFSM& fsm) override; void loopState(ControlFSM& fsm) override; std::string getStateName() const override { return "Land"; } ascend_msgs::ControlFSMState getStateMsg() const override; diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index 5a4a681..1b660f8 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -46,6 +46,10 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { if(!control::DroneHandler::isPoseValid()) { throw control::PoseNotValidException(); } + + //reset obstacle avoidance flag + obstacle_avoidance_kicked_in_ = false; + auto pose_stamped = control::DroneHandler::getCurrentPose(); auto& position = pose_stamped.pose.position; //Position XY is ignored in typemask, but the values are set as a precaution. @@ -93,6 +97,13 @@ void LandState::loopState(ControlFSM& fsm) { } else { setpoint_.type_mask = default_mask | SETPOINT_TYPE_LAND | IGNORE_PX | IGNORE_PY; } + + + if (obstacle_avoidance_kicked_in_){ + RequestEvent abort_event(RequestType::ABORT); + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + } + //Check landing if(LandDetector::isOnGround()) { if(cmd_.isValidCMD()) { @@ -113,6 +124,18 @@ void LandState::loopState(ControlFSM& fsm) { } } + +//Initialize state +void LandState::stateInit(ControlFSM& fsm) { + std::function obstacleAvoidanceCB = [this]()->void { + this->obstacle_avoidance_kicked_in_ = true; + }; + fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); + + control::handleInfoMsg("Land init completed!"); +} + + const mavros_msgs::PositionTarget* LandState::getSetpointPtr() { setpoint_.header.stamp = ros::Time::now(); return &setpoint_; From d3d9e69e44aab5bc7b7babf6620a913d5ba5f5c0 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Wed, 14 Mar 2018 09:19:21 +0100 Subject: [PATCH 155/200] Logic error, land should be checked and aborted in stateBegin! This reverts commit a40cc0c84c1fcf82665f03caca2428fe7fae9226. --- include/control/fsm/land_state.hpp | 3 --- src/fsm/land_state.cpp | 23 ----------------------- 2 files changed, 26 deletions(-) diff --git a/include/control/fsm/land_state.hpp b/include/control/fsm/land_state.hpp index 215ae11..74e51d5 100644 --- a/include/control/fsm/land_state.hpp +++ b/include/control/fsm/land_state.hpp @@ -6,13 +6,10 @@ class LandState : public StateInterface { private: EventData cmd_; - - bool obstacle_avoidance_kicked_in_ = false; public: LandState(); void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; - void stateInit(ControlFSM& fsm) override; void loopState(ControlFSM& fsm) override; std::string getStateName() const override { return "Land"; } ascend_msgs::ControlFSMState getStateMsg() const override; diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index 1b660f8..5a4a681 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -46,10 +46,6 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { if(!control::DroneHandler::isPoseValid()) { throw control::PoseNotValidException(); } - - //reset obstacle avoidance flag - obstacle_avoidance_kicked_in_ = false; - auto pose_stamped = control::DroneHandler::getCurrentPose(); auto& position = pose_stamped.pose.position; //Position XY is ignored in typemask, but the values are set as a precaution. @@ -97,13 +93,6 @@ void LandState::loopState(ControlFSM& fsm) { } else { setpoint_.type_mask = default_mask | SETPOINT_TYPE_LAND | IGNORE_PX | IGNORE_PY; } - - - if (obstacle_avoidance_kicked_in_){ - RequestEvent abort_event(RequestType::ABORT); - fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); - } - //Check landing if(LandDetector::isOnGround()) { if(cmd_.isValidCMD()) { @@ -124,18 +113,6 @@ void LandState::loopState(ControlFSM& fsm) { } } - -//Initialize state -void LandState::stateInit(ControlFSM& fsm) { - std::function obstacleAvoidanceCB = [this]()->void { - this->obstacle_avoidance_kicked_in_ = true; - }; - fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); - - control::handleInfoMsg("Land init completed!"); -} - - const mavros_msgs::PositionTarget* LandState::getSetpointPtr() { setpoint_.header.stamp = ros::Time::now(); return &setpoint_; From 2938c5d317125e1afdfd3a6d247c5de61b6ca12a Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Wed, 14 Mar 2018 09:20:43 +0100 Subject: [PATCH 156/200] Revert "Logic error, land should be checked and aborted in stateBegin!" This reverts commit d3d9e69e44aab5bc7b7babf6620a913d5ba5f5c0. --- include/control/fsm/land_state.hpp | 3 +++ src/fsm/land_state.cpp | 23 +++++++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/include/control/fsm/land_state.hpp b/include/control/fsm/land_state.hpp index 74e51d5..215ae11 100644 --- a/include/control/fsm/land_state.hpp +++ b/include/control/fsm/land_state.hpp @@ -6,10 +6,13 @@ class LandState : public StateInterface { private: EventData cmd_; + + bool obstacle_avoidance_kicked_in_ = false; public: LandState(); void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; + void stateInit(ControlFSM& fsm) override; void loopState(ControlFSM& fsm) override; std::string getStateName() const override { return "Land"; } ascend_msgs::ControlFSMState getStateMsg() const override; diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index 5a4a681..1b660f8 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -46,6 +46,10 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { if(!control::DroneHandler::isPoseValid()) { throw control::PoseNotValidException(); } + + //reset obstacle avoidance flag + obstacle_avoidance_kicked_in_ = false; + auto pose_stamped = control::DroneHandler::getCurrentPose(); auto& position = pose_stamped.pose.position; //Position XY is ignored in typemask, but the values are set as a precaution. @@ -93,6 +97,13 @@ void LandState::loopState(ControlFSM& fsm) { } else { setpoint_.type_mask = default_mask | SETPOINT_TYPE_LAND | IGNORE_PX | IGNORE_PY; } + + + if (obstacle_avoidance_kicked_in_){ + RequestEvent abort_event(RequestType::ABORT); + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + } + //Check landing if(LandDetector::isOnGround()) { if(cmd_.isValidCMD()) { @@ -113,6 +124,18 @@ void LandState::loopState(ControlFSM& fsm) { } } + +//Initialize state +void LandState::stateInit(ControlFSM& fsm) { + std::function obstacleAvoidanceCB = [this]()->void { + this->obstacle_avoidance_kicked_in_ = true; + }; + fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); + + control::handleInfoMsg("Land init completed!"); +} + + const mavros_msgs::PositionTarget* LandState::getSetpointPtr() { setpoint_.header.stamp = ros::Time::now(); return &setpoint_; From 59b7e8200e6edcc64d0ed5304c078cd90d091c9a Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Mar 2018 01:24:15 +0100 Subject: [PATCH 157/200] Add min/max config params to obstacle avoidance --- include/control/tools/config.hpp | 9 ++++++--- src/tools/config.cpp | 18 ++++++++++++------ src/tools/obstacle_avoidance.cpp | 16 ++++++++-------- 3 files changed, 26 insertions(+), 17 deletions(-) diff --git a/include/control/tools/config.hpp b/include/control/tools/config.hpp index bb1f9de..29cf387 100644 --- a/include/control/tools/config.hpp +++ b/include/control/tools/config.hpp @@ -82,11 +82,14 @@ class Config { /// \fsmparam Drone safezone static double obstacle_too_close_dist; /// \fsmparam Clearance osbtacle avoidance to right and left of obstacle - static double obstacle_clearance_side; + static double obstacle_clearance_side_max; + static double obstacle_clearance_side_min; /// \fsmparam Clearance osbtacle avoidance in front of obstacle - static double obstacle_clearance_front; + static double obstacle_clearance_front_max; + static double obstacle_clearance_front_min; /// \fsmparam Clearance osbtacle avoidance behind obstacle - static double obstacle_clearance_back; + static double obstacle_clearance_back_max; + static double obstacle_clearance_back_min; /// \fsmparam Radius in which obstacle collisions will be checked for static double obstacle_clearance_checkradius; /// \fsmparam Topic to listen for info about obstacles diff --git a/src/tools/config.cpp b/src/tools/config.cpp index d1d1e39..461a10a 100644 --- a/src/tools/config.cpp +++ b/src/tools/config.cpp @@ -42,9 +42,12 @@ double Config::obstacle_too_close_dist = 2.0; std::string Config::lidar_topic = "perception/obstacles/lidar"; bool Config::require_all_data_streams = true; bool Config::require_obstacle_detection = true; -double Config::obstacle_clearance_side = 2.0f; -double Config::obstacle_clearance_front = 2.0f; -double Config::obstacle_clearance_back = 2.0f; +double Config::obstacle_clearance_side_max = 2.0f; +double Config::obstacle_clearance_side_min = 1.0f; +double Config::obstacle_clearance_front_max = 2.0f; +double Config::obstacle_clearance_front_min = 1.4f; +double Config::obstacle_clearance_back_max = 2.0f; +double Config::obstacle_clearance_back_min = 1.0f; double Config::obstacle_clearance_checkradius = 2.0f; void Config::loadParams() { @@ -148,9 +151,12 @@ void Config::loadParams() { //Obstacles getStringParam("obstacle_state_topic", obstacle_state_topic); //Obstacle avoidance - getDoubleParam("obstacle_clearance_side", obstacle_clearance_side, 0, 100); - getDoubleParam("obstacle_clearance_front", obstacle_clearance_front, 0, 100); - getDoubleParam("obstacle_clearance_back", obstacle_clearance_back, 0, 100); + getDoubleParam("obstacle_clearance_side_max", obstacle_clearance_side_max, 0, 100); + getDoubleParam("obstacle_clearance_side_min", obstacle_clearance_side_min, 0, 100); + getDoubleParam("obstacle_clearance_front_max", obstacle_clearance_front_max, 0, 100); + getDoubleParam("obstacle_clearance_front_min", obstacle_clearance_front_min, 0, 100); + getDoubleParam("obstacle_clearance_back_max", obstacle_clearance_back_max, 0, 100); + getDoubleParam("obstacle_clearance_back_min", obstacle_clearance_back_min, 0, 100); getDoubleParam("obstacle_clearance_checkradius", obstacle_clearance_checkradius, 0, 100); //Action server getStringParam("action_server_topic", action_server_topic); diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 321ac7d..7f9aa85 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -68,7 +68,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); - const float drone_speed_ratio = std::min((float)std::sqrt(drone_speed/3.f), 1.f); // 3.f m/s is assumed drone max speed + const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed ascend_msgs::PolygonArray zone_msg; @@ -91,13 +91,13 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget //(drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && //setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); - // magic numbers are minimum clearance no matter what. - // TODO: Turn into parameters and include - // this logic in the avoidZone function - //ROS_INFO_THROTTLE("Drone speed ratio: %.3f", drone_speed_ratio); - const auto clearance_front = std::max((float)Config::obstacle_clearance_front*drone_speed_ratio, 1.3f); - const auto clearance_back = std::max((float)Config::obstacle_clearance_back*drone_speed_ratio, 1.f); - const auto clearance_side = std::max((float)Config::obstacle_clearance_side*drone_speed_ratio, 1.f); + // TODO: this logic in the avoidZone function + const auto clearance_front = std::max(Config::obstacle_clearance_front_max*drone_speed_ratio, + Config::obstacle_clearance_front_min); + const auto clearance_back = std::max(Config::obstacle_clearance_back_max*drone_speed_ratio, + Config::obstacle_clearance_back_min); + const auto clearance_side = std::max(Config::obstacle_clearance_side_max*drone_speed_ratio, + Config::obstacle_clearance_side_min); geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, clearance_front, clearance_back, clearance_side); From 1a388e4a986a7ea398bcbf10651bef634e6885df Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Mar 2018 01:25:32 +0100 Subject: [PATCH 158/200] Add defaults for new params --- control_fsm_default_config.yaml | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index 7ca04ff..959dfc8 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -62,11 +62,14 @@ #Margin to determine if velocity is reached velocity_reached_margin: 0.2, #Obstacle avoidance clearance on sides - obstacle_clearance_side: 1.8, + obstacle_clearance_side_max: 1.8, + obstacle_clearance_side_min: 1.0, #Obstacle avoidance clearance in front - obstacle_clearance_front: 3.5, + obstacle_clearance_front_max: 2.5, + obstacle_clearance_front_min: 1.5, #Obstacle avoidance clearance in back - obstacle_clearance_back: 2.0, + obstacle_clearance_back_max: 2.0, + obstacle_clearance_back_min: 1.0, #Obstacle avoidance checkradius obstacle_clearance_checkradius: 100.0 } From b1519b0543a27d3911a3948c485b25b082b1dcbb Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Mar 2018 01:28:10 +0100 Subject: [PATCH 159/200] Remove stateEnd since it is no longer needed --- include/control/fsm/go_to_state.hpp | 2 -- include/control/fsm/position_hold_state.hpp | 1 - 2 files changed, 3 deletions(-) diff --git a/include/control/fsm/go_to_state.hpp b/include/control/fsm/go_to_state.hpp index 28fa53a..c472d89 100644 --- a/include/control/fsm/go_to_state.hpp +++ b/include/control/fsm/go_to_state.hpp @@ -32,8 +32,6 @@ class GoToState : public StateInterface { void stateBegin(ControlFSM& fsm, const EventData& event) override; // Poll drone position and set setpoint void loopState(ControlFSM& fsm) override; - // - void stateEnd(ControlFSM& fsm, const EventData& event) override; std::string getStateName() const override { return "GoTo";} ascend_msgs::ControlFSMState getStateMsg() const override; const mavros_msgs::PositionTarget* getSetpointPtr() override; diff --git a/include/control/fsm/position_hold_state.hpp b/include/control/fsm/position_hold_state.hpp index 3914492..30ab001 100644 --- a/include/control/fsm/position_hold_state.hpp +++ b/include/control/fsm/position_hold_state.hpp @@ -14,7 +14,6 @@ class PositionHoldState : public StateInterface { void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; void loopState(ControlFSM& fsm) override; - void stateEnd(ControlFSM& fsm, const EventData& event) override; void stateInit(ControlFSM& fsm) override; std::string getStateName() const override { return "Position hold"; } ascend_msgs::ControlFSMState getStateMsg() const override; From f133374f4ed3e31bb1048945b5d16040ce4aa463 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Mar 2018 01:29:41 +0100 Subject: [PATCH 160/200] Disable obstacle avoidance in land state. Only as a temporary solution until prediction is added to obstacle avoidance! --- src/fsm/go_to_state.cpp | 11 +++-------- src/fsm/land_state.cpp | 7 +++++++ src/fsm/position_hold_state.cpp | 10 +++------- 3 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index e573181..15c1e9a 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -132,11 +132,11 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { using control::getMavrosCorrectedTargetYaw; using control::DroneHandler; auto quat = DroneHandler::getCurrentPose().pose.orientation; - auto pos = DroneHandler::getCurrentPose().pose.position; + //auto pos = DroneHandler::getCurrentPose().pose.position; // calculate dx,dy - auto dx = setpoint_.position.x - pos.x; - auto dy = setpoint_.position.y - pos.y; + //auto dx = setpoint_.position.x - pos.x; + //auto dy = setpoint_.position.y - pos.y; setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(quat))); /* @@ -156,11 +156,6 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { } } -void GoToState::stateEnd(ControlFSM& fsm, const EventData& event) { - // Set obstacle avoidance flag on exit - obstacle_avoidance_kicked_in_ = false; -} - void GoToState::loopState(ControlFSM& fsm) { try { //Check that position data is valid diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index 5a4a681..3dc5831 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -62,6 +62,13 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { } } + + // TODO:Check with obstacle_avoidance if it is ok to land + if (true){ + // assuming obstacle_avoidance doesn't complain, disable it during landing + fsm.obstacle_avoidance_.relaxResponsibility(); + } + //Only land blind when the drone is below a certain altitude if(position.z >= control::Config::min_in_air_alt) { setpoint_.type_mask = default_mask | SETPOINT_TYPE_LAND; diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index cf87b69..5732b7a 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -48,6 +48,9 @@ void PositionHoldState::handleEvent(ControlFSM& fsm, const EventData& event) { void PositionHoldState::stateInit(ControlFSM& fsm) { std::function obstacleAvoidanceCB = [this]()->void { //TODO Why not set position setpoint to current position here?? + //NOTE: advantage: collect logic in one place (assuming it can be completely removed from loopState) + //NOTE: disadvantage: most callbacks in total don't do anything (are applied to inactive states) so might be better keep them minimal, but this is insignificant in this case, more control over timing if it placed in loopState (less dependent on OA implementation) + this->obstacle_avoidance_kicked_in_ = true; }; @@ -74,7 +77,6 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { // Set flag for obstacle avoidance obstacle_avoidance_kicked_in_ = false; - //Get pose - and position auto pose_stamped = control::DroneHandler::getCurrentPose(); auto& position = pose_stamped.pose.position; @@ -137,12 +139,6 @@ void PositionHoldState::loopState(ControlFSM& fsm){ obstacle_avoidance_kicked_in_ = false; } -void PositionHoldState::stateEnd(ControlFSM& fsm, const EventData& event) { - // Set flag for obstacle avoidance - obstacle_avoidance_kicked_in_ = false; -} - - //Returns setpoint const mavros_msgs::PositionTarget* PositionHoldState::getSetpointPtr() { setpoint_.header.stamp = ros::Time::now(); From fa2ad6fe6cc4305dbe68687b90835b9b2857031b Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 15 Mar 2018 02:01:30 +0100 Subject: [PATCH 161/200] add debug printout, move is_ready_ to be protected in state_interface --- include/control/fsm/land_state.hpp | 1 + include/control/fsm/state_interface.hpp | 5 +++-- include/control/tools/obstacle_avoidance.hpp | 10 ++++++++-- src/fsm/land_state.cpp | 5 +++++ 4 files changed, 17 insertions(+), 4 deletions(-) diff --git a/include/control/fsm/land_state.hpp b/include/control/fsm/land_state.hpp index 215ae11..9f641cc 100644 --- a/include/control/fsm/land_state.hpp +++ b/include/control/fsm/land_state.hpp @@ -13,6 +13,7 @@ class LandState : public StateInterface { void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; void stateInit(ControlFSM& fsm) override; + bool stateIsReady(ControlFSM &fsm) override; void loopState(ControlFSM& fsm) override; std::string getStateName() const override { return "Land"; } ascend_msgs::ControlFSMState getStateMsg() const override; diff --git a/include/control/fsm/state_interface.hpp b/include/control/fsm/state_interface.hpp index ef32000..d53fa4d 100644 --- a/include/control/fsm/state_interface.hpp +++ b/include/control/fsm/state_interface.hpp @@ -45,8 +45,6 @@ undefined behaviour. All state methods should gurantee nothrow! class StateInterface; class StateInterface { private: - ///Flag used to check if state is ready - should be set by state init - bool is_ready_ = false; /** * get vector of all instianciated states @@ -58,6 +56,9 @@ class StateInterface { ///Assigmnet operator should be removed StateInterface& operator=(const StateInterface&) = delete; protected: + ///Flag used to check if state is ready - should be set by state init + bool is_ready_ = false; + ///All states needs to return a valid setpoint mavros_msgs::PositionTarget setpoint_; public: diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index b5fe222..9aada42 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -57,9 +57,15 @@ class ObstacleAvoidance { ///Obstacle avoidance has reduced responsibility. ///It won't change setpoints and only calls warning level callbacks - void relaxResponsibility() { has_setpoint_responsibility_ = false; } + void relaxResponsibility() { + ROS_INFO("[obs_avoid] relaxing responsibility"); + has_setpoint_responsibility_ = false; + } ///Obstacle avoidance takes full responsibility over setpoints and calls all callbacks - void takeResponsibility() { has_setpoint_responsibility_ = true; } + void takeResponsibility() { + ROS_INFO("[obs_avoid] taking responsibility"); + has_setpoint_responsibility_ = true; + } ///Modifies setpoint if obstacle is too close mavros_msgs::PositionTarget run(mavros_msgs::PositionTarget setpoint); diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index 971f142..91532c5 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -37,6 +37,10 @@ void LandState::handleEvent(ControlFSM& fsm, const EventData& event) { } } +bool LandState::stateIsReady(ControlFSM &fsm) { + return is_ready_; +} + void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { if(event.isValidCMD()) { cmd_ = event; @@ -139,6 +143,7 @@ void LandState::stateInit(ControlFSM& fsm) { }; fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); + is_ready_ = true; control::handleInfoMsg("Land init completed!"); } From 4d22640c602759eafb452b727016e764e44b571c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 22 Mar 2018 11:21:54 +0100 Subject: [PATCH 162/200] Allow empty data fields if no obstacle detected --- src/tools/obstacle_avoidance.cpp | 4 +++- src/tools/obstacle_state_handler.cpp | 5 +---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 321ac7d..d808026 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -71,8 +71,10 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const float drone_speed_ratio = std::min((float)std::sqrt(drone_speed/3.f), 1.f); // 3.f m/s is assumed drone max speed ascend_msgs::PolygonArray zone_msg; + + bool obstacles_valid = control::ObstacleStateHandler::isInstanceReady(); - for (int i = 0; i < obstacles.count; i++){ + for (int i = 0; i < obstacles.count && obstacles_valid; i++){ const auto obstacle_position = obstacles.global_robot_position[i]; const float obstacle_direction = obstacles.direction[i]; const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_position); diff --git a/src/tools/obstacle_state_handler.cpp b/src/tools/obstacle_state_handler.cpp index d11170d..b6109ca 100644 --- a/src/tools/obstacle_state_handler.cpp +++ b/src/tools/obstacle_state_handler.cpp @@ -41,10 +41,7 @@ bool ObstacleStateHandler::isInstanceReady() { } bool ObstacleStateHandler::isReady() const { - if(last_msg_p_->count == 0) { - control::handleWarnMsg("Obstacle handler: No data available"); - return false; - } else if(control::message::hasTimedOut(*last_msg_p_)) { + if(control::message::hasTimedOut(*last_msg_p_)) { control::handleWarnMsg("Obstacle handler: Using old data"); return false; } From 7ee9796b15f0d249d893fa9d315abf0f6587e5af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 5 Apr 2018 18:40:12 +0200 Subject: [PATCH 163/200] Fix merge issues --- include/control/fsm/go_to_state.hpp | 1 - include/control/fsm/land_state.hpp | 1 - include/control/fsm/position_hold_state.hpp | 1 - src/fsm/land_state.cpp | 4 ---- src/fsm/position_hold_state.cpp | 6 +----- src/tools/obstacle_avoidance.cpp | 3 ++- 6 files changed, 3 insertions(+), 13 deletions(-) diff --git a/include/control/fsm/go_to_state.hpp b/include/control/fsm/go_to_state.hpp index e9480b5..cb08d14 100644 --- a/include/control/fsm/go_to_state.hpp +++ b/include/control/fsm/go_to_state.hpp @@ -35,7 +35,6 @@ class GoToState : public StateInterface { void stateBegin(ControlFSM& fsm, const EventData& event) override; // Poll drone position and set setpoint void loopState(ControlFSM& fsm) override; - void stateEnd(ControlFSM& fsm, const EventData& event) override; std::string getStateName() const override { return "GoTo";} ascend_msgs::ControlFSMState getStateMsg() const override; const mavros_msgs::PositionTarget* getSetpointPtr() override; diff --git a/include/control/fsm/land_state.hpp b/include/control/fsm/land_state.hpp index 9f641cc..215ae11 100644 --- a/include/control/fsm/land_state.hpp +++ b/include/control/fsm/land_state.hpp @@ -13,7 +13,6 @@ class LandState : public StateInterface { void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; void stateInit(ControlFSM& fsm) override; - bool stateIsReady(ControlFSM &fsm) override; void loopState(ControlFSM& fsm) override; std::string getStateName() const override { return "Land"; } ascend_msgs::ControlFSMState getStateMsg() const override; diff --git a/include/control/fsm/position_hold_state.hpp b/include/control/fsm/position_hold_state.hpp index 30ab001..289b924 100644 --- a/include/control/fsm/position_hold_state.hpp +++ b/include/control/fsm/position_hold_state.hpp @@ -19,7 +19,6 @@ class PositionHoldState : public StateInterface { ascend_msgs::ControlFSMState getStateMsg() const override; const mavros_msgs::PositionTarget* getSetpointPtr() override; void handleManual(ControlFSM &fsm) override; - bool stateIsReady(ControlFSM &fsm) override; }; #endif diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index 6bd9dd5..2164ca7 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -37,10 +37,6 @@ void LandState::handleEvent(ControlFSM& fsm, const EventData& event) { } } -bool LandState::stateIsReady(ControlFSM &fsm) { - return is_ready_; -} - void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { if(event.isValidCMD()) { cmd_ = event; diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index e6c4b2b..977f8b4 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -129,7 +129,7 @@ void PositionHoldState::loopState(ControlFSM& fsm){ //TODO: logic to avoid being pushed around // keep new setpoint after obstacle avoidance - auto pose_stamped = control::DroneHandler::getCurrentPose(); + auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; //Set setpoint to current position setpoint_.position.x = position.x; @@ -158,7 +158,3 @@ ascend_msgs::ControlFSMState PositionHoldState::getStateMsg() const { msg.state_id = ControlFSMState::POS_HOLD_STATE; return msg; } - -bool PositionHoldState::stateIsReady(ControlFSM& fsm){ - return true; -} diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 24a74f8..71f2e3a 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -64,7 +64,8 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget bool setpoint_modified{false}; const auto& obstacles = control::ObstacleStateHandler::getCurrentObstacles(); - const auto& drone_pose = control::DroneHandler::getCurrentPose(); + //TODO Verify obstacles is local! + const auto& drone_pose = control::DroneHandler::getCurrentLocalPose(); const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); From b0d549842d26dae362444263d32eb94886a00b9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 5 Apr 2018 18:40:58 +0200 Subject: [PATCH 164/200] Update position_hold_state.cpp --- src/fsm/position_hold_state.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index 977f8b4..556c60c 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -129,7 +129,7 @@ void PositionHoldState::loopState(ControlFSM& fsm){ //TODO: logic to avoid being pushed around // keep new setpoint after obstacle avoidance - auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); + auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; //Set setpoint to current position setpoint_.position.x = position.x; From 1f1d4f5ed3e2765ede0ea8d602b30d405a4e4de7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 5 Apr 2018 18:43:36 +0200 Subject: [PATCH 165/200] Update land_state.cpp --- src/fsm/land_state.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index 2164ca7..9411012 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -47,8 +47,8 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { throw control::PoseNotValidException(); } - //reset obstacle avoidance flag - obstacle_avoidance_kicked_in_ = false; + //reset obstacle avoidance flag + obstacle_avoidance_kicked_in_ = false; auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; From ba7961288055df73c317590c87341d6c040caece Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 5 Apr 2018 18:48:45 +0200 Subject: [PATCH 166/200] Fix merge issues --- src/fsm/land_state.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index 2164ca7..b171220 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -47,8 +47,8 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { throw control::PoseNotValidException(); } - //reset obstacle avoidance flag - obstacle_avoidance_kicked_in_ = false; + //reset obstacle avoidance flag + obstacle_avoidance_kicked_in_ = false; auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; @@ -106,10 +106,10 @@ void LandState::loopState(ControlFSM& fsm) { } - if (obstacle_avoidance_kicked_in_){ + if (obstacle_avoidance_kicked_in_){ RequestEvent abort_event(RequestType::ABORT); fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); - } + } //Check landing if(LandDetector::isOnGround()) { @@ -139,7 +139,7 @@ void LandState::stateInit(ControlFSM& fsm) { }; fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); - is_ready_ = true; + setStateIsReady(); control::handleInfoMsg("Land init completed!"); } From fdaa3605aa36adc56fba633dabb77af06e538691 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 5 Apr 2018 18:54:12 +0200 Subject: [PATCH 167/200] Fix alignment issues --- src/fsm/position_hold_state.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index 977f8b4..8070a4e 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -48,9 +48,9 @@ void PositionHoldState::handleEvent(ControlFSM& fsm, const EventData& event) { void PositionHoldState::stateInit(ControlFSM& fsm) { std::function obstacleAvoidanceCB = [this]()->void { //TODO Why not set position setpoint to current position here?? - //NOTE: advantage: collect logic in one place (assuming it can be completely removed from loopState) - //NOTE: disadvantage: most callbacks in total don't do anything (are applied to inactive states) so might be better keep them minimal, but this is insignificant in this case, more control over timing if it placed in loopState (less dependent on OA implementation) - + // NOTE: advantage: collect logic in one place (assuming it can be completely removed from loopState) + // NOTE: disadvantage: most callbacks in total don't do anything (are applied to inactive states) so might be better keep them minimal, but this is insignificant in this case, more control over timing if it placed in loopState (less dependent on OA implementation) + this->obstacle_avoidance_kicked_in_ = true; }; @@ -126,10 +126,10 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { void PositionHoldState::loopState(ControlFSM& fsm){ if (obstacle_avoidance_kicked_in_){ - //TODO: logic to avoid being pushed around + //TODO: logic to avoid being pushed around // keep new setpoint after obstacle avoidance - auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); + auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; //Set setpoint to current position setpoint_.position.x = position.x; From bb20dff6bfa85f1bf22ce009357fcc959ac50992 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?H=C3=A5vard=20Sk=C3=A5ra=20Mellbye?= Date: Thu, 5 Apr 2018 18:58:16 +0200 Subject: [PATCH 168/200] Fix config file merge issue --- control_fsm_default_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index 27b9627..800a00c 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -71,7 +71,7 @@ obstacle_clearance_back_max: 2.0, obstacle_clearance_back_min: 1.0, #Obstacle avoidance checkradius - obstacle_clearance_checkradius: 100.0 + obstacle_clearance_checkradius: 100.0, #Frame ID for global frame global_frame_id: map, #Frame ID for local frame From 7460b6da74a556842079d99d85569ffba8f89b91 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 5 Apr 2018 19:54:10 +0200 Subject: [PATCH 169/200] Fix state interface --- include/control/fsm/state_interface.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/control/fsm/state_interface.hpp b/include/control/fsm/state_interface.hpp index a2fdc95..227b4c3 100644 --- a/include/control/fsm/state_interface.hpp +++ b/include/control/fsm/state_interface.hpp @@ -45,6 +45,8 @@ undefined behaviour. All state methods should gurantee nothrow! class StateInterface; class StateInterface { private: + ///Flag used to check if state is ready - should be set by state init + bool is_ready_ = false; /** * get vector of all instianciated states @@ -56,10 +58,9 @@ class StateInterface { ///Assigmnet operator should be removed StateInterface& operator=(const StateInterface&) = delete; protected: - ///Flag used to check if state is ready - should be set by state init - bool is_ready_ = false; ///Mark state as ready void setStateIsReady(){ is_ready_ = true; } + ///All states needs to return a valid setpoint mavros_msgs::PositionTarget setpoint_; public: From fd79aabc1338854ba5f215b177df94fdd1e29174 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 5 Apr 2018 20:13:42 +0200 Subject: [PATCH 170/200] Fix position hold --- include/control/fsm/position_hold_state.hpp | 1 - src/fsm/position_hold_state.cpp | 34 +++++---------------- 2 files changed, 8 insertions(+), 27 deletions(-) diff --git a/include/control/fsm/position_hold_state.hpp b/include/control/fsm/position_hold_state.hpp index 289b924..0b4c14e 100644 --- a/include/control/fsm/position_hold_state.hpp +++ b/include/control/fsm/position_hold_state.hpp @@ -8,7 +8,6 @@ ///Holds the current position class PositionHoldState : public StateInterface { - bool obstacle_avoidance_kicked_in_ = false; public: PositionHoldState(); void handleEvent(ControlFSM& fsm, const EventData& event) override; diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index 8070a4e..694772a 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -47,18 +47,17 @@ void PositionHoldState::handleEvent(ControlFSM& fsm, const EventData& event) { void PositionHoldState::stateInit(ControlFSM& fsm) { std::function obstacleAvoidanceCB = [this]()->void { - //TODO Why not set position setpoint to current position here?? - // NOTE: advantage: collect logic in one place (assuming it can be completely removed from loopState) - // NOTE: disadvantage: most callbacks in total don't do anything (are applied to inactive states) so might be better keep them minimal, but this is insignificant in this case, more control over timing if it placed in loopState (less dependent on OA implementation) + //TODO: logic to avoid being pushed around - this->obstacle_avoidance_kicked_in_ = true; + // keep new setpoint after obstacle avoidance + auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); + auto& position = pose_stamped.pose.position; + //Set setpoint to current position + setpoint_.position.x = position.x; + setpoint_.position.y = position.y; }; fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); - - // Set flag for obstacle avoidance - obstacle_avoidance_kicked_in_ = false; - } void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { @@ -74,9 +73,6 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { throw control::PoseNotValidException(); } - // Set flag for obstacle avoidance - obstacle_avoidance_kicked_in_ = false; - //Get pose - and position auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; @@ -124,21 +120,6 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { } } -void PositionHoldState::loopState(ControlFSM& fsm){ - if (obstacle_avoidance_kicked_in_){ - //TODO: logic to avoid being pushed around - - // keep new setpoint after obstacle avoidance - auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); - auto& position = pose_stamped.pose.position; - //Set setpoint to current position - setpoint_.position.x = position.x; - setpoint_.position.y = position.y; - } - //Only react once - obstacle_avoidance_kicked_in_ = false; -} - //Returns setpoint const mavros_msgs::PositionTarget* PositionHoldState::getSetpointPtr() { setpoint_.header.stamp = ros::Time::now(); @@ -158,3 +139,4 @@ ascend_msgs::ControlFSMState PositionHoldState::getStateMsg() const { msg.state_id = ControlFSMState::POS_HOLD_STATE; return msg; } + From 1e66e2a8a3bc693e79bbc12a813b598ebc4433e4 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 5 Apr 2018 20:21:51 +0200 Subject: [PATCH 171/200] Fix position hold --- include/control/fsm/position_hold_state.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/include/control/fsm/position_hold_state.hpp b/include/control/fsm/position_hold_state.hpp index 0b4c14e..561932e 100644 --- a/include/control/fsm/position_hold_state.hpp +++ b/include/control/fsm/position_hold_state.hpp @@ -12,7 +12,6 @@ class PositionHoldState : public StateInterface { PositionHoldState(); void handleEvent(ControlFSM& fsm, const EventData& event) override; void stateBegin(ControlFSM& fsm, const EventData& event) override; - void loopState(ControlFSM& fsm) override; void stateInit(ControlFSM& fsm) override; std::string getStateName() const override { return "Position hold"; } ascend_msgs::ControlFSMState getStateMsg() const override; From a21bab9a84e3d105d25c52b0a7e745e26bcfdf88 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 5 Apr 2018 21:01:06 +0200 Subject: [PATCH 172/200] start OA interactGB integration --- src/fsm/go_to_state.cpp | 5 ++--- src/fsm/interact_gb_state.cpp | 9 ++++++++- src/fsm/land_state.cpp | 6 ++---- src/tools/obstacle_avoidance.cpp | 2 -- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index b833faa..384c147 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -16,7 +16,6 @@ GoToState::GoToState() : StateInterface::StateInterface() { setpoint_.type_mask = default_mask; } - /** * @brief Returns a yaw that is a multiple of 90 degrees * @details Drone should fly as straight forward as possible @@ -185,8 +184,8 @@ void GoToState::loopState(ControlFSM& fsm) { } if (obstacle_avoidance_kicked_in_){ - RequestEvent abort_event(RequestType::ABORT); - fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + //RequestEvent abort_event(RequestType::ABORT); + //fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); return; } diff --git a/src/fsm/interact_gb_state.cpp b/src/fsm/interact_gb_state.cpp index 7462d4a..b3e9799 100644 --- a/src/fsm/interact_gb_state.cpp +++ b/src/fsm/interact_gb_state.cpp @@ -1,4 +1,5 @@ #include "control/fsm/interact_gb_state.hpp" +#include "control/fsm/control_fsm.hpp" #include "control/tools/setpoint_msg_defines.h" #include @@ -11,7 +12,13 @@ void InteractGBState::handleEvent(ControlFSM& fsm, const EventData& event) { } void InteractGBState::stateBegin(ControlFSM& fsm, const EventData& event) { - //TODO Implement + if (true /*TODO:check with obstacle avoidance*/){ + fsm.obstacle_avoidance_.relaxResponsibility(); + } else{ + RequestEvent abort_event(RequestType::ABORT); + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + return; + } } void InteractGBState::loopState(ControlFSM& fsm) { diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index b171220..bb278da 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -46,9 +46,6 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { if(!control::DroneHandler::isLocalPoseValid()) { throw control::PoseNotValidException(); } - - //reset obstacle avoidance flag - obstacle_avoidance_kicked_in_ = false; auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; @@ -66,6 +63,8 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { } } + //reset obstacle avoidance flag + obstacle_avoidance_kicked_in_ = false; // TODO:Check with obstacle_avoidance if it is ok to land if (true){ @@ -105,7 +104,6 @@ void LandState::loopState(ControlFSM& fsm) { setpoint_.type_mask = default_mask | SETPOINT_TYPE_LAND | IGNORE_PX | IGNORE_PY; } - if (obstacle_avoidance_kicked_in_){ RequestEvent abort_event(RequestType::ABORT); fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 71f2e3a..d831534 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -15,8 +15,6 @@ #include #include -#include // not used? - #include #include #include From f19ec2fe96e3b5354f86f109fbcc56a92caee33a Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 5 Apr 2018 21:05:32 +0200 Subject: [PATCH 173/200] start OA interactGB integration --- src/fsm/interact_gb_state.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsm/interact_gb_state.cpp b/src/fsm/interact_gb_state.cpp index b3e9799..e863c7f 100644 --- a/src/fsm/interact_gb_state.cpp +++ b/src/fsm/interact_gb_state.cpp @@ -12,6 +12,7 @@ void InteractGBState::handleEvent(ControlFSM& fsm, const EventData& event) { } void InteractGBState::stateBegin(ControlFSM& fsm, const EventData& event) { + if (true /*TODO:check with obstacle avoidance*/){ fsm.obstacle_avoidance_.relaxResponsibility(); } else{ From 18e20626cda3ed2a4e30f3a9d6fc80b43450cde3 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 5 Apr 2018 21:45:45 +0200 Subject: [PATCH 174/200] manflight, blindhover integration --- include/control/fsm/manual_flight_state.hpp | 1 + src/fsm/blind_hover_state.cpp | 4 ++++ src/fsm/manual_flight_state.cpp | 3 +++ 3 files changed, 8 insertions(+) diff --git a/include/control/fsm/manual_flight_state.hpp b/include/control/fsm/manual_flight_state.hpp index 541cf21..790530a 100644 --- a/include/control/fsm/manual_flight_state.hpp +++ b/include/control/fsm/manual_flight_state.hpp @@ -10,6 +10,7 @@ class ManualFlightState : public StateInterface { void handleEvent(ControlFSM& fsm, const EventData& event) override; std::string getStateName() const override { return "ManualFlight"; } void loopState(ControlFSM& fsm) override; + void stateBegin(ControlFSM& fsm, const EventData& event) override; ascend_msgs::ControlFSMState getStateMsg() const override; //Returns setpoint const mavros_msgs::PositionTarget* getSetpointPtr() override; diff --git a/src/fsm/blind_hover_state.cpp b/src/fsm/blind_hover_state.cpp index 3e4a18e..d7b8a93 100644 --- a/src/fsm/blind_hover_state.cpp +++ b/src/fsm/blind_hover_state.cpp @@ -74,6 +74,10 @@ void BlindHoverState::stateBegin(ControlFSM& fsm, const EventData& event ) { if(event.isValidCMD()) { cmd_ = event; } + + // Obstacle avoidance has no data to go by, so disable it + fsm.obstacle_avoidance_.relaxResponsibility(); + control::handleWarnMsg("No valid pose available, blindhovering!"); } diff --git a/src/fsm/manual_flight_state.cpp b/src/fsm/manual_flight_state.cpp index 00a3c08..f91d71c 100644 --- a/src/fsm/manual_flight_state.cpp +++ b/src/fsm/manual_flight_state.cpp @@ -62,6 +62,9 @@ void ManualFlightState::handleManual(ControlFSM &fsm) { //Already in manual, nothing to do } +void ManualFlightState::stateBegin(ControlFSM& fsm, const EventData& event){ + fsm.obstacle_avoidance_.relaxResponsibility(); +} ascend_msgs::ControlFSMState ManualFlightState::getStateMsg() const { From 2b5561d2c4762047c000600d667b4e3a9cece805 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 5 Apr 2018 22:06:44 +0200 Subject: [PATCH 175/200] Fix bug in position hold init --- src/fsm/position_hold_state.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index 694772a..ffedc4d 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -58,6 +58,9 @@ void PositionHoldState::stateInit(ControlFSM& fsm) { }; fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); + + setStateIsReady(); + control::handleInfoMsg("PositionHold init completed!"); } void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { From 7cb7137688d2d45c0e49ed19f4a57276c5bf36ee Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 6 Apr 2018 19:12:42 +0200 Subject: [PATCH 176/200] Circular zones, local position maybe? --- control_fsm_default_config.yaml | 14 ++-- include/control/tools/config.hpp | 12 +--- include/control/tools/obstacle_math.hpp | 8 +-- src/tools/config.cpp | 16 ++--- src/tools/obstacle_avoidance.cpp | 91 +++++++++---------------- 5 files changed, 47 insertions(+), 94 deletions(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index 1f86465..a8f4ad4 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -61,17 +61,11 @@ obstacle_state_topic: perception/obstacles/lidar, #Margin to determine if velocity is reached velocity_reached_margin: 0.1, - #Obstacle avoidance clearance on sides - obstacle_clearance_side_max: 1.8, - obstacle_clearance_side_min: 1.0, - #Obstacle avoidance clearance in front - obstacle_clearance_front_max: 2.5, - obstacle_clearance_front_min: 1.5, - #Obstacle avoidance clearance in back - obstacle_clearance_back_max: 2.0, - obstacle_clearance_back_min: 1.0, + #Obstacle avoidance clearance + obstacle_clearance_max: 1.8, + obstacle_clearance_min: 1.5, #Obstacle avoidance checkradius - obstacle_clearance_checkradius: 100.0, + obstacle_clearance_checkradius: 40.0, #Frame ID for global frame global_frame_id: map, #Frame ID for local frame diff --git a/include/control/tools/config.hpp b/include/control/tools/config.hpp index fc06cab..3b01614 100644 --- a/include/control/tools/config.hpp +++ b/include/control/tools/config.hpp @@ -81,15 +81,9 @@ class Config { static double safe_hover_altitude; /// \fsmparam Drone safezone static double obstacle_too_close_dist; - /// \fsmparam Clearance osbtacle avoidance to right and left of obstacle - static double obstacle_clearance_side_max; - static double obstacle_clearance_side_min; - /// \fsmparam Clearance osbtacle avoidance in front of obstacle - static double obstacle_clearance_front_max; - static double obstacle_clearance_front_min; - /// \fsmparam Clearance osbtacle avoidance behind obstacle - static double obstacle_clearance_back_max; - static double obstacle_clearance_back_min; + /// \fsmparam Clearance osbtacle avoidance + static double obstacle_clearance_max; + static double obstacle_clearance_min; /// \fsmparam Radius in which obstacle collisions will be checked for static double obstacle_clearance_checkradius; /// \fsmparam Topic to listen for info about obstacles diff --git a/include/control/tools/obstacle_math.hpp b/include/control/tools/obstacle_math.hpp index d561f2b..b69cc9d 100644 --- a/include/control/tools/obstacle_math.hpp +++ b/include/control/tools/obstacle_math.hpp @@ -44,15 +44,15 @@ inline double calcDistanceToObstacle(const T& point, const K& obstacle_position) } -template -inline N calcAngleToObstacle(const T& point, const K& obstacle_position, const N obstacle_direction){ +template +inline float calcAngleToObstacle(const T& point, const K& obstacle_position){ T delta_drone_obstacle; delta_drone_obstacle.x = point.x - obstacle_position.x; delta_drone_obstacle.y = point.y - obstacle_position.y; - const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); + //const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); - return angle_to_obstacle; + return angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x)); } // Apply 2d transformation matrix diff --git a/src/tools/config.cpp b/src/tools/config.cpp index 3847c4f..9395268 100644 --- a/src/tools/config.cpp +++ b/src/tools/config.cpp @@ -42,12 +42,8 @@ double Config::obstacle_too_close_dist = 2.0; std::string Config::lidar_topic = "perception/obstacles/lidar"; bool Config::require_all_data_streams = true; bool Config::require_obstacle_detection = true; -double Config::obstacle_clearance_side_max = 2.0f; -double Config::obstacle_clearance_side_min = 1.0f; -double Config::obstacle_clearance_front_max = 2.0f; -double Config::obstacle_clearance_front_min = 1.4f; -double Config::obstacle_clearance_back_max = 2.0f; -double Config::obstacle_clearance_back_min = 1.0f; +double Config::obstacle_clearance_max = 2.0f; +double Config::obstacle_clearance_min = 1.5f; double Config::obstacle_clearance_checkradius = 2.0f; std::string Config::global_frame_id = "map"; std::string Config::local_frame_id = "odom"; @@ -159,12 +155,8 @@ void Config::loadParams() { //Obstacles getStringParam("obstacle_state_topic", obstacle_state_topic); //Obstacle avoidance - getDoubleParam("obstacle_clearance_side_max", obstacle_clearance_side_max, 0, 100); - getDoubleParam("obstacle_clearance_side_min", obstacle_clearance_side_min, 0, 100); - getDoubleParam("obstacle_clearance_front_max", obstacle_clearance_front_max, 0, 100); - getDoubleParam("obstacle_clearance_front_min", obstacle_clearance_front_min, 0, 100); - getDoubleParam("obstacle_clearance_back_max", obstacle_clearance_back_max, 0, 100); - getDoubleParam("obstacle_clearance_back_min", obstacle_clearance_back_min, 0, 100); + getDoubleParam("obstacle_clearance_max", obstacle_clearance_max, 0, 100); + getDoubleParam("obstacle_clearance_min", obstacle_clearance_min, 0, 100); getDoubleParam("obstacle_clearance_checkradius", obstacle_clearance_checkradius, 0, 100); //Action server getStringParam("action_server_topic", action_server_topic); diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index d831534..91109e6 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -31,28 +31,13 @@ using obstacle_math::calcDistanceToObstacle; /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle /// which is allowed, referenced from the obstacles coordinate system. x-axis is positive /// direction of motion. Obstacle driving in with obstacle_direction=0 would be driving in x-direction -inline geometry_msgs::Vector3 avoidZone(const float obstacle_direction, - const float front_clearance, const float back_clearance, const float side_clearance){ - - const auto angle_positive = angleWrapper(obstacle_direction); - const bool drone_in_front_of_obstacle = angle_positive <= PI/2 || angle_positive >= 3*PI/2; +inline geometry_msgs::Vector3 avoidZone(const float drone_angle_to_obstacle, const float clearance){ geometry_msgs::Vector3 minimum_vector; + minimum_vector.x = clearance * std::cos(drone_angle_to_obstacle); + minimum_vector.y = clearance * std::sin(drone_angle_to_obstacle); minimum_vector.z = 0.0f; - minimum_vector.y = side_clearance * std::sin(angle_positive); - if (drone_in_front_of_obstacle){ - // Triangle shape - //minimum_vector.x = (front_clearance/side_clearance)*(side_clearance - std::fabs(minimum_vector.y)); - - // Ellipse - minimum_vector.x = front_clearance * std::cos(angle_positive); - } - else { - // Ellipse - minimum_vector.x = back_clearance * std::cos(angle_positive); - } - return minimum_vector; } @@ -61,9 +46,11 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget bool setpoint_modified{false}; - const auto& obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + const auto drone_pose = control::DroneHandler::getCurrentLocalPose(); + //TODO Verify obstacles is local! - const auto& drone_pose = control::DroneHandler::getCurrentLocalPose(); + ROS_INFO_THROTTLE(1,"drone_pos (should be local):\t%.3f\t%.3f", drone_pose.pose.position.x, drone_pose.pose.position.y); const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); @@ -75,62 +62,48 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget for (int i = 0; i < obstacles.count && obstacles_valid; i++){ const auto obstacle_position = obstacles.global_robot_position[i]; - const float obstacle_direction = obstacles.direction[i]; + ROS_INFO_THROTTLE(1,"obstacle_pos (should be local):\t%.3f\t%.3f", obstacle_position.x, obstacle_position.y); + //const float obstacle_direction = obstacles.direction[i]; const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_position); const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance - const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_position, obstacle_direction); - const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_position, obstacle_direction); + const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_position); + const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_position); // True if setpoint is within a 120 deg cone away from the obstacle // TODO: turn this angle into a param once complete const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; - //ROS_INFO_THROTTLE(2, "d_angle - s_angle: %.3f", fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI))); - //(drone_angle_to_obstacle - 3*PI/8) < setpoint_angle_to_obstacle && - //setpoint_angle_to_obstacle < (drone_angle_to_obstacle + 3*PI/8); - - // TODO: this logic in the avoidZone function - const auto clearance_front = std::max(Config::obstacle_clearance_front_max*drone_speed_ratio, - Config::obstacle_clearance_front_min); - const auto clearance_back = std::max(Config::obstacle_clearance_back_max*drone_speed_ratio, - Config::obstacle_clearance_back_min); - const auto clearance_side = std::max(Config::obstacle_clearance_side_max*drone_speed_ratio, - Config::obstacle_clearance_side_min); - geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, - clearance_front, clearance_back, clearance_side); + const auto clearance = std::max(Config::obstacle_clearance_max*drone_speed_ratio, Config::obstacle_clearance_min); + geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, clearance); // Rotate to global coordinate system const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); - //ROS_INFO("Local min vec: %.3f\t%.3f", minimum_vector.x, minimum_vector.y); - minimum_vector = rotateXY(minimum_vector, obstacle_direction); + ROS_INFO("Local min vec: %.3f\t%.3f", minimum_vector.x, minimum_vector.y); + //minimum_vector = rotateXY(minimum_vector, obstacle_direction); //ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_position.x + minimum_vector.x, obstacle_position.y + minimum_vector.y); - + // generate points for zone_pub_ - geometry_msgs::Point32 point_front; point_front.x = clearance_front; - geometry_msgs::Point32 point_back; point_back.x = -clearance_back; - geometry_msgs::Point32 point_right; point_right.y = -clearance_side; - geometry_msgs::Point32 point_left; point_left.y = clearance_side; - point_front = rotateXY(point_front, obstacle_direction); - point_back = rotateXY(point_back, obstacle_direction); - point_left = rotateXY(point_left, obstacle_direction); - point_right = rotateXY(point_right, obstacle_direction); - point_front = vectorSum(point_front, obstacle_position); - point_back = vectorSum(point_back, obstacle_position); - point_left = vectorSum(point_left, obstacle_position); - point_right = vectorSum(point_right, obstacle_position); - - geometry_msgs::Polygon polygon; - polygon.points.push_back(point_front); - polygon.points.push_back(point_left); - polygon.points.push_back(point_back); - polygon.points.push_back(point_right); - - zone_msg.polygons.push_back(polygon); + constexpr int N_points{4}; + geometry_msgs::Polygon polygon; + for (int i = 0; i < N_points; i++){ + const float angle = static_cast(i)*2.f*PI/N_points; + auto local_pos = avoidZone(angle, clearance); + local_pos = vectorSum(local_pos, obstacle_position); + + geometry_msgs::Point32 global_pos; + global_pos.x = obstacle_position.x + local_pos.x; + global_pos.y = obstacle_position.y + local_pos.y; + global_pos.z = obstacle_position.z + local_pos.z; + + polygon.points.push_back(global_pos); + } + + zone_msg.polygons.push_back(polygon); if (drone_distance_to_obstacle < minimum_distance){ if (setpoint_reachable From 1b7b3c576a2d33754d5d77ef82b1133a6d4ffe93 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 6 Apr 2018 19:36:40 +0200 Subject: [PATCH 177/200] Fix bugs and clarify --- src/tools/obstacle_avoidance.cpp | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 91109e6..6b7aeb5 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -50,27 +50,24 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto drone_pose = control::DroneHandler::getCurrentLocalPose(); //TODO Verify obstacles is local! - ROS_INFO_THROTTLE(1,"drone_pos (should be local):\t%.3f\t%.3f", drone_pose.pose.position.x, drone_pose.pose.position.y); const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed ascend_msgs::PolygonArray zone_msg; - + bool obstacles_valid = control::ObstacleStateHandler::isInstanceReady(); for (int i = 0; i < obstacles.count && obstacles_valid; i++){ - const auto obstacle_position = obstacles.global_robot_position[i]; - ROS_INFO_THROTTLE(1,"obstacle_pos (should be local):\t%.3f\t%.3f", obstacle_position.x, obstacle_position.y); - //const float obstacle_direction = obstacles.direction[i]; - const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_position); - const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); + const auto& obstacle_global_position = obstacles.global_robot_position[i]; + const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_global_position); + const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ // perform obstacle avoidance - const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_position); - const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_position); + const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_global_position); + const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_global_position); // True if setpoint is within a 120 deg cone away from the obstacle // TODO: turn this angle into a param once complete @@ -82,10 +79,9 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget // Rotate to global coordinate system const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); - ROS_INFO("Local min vec: %.3f\t%.3f", minimum_vector.x, minimum_vector.y); //minimum_vector = rotateXY(minimum_vector, obstacle_direction); - //ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_position.x + minimum_vector.x, obstacle_position.y + minimum_vector.y); + //ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_global_position.x + minimum_vector.x, obstacle_global_position.y + minimum_vector.y); // generate points for zone_pub_ constexpr int N_points{4}; @@ -93,12 +89,12 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget for (int i = 0; i < N_points; i++){ const float angle = static_cast(i)*2.f*PI/N_points; auto local_pos = avoidZone(angle, clearance); - local_pos = vectorSum(local_pos, obstacle_position); + local_pos = vectorSum(local_pos, obstacle_global_position); geometry_msgs::Point32 global_pos; - global_pos.x = obstacle_position.x + local_pos.x; - global_pos.y = obstacle_position.y + local_pos.y; - global_pos.z = obstacle_position.z + local_pos.z; + global_pos.x = obstacle_global_position.x + local_pos.x; + global_pos.y = obstacle_global_position.y + local_pos.y; + global_pos.z = obstacle_global_position.z + local_pos.z; polygon.points.push_back(global_pos); } @@ -120,8 +116,8 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } // need to avoid obstacle setpoint_modified = true; - setpoint->position.x = obstacle_position.x + minimum_vector.x; - setpoint->position.y = obstacle_position.y + minimum_vector.y; + setpoint->position.x = obstacle_global_position.x + minimum_vector.x; + setpoint->position.y = obstacle_global_position.y + minimum_vector.y; if (setpoint->position.z < Config::min_in_air_alt){ setpoint->position.z = Config::min_in_air_alt; } @@ -130,7 +126,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } if (setpoint_modified){ - //const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_position); + //const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); //ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); } } From e2e010c10b7a343779f465c576a6eb608ee305cd Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 6 Apr 2018 19:39:32 +0200 Subject: [PATCH 178/200] Fix bugs and clarify again --- src/tools/obstacle_avoidance.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 6b7aeb5..511f9f3 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -74,15 +74,11 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; const auto clearance = std::max(Config::obstacle_clearance_max*drone_speed_ratio, Config::obstacle_clearance_min); - geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, clearance); + const geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, clearance); // Rotate to global coordinate system const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); - //minimum_vector = rotateXY(minimum_vector, obstacle_direction); - - //ROS_INFO("Minimum pos: %.3f\t%.3f", obstacle_global_position.x + minimum_vector.x, obstacle_global_position.y + minimum_vector.y); - // generate points for zone_pub_ constexpr int N_points{4}; geometry_msgs::Polygon polygon; @@ -116,8 +112,8 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } // need to avoid obstacle setpoint_modified = true; - setpoint->position.x = obstacle_global_position.x + minimum_vector.x; - setpoint->position.y = obstacle_global_position.y + minimum_vector.y; + setpoint->position.x = minimum_vector.x; + setpoint->position.y = minimum_vector.y; if (setpoint->position.z < Config::min_in_air_alt){ setpoint->position.z = Config::min_in_air_alt; } From 5bc165c961230eb8892d8dc85e489d2ddcb1ae76 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 6 Apr 2018 19:45:58 +0200 Subject: [PATCH 179/200] Fix local/global position bug --- src/tools/obstacle_avoidance.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 511f9f3..d7b0b59 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -112,8 +112,8 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } // need to avoid obstacle setpoint_modified = true; - setpoint->position.x = minimum_vector.x; - setpoint->position.y = minimum_vector.y; + setpoint->position.x = obstacle_global_position.x + minimum_vector.x; + setpoint->position.y = obstacle_global_position.y + minimum_vector.y; if (setpoint->position.z < Config::min_in_air_alt){ setpoint->position.z = Config::min_in_air_alt; } From b6c681993d0782e4247a1cfac8f1d22b91a4e430 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 6 Apr 2018 20:23:25 +0200 Subject: [PATCH 180/200] Fix polygon publishing --- control_fsm_default_config.yaml | 2 +- src/tools/obstacle_avoidance.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index a8f4ad4..f98cb26 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -62,7 +62,7 @@ #Margin to determine if velocity is reached velocity_reached_margin: 0.1, #Obstacle avoidance clearance - obstacle_clearance_max: 1.8, + obstacle_clearance_max: 2.8, obstacle_clearance_min: 1.5, #Obstacle avoidance checkradius obstacle_clearance_checkradius: 40.0, diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index d7b0b59..65aff95 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -54,6 +54,8 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed + + ROS_INFO_THROTTLE(1, "drone_speed: %.3f", drone_speed); ascend_msgs::PolygonArray zone_msg; @@ -80,12 +82,11 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); // generate points for zone_pub_ - constexpr int N_points{4}; + constexpr int N_points{6}; geometry_msgs::Polygon polygon; for (int i = 0; i < N_points; i++){ const float angle = static_cast(i)*2.f*PI/N_points; auto local_pos = avoidZone(angle, clearance); - local_pos = vectorSum(local_pos, obstacle_global_position); geometry_msgs::Point32 global_pos; global_pos.x = obstacle_global_position.x + local_pos.x; From eee1aa15348687e4a6950f0b78abc67119af1410 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Tue, 10 Apr 2018 12:49:02 +0200 Subject: [PATCH 181/200] Optimize with drone_direction, 1st try --- include/control/tools/obstacle_math.hpp | 3 +-- src/tools/obstacle_avoidance.cpp | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/include/control/tools/obstacle_math.hpp b/include/control/tools/obstacle_math.hpp index b69cc9d..a549af6 100644 --- a/include/control/tools/obstacle_math.hpp +++ b/include/control/tools/obstacle_math.hpp @@ -8,8 +8,7 @@ namespace obstacle_math { constexpr double PI{3.14159265358979323846264338327950288419}; // wrap any angle to range [0, 2pi) -template::value, T>::type> -T angleWrapper(const T angle){ +float angleWrapper(const float angle){ return angle - 2*PI*floor(angle/(2*PI)); } diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 65aff95..7302538 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -31,10 +31,17 @@ using obstacle_math::calcDistanceToObstacle; /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle /// which is allowed, referenced from the obstacles coordinate system. x-axis is positive /// direction of motion. Obstacle driving in with obstacle_direction=0 would be driving in x-direction -inline geometry_msgs::Vector3 avoidZone(const float drone_angle_to_obstacle, const float clearance){ +inline geometry_msgs::Vector3 calcMinimumLocalVector(const float drone_angle_to_obstacle, const float drone_direction, + const float clearance){ + + const auto angle = angleWrapper(drone_direction - drone_angle_to_obstacle); geometry_msgs::Vector3 minimum_vector; - minimum_vector.x = clearance * std::cos(drone_angle_to_obstacle); + if (angle > PI){ // drone is driving towards obstacle + minimum_vector.x = 2.0*clearance * std::cos(drone_angle_to_obstacle); + } else { + minimum_vector.x = clearance * std::cos(drone_angle_to_obstacle); + } minimum_vector.y = clearance * std::sin(drone_angle_to_obstacle); minimum_vector.z = 0.0f; @@ -52,6 +59,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget //TODO Verify obstacles is local! const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; + const auto drone_direction = std::atan2(drone_vel.y, drone_vel.x); const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed @@ -76,7 +84,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; const auto clearance = std::max(Config::obstacle_clearance_max*drone_speed_ratio, Config::obstacle_clearance_min); - const geometry_msgs::Vector3 minimum_vector = avoidZone(drone_angle_to_obstacle, clearance); + const auto minimum_vector = calcMinimumLocalVector(drone_angle_to_obstacle, drone_direction, clearance); // Rotate to global coordinate system const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); @@ -86,7 +94,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget geometry_msgs::Polygon polygon; for (int i = 0; i < N_points; i++){ const float angle = static_cast(i)*2.f*PI/N_points; - auto local_pos = avoidZone(angle, clearance); + auto local_pos = calcMinimumLocalVector(angle, drone_direction, clearance); geometry_msgs::Point32 global_pos; global_pos.x = obstacle_global_position.x + local_pos.x; From 00cd85f0025a81a400391ec957f3e114e2f99854 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 11 Apr 2018 16:26:13 +0200 Subject: [PATCH 182/200] Avoid obstacles using heuristic instead of 'removing' the nodes from the graph --- src/path_planner/path_planner.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 76c924b..52dd4a0 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -49,7 +49,6 @@ void PathPlanner::addObstacle(float center_x, float center_y) { // Do this to fill the circle for(float i = center_y-y; i<=center_y+y; i+= node_distance) { if (isValidCoordinate(x, i)) { - graph[coordToIndex(x)][coordToIndex(i)].closed = true; graph[coordToIndex(x)][coordToIndex(i)].obstacle = true; } } @@ -60,7 +59,6 @@ void PathPlanner::addObstacle(float center_x, float center_y) { float x = sqrt(obstacle_radius*obstacle_radius-(y-center_y)*(y-center_y)); for(float i = center_x-x; i<=center_x+x; i+= node_distance) { if (isValidCoordinate(i, y)) { - graph[coordToIndex(i)][coordToIndex(y)].closed = true; graph[coordToIndex(i)][coordToIndex(y)].obstacle = true; } } @@ -94,7 +92,6 @@ void PathPlanner::addUnsafeZone(float center_x, float center_y) { // Do this to fill the circle for(float i = center_y-y; i<=center_y+y; i+= node_distance) { if (isValidCoordinate(x, i)) { - graph[coordToIndex(x)][coordToIndex(i)].closed = true; graph[coordToIndex(x)][coordToIndex(i)].unsafe = true; } } @@ -105,7 +102,6 @@ void PathPlanner::addUnsafeZone(float center_x, float center_y) { float x = sqrt(unsafe_radius*unsafe_radius-(y-center_y)*(y-center_y)); for(float i = center_x-x; i<=center_x+x; i+= node_distance) { if (isValidCoordinate(i, y)) { - graph[coordToIndex(i)][coordToIndex(y)].closed = true; graph[coordToIndex(i)][coordToIndex(y)].unsafe = true; } } @@ -193,6 +189,13 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent float new_g = graph[coordToIndex(parent_x)][coordToIndex(parent_y)].getG() + distance_to_parent; // Update graph and open list if distance is less than before through the parent if(graph[x_index][y_index].getG() > new_g) { + // The heuristic values 100 and 50 are just found by testing, and can be further tuned + // These values are to make the cost of moving through the current node larger + if (graph[x_index][y_index].obstacle) { + new_g = new_g * 100; + } else if (graph[x_index][y_index].unsafe) { + new_g = new_g * 50; + } graph[x_index][y_index].updateF(new_g); // For visualization if(max_f < graph[x_index][y_index].getF()){ @@ -265,7 +268,7 @@ bool PathPlanner::canSimplifyLine(float x1, float y1, float x2, float y2) { y1 += delta_y/num_points; // checking if the line goes through an obstacle at this point if(isValidCoordinate(x1,y1)){ - if (graph[coordToIndex(x1)][coordToIndex(y1)].unsafe) { + if (graph[coordToIndex(x1)][coordToIndex(y1)].obstacle) { return false; } } From ed2bba89ac854d2251b1bb5f48aaf7ca0ab5d0c4 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 11 Apr 2018 16:26:52 +0200 Subject: [PATCH 183/200] Small fixes for nicer code --- src/path_planner/path_planner.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 52dd4a0..c38af67 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -168,7 +168,7 @@ void PathPlanner::relaxGraph(){ } void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent_y, float distance_to_parent) { - std::cout << "Successor = " << x << ", " << y << std::endl; + //std::cout << "Successor = " << x << ", " << y << std::endl; if(isValidCoordinate(x,y)){ int x_index = coordToIndex(x); int y_index = coordToIndex(y); @@ -210,7 +210,7 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent } void PathPlanner::handleAllSuccessors(float x, float y) { - std::cout << "q = " << x << ", " << y << std::endl; + //std::cout << "q = " << x << ", " << y << std::endl; handleSuccessor(x+node_distance,y, x, y, node_distance); if(destination_reached){return;} handleSuccessor(x+node_distance, y+node_distance, x, y, diagonal_node_distance); @@ -436,9 +436,7 @@ void PathPlanner::simplifyPlan() { std::list::iterator third = ++(simple_plan.begin()); third++; - if(first == simple_plan.end()){ - return; - } else if(second == simple_plan.end()){ + if(first == simple_plan.end() || second == simple_plan.end()){ return; } From c143ceab026d81da330d02f7239395f4e72d7f20 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 11 Apr 2018 17:23:15 +0200 Subject: [PATCH 184/200] Make plan even though end or start point is close to/on an obstacle --- src/path_planner/path_planner.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index c38af67..1fd9600 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -316,11 +316,6 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo refreshObstacles(); refreshUnsafeZones(); - if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { - std::cout << "\n GET OOOOOUT!!! \n\n"; - return; - } - /* if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { std::cout << "\n GET OOOOOUT!!! \n\n"; @@ -376,12 +371,6 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo int x_index = coordToIndex(x); int y_index = coordToIndex(y); - if(graph[x_index][y_index].unsafe){ - std::cout << "End point unsafe, invalid!" << std::endl; - return; - } - - // Calculate all f values and set the parents relaxGraph(); From 51fcd0c57b964aca5163028725594581b836a465 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 12 Apr 2018 12:58:05 +0200 Subject: [PATCH 185/200] Remove unused code --- src/path_planner/path_planner.cpp | 77 ------------------------------- 1 file changed, 77 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 1fd9600..eabe667 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -138,17 +138,6 @@ float PathPlanner::calculateEuclidianHeuristic(float x, float y) { return(sqrt((x-end_node.getX())*(x-end_node.getX())+(y-end_node.getY())*(y-end_node.getY()))); } -/*void PathPlanner::printGraph(){ - for(int i = 0; i < graph_size; i++){ - for(int j = 0; j < graph_size; j++){ - std::cout << std::fixed; - std::cout << std::setprecision(2); - std::cout << graph[i][j].getF() << " "; - } - std::cout << std::endl; - } -}*/ - void PathPlanner::relaxGraph(){ if (debug) {std::cout << "relaxGraph" << std::endl;} Node current_node; @@ -316,55 +305,6 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo refreshObstacles(); refreshUnsafeZones(); - /* - if(graph[coordToIndex(current_x)][coordToIndex(current_y)].unsafe) { - std::cout << "\n GET OOOOOUT!!! \n\n"; - escape_from_obstacle = true; - - // ---- make this a function ---- - - // Figure out which obstacle is blocking - Obstacle* blocking_obstacle = findBlockingObstacle(current_x, current_y); - if (!blocking_obstacle) { - std::cout << "Unable to solve problem!" << std::endl; - return; - } - - // Find the direction of the obstacle - float obstacle_dir = blocking_obstacle->dir; - - // Find desired direction of the drone - struct Point {float x,y;}; - Point current_point{current_x, current_y}; - float drone_dir = obstacle_math::calcAngleToObstacle(current_point, *blocking_obstacle, obstacle_dir); - - std::cout << "Drone direction: " << drone_dir << std::endl; - - // Find setpoint in right direction outside unsafe zone - // The safe zone is obstacle_radius*2, but to ensure that - // the new point is outside the safe zone, 2.5 is used here - float safe_radius = obstacle_radius*2.5; - float safe_x = safe_radius*cos(drone_dir)+blocking_obstacle->x; - float safe_y = safe_radius*sin(drone_dir)+blocking_obstacle->y; - - if(!isValidCoordinate(safe_x, safe_y)) { - std::cout << "Cannot find safe point!" << std::endl; - return; - } - std::cout << "Safe point: " << safe_x << ", " << safe_y << std::endl; - - - // Add starting point to plan and plan a new plan from the - // setpoint to target - resetParameters(); - start_node = Node(safe_x, safe_y, 0, 0); - start_node.setParentX(0); - start_node.setParentY(0); - initializeGraph(); - refreshObstacles(); - refreshUnsafeZones(); - }*/ - float x = end_node.getX(); float y = end_node.getY(); @@ -402,19 +342,8 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo plan.push_front(graph.at(x_index).at(y_index)); } - - - // Print the plan - can be removed - /*std::cout << std::endl << "Whole plan:" << std::endl; - for (std::list::iterator it = plan.begin(); it!= plan.end(); ++it){ - std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; - }*/ - // Delete unnecessary points simplifyPlan(); - if(escape_from_obstacle) { - simple_plan.push_front(graph[current_x][current_y]); - } } void PathPlanner::simplifyPlan() { @@ -453,12 +382,6 @@ void PathPlanner::simplifyPlan() { third++; } } - - // Print the remaining points - /*std::cout << "Simple plan: " << std::endl; - for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ - std::cout << "x: " << it->getX() << " y: " << it->getY() << std::endl; - }*/ } bool PathPlanner::isPlanSafe(float current_x, float current_y) { From 28b86b77c9add31cd9334a5ddc5b113c049c57d3 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 12 Apr 2018 15:07:11 +0200 Subject: [PATCH 186/200] Split up into functions --- control_fsm_default_config.yaml | 4 +- src/tools/obstacle_avoidance.cpp | 160 ++++++++++++++++++------------- 2 files changed, 94 insertions(+), 70 deletions(-) diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index e9cce4c..6478a01 100644 --- a/control_fsm_default_config.yaml +++ b/control_fsm_default_config.yaml @@ -64,8 +64,8 @@ #Margin to determine if velocity is reached velocity_reached_margin: 0.1, #Obstacle avoidance clearance - obstacle_clearance_max: 2.8, - obstacle_clearance_min: 1.5, + obstacle_clearance_max: 1.1, + obstacle_clearance_min: 1.0, #Obstacle avoidance checkradius obstacle_clearance_checkradius: 40.0, #Frame ID for global frame diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 7302538..1e8ce0d 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -31,105 +31,129 @@ using obstacle_math::calcDistanceToObstacle; /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle /// which is allowed, referenced from the obstacles coordinate system. x-axis is positive /// direction of motion. Obstacle driving in with obstacle_direction=0 would be driving in x-direction -inline geometry_msgs::Vector3 calcMinimumLocalVector(const float drone_angle_to_obstacle, const float drone_direction, - const float clearance){ +geometry_msgs::Vector3 calcMinimumLocalVector(const float drone_angle_to_obstacle, const float drone_direction, const float drone_speed_ratio, + const float clearance_min, const float clearance_max){ - const auto angle = angleWrapper(drone_direction - drone_angle_to_obstacle); + auto angle = angleWrapper(drone_angle_to_obstacle - drone_direction); - geometry_msgs::Vector3 minimum_vector; - if (angle > PI){ // drone is driving towards obstacle - minimum_vector.x = 2.0*clearance * std::cos(drone_angle_to_obstacle); - } else { - minimum_vector.x = clearance * std::cos(drone_angle_to_obstacle); + if (drone_speed_ratio < 0.1){ + // direction is proabably noise, ignore it + angle = 0.0; + ROS_INFO_THROTTLE(5,"drone_speed_ratio %f", drone_speed_ratio); + } + + auto front_clearance = clearance_min; + if (angle < PI){ // drone is driving towards obstacle + front_clearance = clearance_max; } - minimum_vector.y = clearance * std::sin(drone_angle_to_obstacle); + + geometry_msgs::Vector3 minimum_vector; + minimum_vector.x = front_clearance * std::cos(drone_angle_to_obstacle); + minimum_vector.y = clearance_min * std::sin(drone_angle_to_obstacle); minimum_vector.z = 0.0f; + //ROS_INFO_THROTTLE(1, "local(%.3f,%.3f,%.3f)", local_pos.x, local_pos.y, local_pos.z); + return minimum_vector; } -bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { +template // vector types with x,y,z +bool checkAndAvoidSingleObstacle(mavros_msgs::PositionTarget* setpoint, const V1& drone_position, const V2& drone_velocity, const V3& obstacle_global_position, ascend_msgs::PolygonArray& zone_msg){ using control::Config; + + const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_position, obstacle_global_position); + const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); + + const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); + const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); + const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed + + bool setpoint_modified = false; + if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ + // perform obstacle avoidance + const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_position, obstacle_global_position); + const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_global_position); + + // True if setpoint is within a 120 deg cone away from the obstacle + // TODO: turn this angle into a param once complete + const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; + + const auto clearance = std::max(Config::obstacle_clearance_max*drone_speed_ratio, Config::obstacle_clearance_min); + const auto minimum_vector = calcMinimumLocalVector(drone_angle_to_obstacle, drone_direction, drone_speed_ratio, clearance, 2.0*clearance); + + const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); + if (drone_distance_to_obstacle < minimum_distance){ + if (setpoint_reachable + && setpoint_distance_to_obstacle > drone_distance_to_obstacle + && setpoint_distance_to_obstacle > minimum_distance){ + // no action, maybe logging? + ROS_INFO_THROTTLE(1, "[obstacle avoidance]: Prefer setpoint over avoid algorithm"); + } + else { + if (setpoint_modified){ + // TODO: find out a better solution to this problem + ROS_WARN("[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); + setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) + } + // need to avoid obstacle + setpoint_modified = true; + + setpoint->position.x = obstacle_global_position.x + minimum_vector.x; + setpoint->position.y = obstacle_global_position.y + minimum_vector.y; + if (setpoint->position.z < Config::min_in_air_alt){ + setpoint->position.z = Config::min_in_air_alt; + } + } + } + } + + return setpoint_modified; +} + +bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { bool setpoint_modified{false}; const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); const auto drone_pose = control::DroneHandler::getCurrentLocalPose(); - - //TODO Verify obstacles is local! - - const auto drone_vel = control::DroneHandler::getCurrentTwist().twist.linear; - const auto drone_direction = std::atan2(drone_vel.y, drone_vel.x); - const auto drone_speed = std::sqrt(std::pow(drone_vel.x,2) + std::pow(drone_vel.y,2)); - const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed + const auto drone_velocity = control::DroneHandler::getCurrentTwist().twist.linear; - ROS_INFO_THROTTLE(1, "drone_speed: %.3f", drone_speed); - ascend_msgs::PolygonArray zone_msg; bool obstacles_valid = control::ObstacleStateHandler::isInstanceReady(); for (int i = 0; i < obstacles.count && obstacles_valid; i++){ - const auto& obstacle_global_position = obstacles.global_robot_position[i]; - const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_pose.pose.position, obstacle_global_position); - const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); - - if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ - // perform obstacle avoidance - const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_pose.pose.position, obstacle_global_position); - const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_global_position); - - // True if setpoint is within a 120 deg cone away from the obstacle - // TODO: turn this angle into a param once complete - const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; - - const auto clearance = std::max(Config::obstacle_clearance_max*drone_speed_ratio, Config::obstacle_clearance_min); - const auto minimum_vector = calcMinimumLocalVector(drone_angle_to_obstacle, drone_direction, clearance); - - // Rotate to global coordinate system - const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); + + if (checkAndAvoidSingleObstacle(setpoint, drone_pose.pose.position, drone_velocity, obstacles.global_robot_position[i], zone_msg)){ + setpoint_modified = true; + } + + if (i == 0){} // generate points for zone_pub_ - constexpr int N_points{6}; + constexpr int N_points{16}; geometry_msgs::Polygon polygon; for (int i = 0; i < N_points; i++){ const float angle = static_cast(i)*2.f*PI/N_points; - auto local_pos = calcMinimumLocalVector(angle, drone_direction, clearance); + const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); + const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); + const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed + + const auto local_pos = calcMinimumLocalVector(angle, drone_direction, drone_speed_ratio, control::Config::obstacle_clearance_min, 2.0*control::Config::obstacle_clearance_min); geometry_msgs::Point32 global_pos; - global_pos.x = obstacle_global_position.x + local_pos.x; - global_pos.y = obstacle_global_position.y + local_pos.y; - global_pos.z = obstacle_global_position.z + local_pos.z; - - polygon.points.push_back(global_pos); - } + global_pos.x = obstacles.global_robot_position[i].x + local_pos.x; + global_pos.y = obstacles.global_robot_position[i].y + local_pos.y; + global_pos.z = obstacles.global_robot_position[i].z + local_pos.z; - zone_msg.polygons.push_back(polygon); + polygon.points.push_back(global_pos); - if (drone_distance_to_obstacle < minimum_distance){ - if (setpoint_reachable - && setpoint_distance_to_obstacle > drone_distance_to_obstacle - && setpoint_distance_to_obstacle > minimum_distance){ - // no action, maybe logging? - ROS_INFO_THROTTLE(1, "[obstacle avoidance]: Prefer setpoint over avoid algorithm"); - } - else { - if (setpoint_modified){ - // TODO: find out a better solution to this problem - ROS_WARN("[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); - setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) - } - // need to avoid obstacle - setpoint_modified = true; - setpoint->position.x = obstacle_global_position.x + minimum_vector.x; - setpoint->position.y = obstacle_global_position.y + minimum_vector.y; - if (setpoint->position.z < Config::min_in_air_alt){ - setpoint->position.z = Config::min_in_air_alt; - } - } - } + zone_msg.polygons.push_back(polygon); } + + + if (setpoint_modified){ //const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); //ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); From 172caf80710db54c7760eb5aa5f93802b2923447 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 12 Apr 2018 15:26:55 +0200 Subject: [PATCH 187/200] Change to local pose --- src/nodes/path_planner_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 812b004..988e238 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -71,7 +71,7 @@ int main(int argc, char** argv){ // For saving memory points_in_plan.reserve(20); - while(!control::DroneHandler::isPoseValid() && control::ObstacleStateHandler::isInstanceReady() && ros::ok()){ + while(!control::DroneHandler::isLocalPoseValid() && control::ObstacleStateHandler::isInstanceReady() && ros::ok()){ ros::spinOnce(); ros::Duration(1.0).sleep(); } @@ -95,7 +95,7 @@ int main(int argc, char** argv){ plan.refreshUnsafeZones(); // Update planner state - geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentPose(); + geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentLocalPose(); auto& position = current_pose.pose.position; planner_state.current_x = position.x; planner_state.current_y = position.y; From ec9b678a3149372a6780bbe15d825cb7eb374ff0 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 12 Apr 2018 16:15:55 +0200 Subject: [PATCH 188/200] Upgrade area --- .gitignore | 1 + src/tools/obstacle_avoidance.cpp | 20 ++++++-------------- 2 files changed, 7 insertions(+), 14 deletions(-) diff --git a/.gitignore b/.gitignore index 6844267..e001a26 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,5 @@ cmake-build-debug/ *.out tags .vscode +*.swp diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 1e8ce0d..3877f14 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -34,22 +34,18 @@ using obstacle_math::calcDistanceToObstacle; geometry_msgs::Vector3 calcMinimumLocalVector(const float drone_angle_to_obstacle, const float drone_direction, const float drone_speed_ratio, const float clearance_min, const float clearance_max){ - auto angle = angleWrapper(drone_angle_to_obstacle - drone_direction); + auto angle = angleWrapper(PI + drone_angle_to_obstacle + drone_direction); + auto front_clearance = clearance_min; if (drone_speed_ratio < 0.1){ // direction is proabably noise, ignore it - angle = 0.0; - ROS_INFO_THROTTLE(5,"drone_speed_ratio %f", drone_speed_ratio); - } - - auto front_clearance = clearance_min; - if (angle < PI){ // drone is driving towards obstacle front_clearance = clearance_max; + ROS_INFO_THROTTLE(5,"drone_speed_ratio %f", drone_speed_ratio); } geometry_msgs::Vector3 minimum_vector; - minimum_vector.x = front_clearance * std::cos(drone_angle_to_obstacle); - minimum_vector.y = clearance_min * std::sin(drone_angle_to_obstacle); + minimum_vector.x = front_clearance * std::cos(angle); + minimum_vector.y = clearance_min * std::sin(angle); minimum_vector.z = 0.0f; //ROS_INFO_THROTTLE(1, "local(%.3f,%.3f,%.3f)", local_pos.x, local_pos.y, local_pos.z); @@ -127,14 +123,13 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget if (checkAndAvoidSingleObstacle(setpoint, drone_pose.pose.position, drone_velocity, obstacles.global_robot_position[i], zone_msg)){ setpoint_modified = true; } - if (i == 0){} // generate points for zone_pub_ constexpr int N_points{16}; geometry_msgs::Polygon polygon; for (int i = 0; i < N_points; i++){ - const float angle = static_cast(i)*2.f*PI/N_points; + const float angle = static_cast(i)*2*PI/N_points; const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed @@ -151,9 +146,6 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget zone_msg.polygons.push_back(polygon); } - - - if (setpoint_modified){ //const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); //ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); From d92a46ee004f8b04f032125e6cc30de1538feb7e Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 12 Apr 2018 16:42:49 +0200 Subject: [PATCH 189/200] Fix misplaced bracket --- src/tools/obstacle_avoidance.cpp | 47 ++++++++++++++++---------------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 3877f14..900f07e 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -34,10 +34,10 @@ using obstacle_math::calcDistanceToObstacle; geometry_msgs::Vector3 calcMinimumLocalVector(const float drone_angle_to_obstacle, const float drone_direction, const float drone_speed_ratio, const float clearance_min, const float clearance_max){ - auto angle = angleWrapper(PI + drone_angle_to_obstacle + drone_direction); + auto angle = angleWrapper(PI + drone_angle_to_obstacle - drone_direction); auto front_clearance = clearance_min; - if (drone_speed_ratio < 0.1){ + if (drone_speed_ratio > 0.1){ // direction is proabably noise, ignore it front_clearance = clearance_max; ROS_INFO_THROTTLE(5,"drone_speed_ratio %f", drone_speed_ratio); @@ -123,27 +123,28 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget if (checkAndAvoidSingleObstacle(setpoint, drone_pose.pose.position, drone_velocity, obstacles.global_robot_position[i], zone_msg)){ setpoint_modified = true; } - - if (i == 0){} - // generate points for zone_pub_ - constexpr int N_points{16}; - geometry_msgs::Polygon polygon; - for (int i = 0; i < N_points; i++){ - const float angle = static_cast(i)*2*PI/N_points; - const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); - const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); - const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed - - const auto local_pos = calcMinimumLocalVector(angle, drone_direction, drone_speed_ratio, control::Config::obstacle_clearance_min, 2.0*control::Config::obstacle_clearance_min); - - geometry_msgs::Point32 global_pos; - global_pos.x = obstacles.global_robot_position[i].x + local_pos.x; - global_pos.y = obstacles.global_robot_position[i].y + local_pos.y; - global_pos.z = obstacles.global_robot_position[i].z + local_pos.z; - - polygon.points.push_back(global_pos); - - zone_msg.polygons.push_back(polygon); + + // generate points for zone_pub_ + constexpr int N_points{8}; + geometry_msgs::Polygon polygon; + for (int j = 0; j < N_points; j++){ + const float angle = static_cast(j)*2.f*PI/static_cast(N_points); + const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); + const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); + const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed + + const auto local_pos = calcMinimumLocalVector(angle, drone_direction, drone_speed_ratio, control::Config::obstacle_clearance_min, 2.0*control::Config::obstacle_clearance_min); + + ROS_INFO_THROTTLE(1,"angle, local_pos: %.3f %.3f %.3f %.3f", angle, local_pos.x, local_pos.y, local_pos.z); + + geometry_msgs::Point32 global_pos; + global_pos.x = obstacles.global_robot_position[i].x + local_pos.x; + global_pos.y = obstacles.global_robot_position[i].y + local_pos.y; + global_pos.z = obstacles.global_robot_position[i].z + local_pos.z; + + polygon.points.push_back(global_pos); + + zone_msg.polygons.push_back(polygon); } if (setpoint_modified){ From af0bfcdbb01152ee1f493774964b61044009b66b Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 12 Apr 2018 17:55:15 +0200 Subject: [PATCH 190/200] Expand the unsafe zone to include the next positions of the obstacles --- include/control/planner/path_planner.hpp | 1 + src/path_planner/path_planner.cpp | 15 +++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index fc0af93..6767f18 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -68,6 +68,7 @@ class PathPlanner{ // Add a circular obstacle void addObstacle(float center_x, float center_y); + Obstacle obstacleNextPos(float obstacle_x, float obstacle_y); void refreshObstacles(); Obstacle* findBlockingObstacle(float current_x, float current_y); diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index eabe667..748525f 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -83,6 +83,17 @@ void PathPlanner::refreshObstacles() { } } +Obstacle PathPlanner::obstacleNextPos(float obstacle_x, float obstacle_y) { + float center_x = 10; + float center_y = 10; + float obstacle_angle = atan2(obstacle_y-center_y, obstacle_x-center_x); // radians + float angle_to_next_pos = -0.17; // radians + float radius = sqrt((center_x-obstacle_x)*(center_x-obstacle_x)+(center_y-obstacle_y)*(center_y-obstacle_y)); + float next_x = center_x + radius*cos(obstacle_angle+angle_to_next_pos); + float next_y = center_y + radius*sin(obstacle_angle+angle_to_next_pos); + return Obstacle(next_x,next_y,0); +} + void PathPlanner::addUnsafeZone(float center_x, float center_y) { // Same logic as adding obstacles, only difference is the radius float unsafe_radius = obstacle_radius*2; @@ -123,6 +134,10 @@ void PathPlanner::refreshUnsafeZones(){ for(auto current = obstacles.begin(); current != obstacles.end(); current++){ addUnsafeZone(current->x, current->y); + Obstacle next_obstacle = obstacleNextPos(current->x,current->y); + addUnsafeZone(next_obstacle.x, next_obstacle.y); + next_obstacle = obstacleNextPos(next_obstacle.x,next_obstacle.y); + addUnsafeZone(next_obstacle.x, next_obstacle.y); } } From 001b3c8c8e0e5e48f7463e3074f42174faf3f298 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 12 Apr 2018 17:55:37 +0200 Subject: [PATCH 191/200] Smarter debugging --- src/path_planner/path_planner.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 748525f..ada1670 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -4,7 +4,7 @@ using namespace control::pathplanner; -bool debug = true; +bool debug = false; PathPlanner::PathPlanner(float obstacle_radius, float node_distance) @@ -214,7 +214,7 @@ void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent } void PathPlanner::handleAllSuccessors(float x, float y) { - //std::cout << "q = " << x << ", " << y << std::endl; + if(debug) {std::cout << "q = " << x << ", " << y << std::endl;} handleSuccessor(x+node_distance,y, x, y, node_distance); if(destination_reached){return;} handleSuccessor(x+node_distance, y+node_distance, x, y, diagonal_node_distance); From 4e2630246b64be0d1004427c426442f3c2cd3740 Mon Sep 17 00:00:00 2001 From: atussa Date: Thu, 12 Apr 2018 17:58:34 +0200 Subject: [PATCH 192/200] Obstacles moving counter clockwise --- src/path_planner/path_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index ada1670..d706188 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -87,7 +87,7 @@ Obstacle PathPlanner::obstacleNextPos(float obstacle_x, float obstacle_y) { float center_x = 10; float center_y = 10; float obstacle_angle = atan2(obstacle_y-center_y, obstacle_x-center_x); // radians - float angle_to_next_pos = -0.17; // radians + float angle_to_next_pos = 0.17; // radians float radius = sqrt((center_x-obstacle_x)*(center_x-obstacle_x)+(center_y-obstacle_y)*(center_y-obstacle_y)); float next_x = center_x + radius*cos(obstacle_angle+angle_to_next_pos); float next_y = center_y + radius*sin(obstacle_angle+angle_to_next_pos); From 2e7f4d77f9849a89bca5d871a058cfc91496cb92 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 12 Apr 2018 18:43:13 +0200 Subject: [PATCH 193/200] Be more strict on area in front of an obstacle --- src/path_planner/path_planner.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index d706188..b53ad81 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -80,6 +80,10 @@ void PathPlanner::refreshObstacles() { for(auto current = obstacles.begin(); current != obstacles.end(); current++){ addObstacle(current->x, current->y); + Obstacle next_obstacle = obstacleNextPos(current->x,current->y); + addObstacle(next_obstacle.x, next_obstacle.y); + next_obstacle = obstacleNextPos(next_obstacle.x,next_obstacle.y); + addObstacle(next_obstacle.x, next_obstacle.y); } } @@ -134,10 +138,6 @@ void PathPlanner::refreshUnsafeZones(){ for(auto current = obstacles.begin(); current != obstacles.end(); current++){ addUnsafeZone(current->x, current->y); - Obstacle next_obstacle = obstacleNextPos(current->x,current->y); - addUnsafeZone(next_obstacle.x, next_obstacle.y); - next_obstacle = obstacleNextPos(next_obstacle.x,next_obstacle.y); - addUnsafeZone(next_obstacle.x, next_obstacle.y); } } From ab0ba77bff6694e76d16254624ec609c58ad230b Mon Sep 17 00:00:00 2001 From: atussa Date: Fri, 13 Apr 2018 11:58:03 +0200 Subject: [PATCH 194/200] Obstacle zone should be smaller than unsafe zone --- src/path_planner/path_planner.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index d706188..8a3aef0 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -80,6 +80,8 @@ void PathPlanner::refreshObstacles() { for(auto current = obstacles.begin(); current != obstacles.end(); current++){ addObstacle(current->x, current->y); + Obstacle next_obstacle = obstacleNextPos(current->x,current->y); + addObstacle(next_obstacle.x, next_obstacle.y); } } From c3f0a8ffe7c4f4eafd821f1dc41dbde2ef0920b5 Mon Sep 17 00:00:00 2001 From: atussa Date: Fri, 13 Apr 2018 11:58:22 +0200 Subject: [PATCH 195/200] Remove old variable --- src/path_planner/path_planner.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index 8a3aef0..8037043 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -299,7 +299,6 @@ void PathPlanner::makePlan(float current_x, float current_y, float target_x, flo if (debug) {std::cout << "Run makePlan" << std::endl;} no_plan = false; - bool escape_from_obstacle = false; // If start or end point is not valid, a plan is not created if(!isValidCoordinate(current_x,current_y)){ From 25b534a948ff3d3047a02215c88493308f587d31 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Fri, 13 Apr 2018 12:48:50 +0200 Subject: [PATCH 196/200] Add unsafe zone --- src/path_planner/path_planner.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp index ed6cdac..d38a76d 100644 --- a/src/path_planner/path_planner.cpp +++ b/src/path_planner/path_planner.cpp @@ -136,6 +136,10 @@ void PathPlanner::refreshUnsafeZones(){ for(auto current = obstacles.begin(); current != obstacles.end(); current++){ addUnsafeZone(current->x, current->y); + Obstacle next_obstacle = obstacleNextPos(current->x,current->y); + addUnsafeZone(next_obstacle.x, next_obstacle.y); + next_obstacle = obstacleNextPos(current->x,current->y); + addUnsafeZone(next_obstacle.x, next_obstacle.y); } } From 7fbf8c847b49db490b6629074248d022accbb450 Mon Sep 17 00:00:00 2001 From: atussa Date: Wed, 18 Apr 2018 13:14:06 +0200 Subject: [PATCH 197/200] Changes for running pathplanner on drone irl --- include/control/planner/path_planner.hpp | 2 +- launch/mist.launch | 1 + src/nodes/path_planner_node.cpp | 4 ++-- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index 6767f18..66b8c82 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -17,7 +17,7 @@ namespace control{ namespace pathplanner{ -#define FIELD_LENGTH 20.0 +#define FIELD_LENGTH 10.0 struct Obstacle { diff --git a/launch/mist.launch b/launch/mist.launch index 85a9f5b..f416ecf 100644 --- a/launch/mist.launch +++ b/launch/mist.launch @@ -10,4 +10,5 @@ + diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp index 91af025..64f3d64 100644 --- a/src/nodes/path_planner_node.cpp +++ b/src/nodes/path_planner_node.cpp @@ -71,7 +71,7 @@ int main(int argc, char** argv){ // For saving memory points_in_plan.reserve(20); - while(!control::DroneHandler::isLocalPoseValid() && control::ObstacleStateHandler::isInstanceReady() && ros::ok()){ + while(!control::DroneHandler::isGlobalPoseValid() && control::ObstacleStateHandler::isInstanceReady() && ros::ok()){ ros::spinOnce(); ros::Duration(1.0).sleep(); } @@ -95,7 +95,7 @@ int main(int argc, char** argv){ plan.refreshUnsafeZones(); // Update planner state - geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentLocalPose(); + geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentGlobalPose(); auto& position = current_pose.pose.position; planner_state.current_x = position.x; planner_state.current_y = position.y; From 415f5e01f32c31ac9272db71db724bbd8035e96c Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Wed, 18 Apr 2018 18:55:07 +0200 Subject: [PATCH 198/200] account for drone direction --- include/control/tools/obstacle_math.hpp | 2 +- src/tools/obstacle_avoidance.cpp | 47 ++++++++++++++----------- 2 files changed, 28 insertions(+), 21 deletions(-) diff --git a/include/control/tools/obstacle_math.hpp b/include/control/tools/obstacle_math.hpp index a549af6..7656ada 100644 --- a/include/control/tools/obstacle_math.hpp +++ b/include/control/tools/obstacle_math.hpp @@ -56,7 +56,7 @@ inline float calcAngleToObstacle(const T& point, const K& obstacle_position){ // Apply 2d transformation matrix template -inline T rotateXY(const T& point, const float angle){ +T rotateXY(const T& point, const float angle){ T new_point; new_point.x = point.x * std::cos(angle) - point.y * std::sin(angle); new_point.y = point.x * std::sin(angle) + point.y * std::cos(angle); diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 900f07e..d4b046b 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -31,25 +31,30 @@ using obstacle_math::calcDistanceToObstacle; /// Return a vector (x,y,z) which corresponds to the point closest to the obstacle /// which is allowed, referenced from the obstacles coordinate system. x-axis is positive /// direction of motion. Obstacle driving in with obstacle_direction=0 would be driving in x-direction -geometry_msgs::Vector3 calcMinimumLocalVector(const float drone_angle_to_obstacle, const float drone_direction, const float drone_speed_ratio, +geometry_msgs::Vector3 calcMinimumVector(const float drone_angle_to_obstacle, const float drone_direction, const float drone_speed_ratio, const float clearance_min, const float clearance_max){ - auto angle = angleWrapper(PI + drone_angle_to_obstacle - drone_direction); - - auto front_clearance = clearance_min; + + geometry_msgs::Vector3 minimum_vector; if (drone_speed_ratio > 0.1){ - // direction is proabably noise, ignore it - front_clearance = clearance_max; - ROS_INFO_THROTTLE(5,"drone_speed_ratio %f", drone_speed_ratio); - } + const auto worst_direction = PI + drone_direction; + const auto angle = angleWrapper(drone_angle_to_obstacle - worst_direction); + + const auto clearance_front = clearance_min + drone_speed_ratio*(clearance_max - clearance_min); - geometry_msgs::Vector3 minimum_vector; - minimum_vector.x = front_clearance * std::cos(angle); - minimum_vector.y = clearance_min * std::sin(angle); + minimum_vector.x = clearance_front * std::cos(angle); + + minimum_vector.y = clearance_min * std::sin(angle); + minimum_vector = rotateXY(minimum_vector, worst_direction); + + } else { + minimum_vector.x = clearance_min * std::cos(drone_angle_to_obstacle); + minimum_vector.y = clearance_min * std::sin(drone_angle_to_obstacle); + minimum_vector.z = 0.0f; + } + minimum_vector.z = 0.0f; - - //ROS_INFO_THROTTLE(1, "local(%.3f,%.3f,%.3f)", local_pos.x, local_pos.y, local_pos.z); - + return minimum_vector; } @@ -75,7 +80,7 @@ bool checkAndAvoidSingleObstacle(mavros_msgs::PositionTarget* setpoint, const V1 const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; const auto clearance = std::max(Config::obstacle_clearance_max*drone_speed_ratio, Config::obstacle_clearance_min); - const auto minimum_vector = calcMinimumLocalVector(drone_angle_to_obstacle, drone_direction, drone_speed_ratio, clearance, 2.0*clearance); + const auto minimum_vector = calcMinimumVector(drone_angle_to_obstacle, drone_direction, drone_speed_ratio, clearance, 2.0*clearance); const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); if (drone_distance_to_obstacle < minimum_distance){ @@ -113,7 +118,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); const auto drone_pose = control::DroneHandler::getCurrentLocalPose(); const auto drone_velocity = control::DroneHandler::getCurrentTwist().twist.linear; - + ascend_msgs::PolygonArray zone_msg; bool obstacles_valid = control::ObstacleStateHandler::isInstanceReady(); @@ -125,7 +130,7 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget } // generate points for zone_pub_ - constexpr int N_points{8}; + constexpr int N_points{16}; geometry_msgs::Polygon polygon; for (int j = 0; j < N_points; j++){ const float angle = static_cast(j)*2.f*PI/static_cast(N_points); @@ -133,8 +138,10 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed - const auto local_pos = calcMinimumLocalVector(angle, drone_direction, drone_speed_ratio, control::Config::obstacle_clearance_min, 2.0*control::Config::obstacle_clearance_min); + const auto local_pos = calcMinimumVector(angle, drone_direction, drone_speed_ratio, control::Config::obstacle_clearance_min, 2.0*control::Config::obstacle_clearance_min); + + ROS_INFO_THROTTLE(1,"angle, local_pos: %.3f %.3f %.3f %.3f", angle, local_pos.x, local_pos.y, local_pos.z); geometry_msgs::Point32 global_pos; @@ -143,9 +150,9 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget global_pos.z = obstacles.global_robot_position[i].z + local_pos.z; polygon.points.push_back(global_pos); - - zone_msg.polygons.push_back(polygon); } + + zone_msg.polygons.push_back(polygon); if (setpoint_modified){ //const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); From a30a3a2db5b5c96546e1831c8889e0b7db4675bb Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Wed, 18 Apr 2018 20:27:21 +0200 Subject: [PATCH 199/200] account for drone direction --- src/tools/obstacle_avoidance.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index d4b046b..cbec7c8 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -67,7 +67,7 @@ bool checkAndAvoidSingleObstacle(mavros_msgs::PositionTarget* setpoint, const V1 const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); - const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed + const float drone_speed_ratio = std::min((float)drone_speed/1.0f, 1.f); // 1.0 m/s is assumed drone max speed bool setpoint_modified = false; if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ From bee2fa97bacfbcf9866bf484ad2f656663ec3f45 Mon Sep 17 00:00:00 2001 From: Rendell Cale Date: Thu, 19 Apr 2018 12:28:09 +0200 Subject: [PATCH 200/200] remove debug prints --- ' | 214 +++++++++++++++++++++++++++++++ src/tools/obstacle_avoidance.cpp | 21 ++- 2 files changed, 223 insertions(+), 12 deletions(-) create mode 100644 ' diff --git a/' b/' new file mode 100644 index 0000000..816f5ad --- /dev/null +++ b/' @@ -0,0 +1,214 @@ +#include + +#include "control/tools/obstacle_avoidance.hpp" +#include "control/tools/obstacle_math.hpp" +#include "control/tools/obstacle_state_handler.hpp" +#include "control/tools/drone_handler.hpp" +#include "control/tools/config.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using obstacle_math::PI; +using obstacle_math::rotateXY; +using obstacle_math::angleWrapper; +using obstacle_math::vectorSum; +using obstacle_math::vectorDifference; +using obstacle_math::calcAngleToObstacle; +using obstacle_math::calcDistanceToObstacle; + +/// Return a vector (x,y,z) which corresponds to the point closest to the obstacle +/// which is allowed, referenced from the obstacles coordinate system. x-axis is positive +/// direction of motion. Obstacle driving in with obstacle_direction=0 would be driving in x-direction +geometry_msgs::Vector3 calcMinimumVector(const float drone_angle_to_obstacle, const float drone_direction, const float drone_speed_ratio, + const float clearance_min, const float clearance_max){ + + geometry_msgs::Vector3 minimum_vector; + + const auto clearance_front = clearance_min + drone_speed_ratio*(clearance_max - clearance_min); + + if (drone_speed_ratio > 0.1){ + const auto worst_direction = PI + drone_direction; + const auto angle = angleWrapper(drone_angle_to_obstacle - worst_direction); + + minimum_vector.x = clearance_front * std::cos(angle); + minimum_vector.y = clearance_min * std::sin(angle); + minimum_vector = rotateXY(minimum_vector, worst_direction); + + } else { + minimum_vector.x = clearance_min * std::cos(drone_angle_to_obstacle); + minimum_vector.y = clearance_min * std::sin(drone_angle_to_obstacle); + } + + minimum_vector.z = 0.0f; + + return minimum_vector; +} + +template // vector types with x,y,z +bool checkAndAvoidSingleObstacle(mavros_msgs::PositionTarget* setpoint, const V1& drone_position, const V2& drone_velocity, const V3& obstacle_global_position, ascend_msgs::PolygonArray& zone_msg){ + using control::Config; + + const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_position, obstacle_global_position); + const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); + + const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); + const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); + const float drone_speed_ratio = std::min((float)drone_speed/1.0f, 1.f); // 1.0 m/s is assumed drone max speed + + bool setpoint_modified = false; + if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ + // perform obstacle avoidance + const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_position, obstacle_global_position); + const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_global_position); + + // True if setpoint is within a 120 deg cone away from the obstacle + // TODO: turn this angle into a param once complete + const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; + + const auto minimum_vector = calcMinimumVector(drone_angle_to_obstacle, drone_direction, drone_speed_ratio, Config::obstacle_clearance_min, Config::obstacle_clearance_max); + + const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); + if (drone_distance_to_obstacle < minimum_distance){ + if (setpoint_reachable + && setpoint_distance_to_obstacle > drone_distance_to_obstacle + && setpoint_distance_to_obstacle > minimum_distance){ + // no action, maybe logging? + ROS_INFO_THROTTLE(1, "[obstacle avoidance]: Prefer setpoint over avoid algorithm"); + } + else { + if (setpoint_modified){ + // TODO: find out a better solution to this problem + ROS_WARN("[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); + setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) + } + // need to avoid obstacle + setpoint_modified = true; + + setpoint->position.x = obstacle_global_position.x + minimum_vector.x; + setpoint->position.y = obstacle_global_position.y + minimum_vector.y; + if (setpoint->position.z < Config::min_in_air_alt){ + setpoint->position.z = Config::min_in_air_alt; + } + } + } + } + + return setpoint_modified; +} + +bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { + + bool setpoint_modified{false}; + + const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + const auto drone_pose = control::DroneHandler::getCurrentLocalPose(); + const auto drone_velocity = control::DroneHandler::getCurrentTwist().twist.linear; + + ascend_msgs::PolygonArray zone_msg; + + bool obstacles_valid = control::ObstacleStateHandler::isInstanceReady(); + + for (int i = 0; i < obstacles.count && obstacles_valid; i++){ + + if (checkAndAvoidSingleObstacle(setpoint, drone_pose.pose.position, drone_velocity, obstacles.global_robot_position[i], zone_msg)){ + setpoint_modified = true; + } + + // generate points for zone_pub_ + constexpr int N_points{16}; + geometry_msgs::Polygon polygon; + for (int j = 0; j < N_points; j++){ + const float angle = static_cast(j)*2.f*PI/static_cast(N_points); + const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); + const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); + const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed + + const auto local_pos = calcMinimumVector(angle, drone_direction, drone_speed_ratio, control::Config::obstacle_clearance_min, 2.0*control::Config::obstacle_clearance_min); + + + + ROS_INFO_THROTTLE(1,"angle, local_pos: %.3f %.3f %.3f %.3f", angle, local_pos.x, local_pos.y, local_pos.z); + + geometry_msgs::Point32 global_pos; + global_pos.x = obstacles.global_robot_position[i].x + local_pos.x; + global_pos.y = obstacles.global_robot_position[i].y + local_pos.y; + global_pos.z = obstacles.global_robot_position[i].z + local_pos.z; + + polygon.points.push_back(global_pos); + } + + zone_msg.polygons.push_back(polygon); + + if (setpoint_modified){ + //const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); + //ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); + } + } + + zone_pub_.publish(zone_msg); + + return setpoint_modified; +} + +void control::ObstacleAvoidance::onModified() { + //Run all callbacks + for(auto& cb_p : on_modified_cb_set_ ) { + (*cb_p)(); + } +} + +void control::ObstacleAvoidance::onWarn() { + //Run all callbacks + for(auto& cb_p : on_warn_cb_set_ ) { + (*cb_p)(); + } +} + +mavros_msgs::PositionTarget control::ObstacleAvoidance::run(mavros_msgs::PositionTarget setpoint) { + mavros_msgs::PositionTarget setpoint_unchanged = setpoint; + + const bool setpoint_modified = doObstacleAvoidance(&setpoint); + + if (setpoint_modified){ + //Notify states + onWarn(); + if (has_setpoint_responsibility_){ + onModified(); + return setpoint; + } + } + + return setpoint_unchanged; +} + +void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr >& cb_p) { + const auto it = on_modified_cb_set_.find(cb_p); + if (it != on_modified_cb_set_.cend()){ + on_modified_cb_set_.erase(it); + } +} + +void control::ObstacleAvoidance::removeOnWarnCBPtr(const std::shared_ptr >& cb_p) { + const auto it = on_warn_cb_set_.find(cb_p); + if (it != on_warn_cb_set_.cend()) { + on_warn_cb_set_.erase(it); + } +} + + +control::ObstacleAvoidance::ObstacleAvoidance(){ + zone_pub_ = nh_.advertise("/control/obstacle_avoidance/polygons", 10); +} + diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index cbec7c8..66e6aa9 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -34,23 +34,21 @@ using obstacle_math::calcDistanceToObstacle; geometry_msgs::Vector3 calcMinimumVector(const float drone_angle_to_obstacle, const float drone_direction, const float drone_speed_ratio, const float clearance_min, const float clearance_max){ - geometry_msgs::Vector3 minimum_vector; + + const auto clearance_front = clearance_min + drone_speed_ratio*(clearance_max - clearance_min); + if (drone_speed_ratio > 0.1){ const auto worst_direction = PI + drone_direction; const auto angle = angleWrapper(drone_angle_to_obstacle - worst_direction); - - const auto clearance_front = clearance_min + drone_speed_ratio*(clearance_max - clearance_min); minimum_vector.x = clearance_front * std::cos(angle); - minimum_vector.y = clearance_min * std::sin(angle); minimum_vector = rotateXY(minimum_vector, worst_direction); } else { minimum_vector.x = clearance_min * std::cos(drone_angle_to_obstacle); minimum_vector.y = clearance_min * std::sin(drone_angle_to_obstacle); - minimum_vector.z = 0.0f; } minimum_vector.z = 0.0f; @@ -79,8 +77,7 @@ bool checkAndAvoidSingleObstacle(mavros_msgs::PositionTarget* setpoint, const V1 // TODO: turn this angle into a param once complete const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; - const auto clearance = std::max(Config::obstacle_clearance_max*drone_speed_ratio, Config::obstacle_clearance_min); - const auto minimum_vector = calcMinimumVector(drone_angle_to_obstacle, drone_direction, drone_speed_ratio, clearance, 2.0*clearance); + const auto minimum_vector = calcMinimumVector(drone_angle_to_obstacle, drone_direction, drone_speed_ratio, Config::obstacle_clearance_min, Config::obstacle_clearance_max); const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); if (drone_distance_to_obstacle < minimum_distance){ @@ -121,7 +118,11 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget ascend_msgs::PolygonArray zone_msg; - bool obstacles_valid = control::ObstacleStateHandler::isInstanceReady(); + const bool obstacles_valid = control::ObstacleStateHandler::isInstanceReady(); + + if (!obstacles_valid){ + ROS_WARN_THROTTLE(1, "[obstacle_avoidance]: ObstacleStateHandler not ready"); + } for (int i = 0; i < obstacles.count && obstacles_valid; i++){ @@ -140,10 +141,6 @@ bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget const auto local_pos = calcMinimumVector(angle, drone_direction, drone_speed_ratio, control::Config::obstacle_clearance_min, 2.0*control::Config::obstacle_clearance_min); - - - ROS_INFO_THROTTLE(1,"angle, local_pos: %.3f %.3f %.3f %.3f", angle, local_pos.x, local_pos.y, local_pos.z); - geometry_msgs::Point32 global_pos; global_pos.x = obstacles.global_robot_position[i].x + local_pos.x; global_pos.y = obstacles.global_robot_position[i].y + local_pos.y;