Skip to content

Commit

Permalink
Connect nav_node obstacle gathering to cluster_detector
Browse files Browse the repository at this point in the history 
As it was observed last year, the kalmann filter used in lidar
triangulation introduce a small delay, this delay can become fatal for
this node.

In this commit I changed the obstacle detection to gather information
directly from the cluster detector, it imply that nav_node do not know
the exact number of obstacle on the playground nor the position in an
absolute coordinate system.

A new function has been introduce to convert circles from relative
coordinate to absolute coordinate.

Moreover, I unified the datatypes by considering the previous type
"Circle" as the new type "CircleObstacle" from cdf_msgs. This choice was
motivated by the fact that I had to rethink the obstacle processing
algorithm.

Signed-off-by: Thomas Bonnefille <thomas.bonnefille@outlook.com>
  • Loading branch information
@Taumille
Taumille committed Nov 5, 2023
1 parent ede338c commit cef0f17
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 74 deletions.
19 changes: 7 additions & 12 deletions include/nav_node/nav_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,16 +20,12 @@
// Messages
#include "cdf_msgs/msg/trajectoire.hpp"
#include "cdf_msgs/msg/pic_action.hpp"
#include "cdf_msgs/msg/merged_data_bis.hpp"
#include "cdf_msgs/msg/obstacles.hpp"
#include "cdf_msgs/msg/circle_obstacle.hpp"
#include "cdf_msgs/msg/robot_data.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "std_msgs/msg/bool.hpp"

struct Circle{
Point center;
float radius;
};

class Nav_node : public rclcpp::Node
{
public:
Expand All @@ -48,7 +44,7 @@ class Nav_node : public rclcpp::Node
rclcpp::Subscription<cdf_msgs::msg::RobotData>::SharedPtr sub_robot_data;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr stop_sub;
rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr goal_sub;
rclcpp::Subscription<cdf_msgs::msg::MergedDataBis>::SharedPtr obstacles_sub;
rclcpp::Subscription<cdf_msgs::msg::Obstacles>::SharedPtr obstacles_sub;



Expand All @@ -61,14 +57,13 @@ class Nav_node : public rclcpp::Node
void robot_data_callback(const cdf_msgs::msg::RobotData msg);
void stop_callback(const std_msgs::msg::Bool msg);
void goal_callback(const geometry_msgs::msg::Point msg);
void obstacles_callback(const cdf_msgs::msg::MergedDataBis msg);
void obstacles_callback(const cdf_msgs::msg::Obstacles msg);

// Functions
void get_next_goal();
void obstacle_processing(Circle obstacle[3]);
void obstacle_disjunction(cdf_msgs::msg::MergedDataBis obstacles);
void obstacle_processing(std::vector<cdf_msgs::msg::CircleObstacle> obstacle);
void or_map(bool map[MAP_WIDTH][MAP_HEIGHT], bool map2[MAP_WIDTH][MAP_HEIGHT]);
void make_circle_map(Circle obstacle, bool map[MAP_WIDTH][MAP_HEIGHT]);
void make_circle_map(cdf_msgs::msg::CircleObstacle obstacle, bool map[MAP_WIDTH][MAP_HEIGHT]);

// Navigation algorithm objects
PStar nav_alg;
Expand All @@ -83,7 +78,7 @@ class Nav_node : public rclcpp::Node
std::string map_file;

Point next_goal;
Circle obstacles[3];
std::vector<cdf_msgs::msg::CircleObstacle> obstacles;

// ROS Parameters
float goal_tolerance;
Expand Down
3 changes: 2 additions & 1 deletion include/nav_node/pstar.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
struct Point{
int x;
int y;
float theta;
};

class PStar{
Expand All @@ -24,4 +25,4 @@ class PStar{
bool get_cost(Point p);
};

#endif // PSTAR_HPP
#endif // PSTAR_HPP
116 changes: 55 additions & 61 deletions src/nav_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,27 @@ bool is_defined(Point a){
return (a.x != -1 && a.y != -1);
}

cdf_msgs::msg::CircleObstacle to_absolute_coordinate(cdf_msgs::msg::CircleObstacle circle, Point robot_position){
/* Convert point from the robot coordinate system to
the playground coordinate system.
circle : a circle_obstacle object containing the object coordinate to convert
robot_position : Position of the robot in the playground coordinate system
rotation : angle between the x axis of the playground and the front of the robot
*/
cdf_msgs::msg::CircleObstacle result;

float sin_p, cos_p;
sin_p = sin(robot_position.theta);
cos_p = cos(robot_position.theta);

result.center.x = robot_position.x + 100 * (cos_p * circle.center.x - sin_p * circle.center.y);
result.center.y = robot_position.y + 100 * (sin_p * circle.center.x - cos_p * circle.center.y);
result.radius = circle.radius;

return result;
}

Nav_node::Nav_node() : Node("nav_node"){
//Constructor
this->path = std::vector<Point>();
Expand Down Expand Up @@ -45,6 +66,7 @@ Nav_node::Nav_node() : Node("nav_node"){
this->goal_sub = this->create_subscription<geometry_msgs::msg::Point>(position_goal_topic, 1000, std::bind(&Nav_node::goal_callback, this, _1));
this->sub_robot_data = this->create_subscription<cdf_msgs::msg::RobotData>(robot_data_topic, 1000, std::bind(&Nav_node::robot_data_callback, this, _1));
this->stop_sub = this->create_subscription<std_msgs::msg::Bool>(stop_topic, 1000, std::bind(&Nav_node::stop_callback, this, _1));
this->obstacles_sub = this->create_subscription<cdf_msgs::msg::Obstacles>(stop_topic, 1000, std::bind(&Nav_node::obstacles_callback, this, _1));

// Load the map
this->load_map_file(map_file);
Expand Down Expand Up @@ -76,7 +98,7 @@ void Nav_node::or_map(bool map[MAP_WIDTH][MAP_HEIGHT], bool map2[MAP_WIDTH][MAP_
}
}

void Nav_node::make_circle_map(Circle obstacle, bool map[MAP_WIDTH][MAP_HEIGHT]){
void Nav_node::make_circle_map(cdf_msgs::msg::CircleObstacle obstacle, bool map[MAP_WIDTH][MAP_HEIGHT]){
/*
Create a map with a circle obstacle
*/
Expand All @@ -94,73 +116,19 @@ void Nav_node::make_circle_map(Circle obstacle, bool map[MAP_WIDTH][MAP_HEIGHT])

}

void Nav_node::obstacle_disjunction(cdf_msgs::msg::MergedDataBis MergedData){
/*
This function will take the MergedDataBis message and extract the three obstacles
*/

Circle obstacle[3];

if (this->robot_number == 1){
auto tmp = MergedData.robot_2.size();

if (MergedData.robot_2[tmp - 1].position.x == 0 && MergedData.robot_2[tmp - 1].position.y == 0){
// Robot 2 is not defined
tmp = MergedData.ennemi_3.size();
obstacle[0].center.x = MergedData.ennemi_3[tmp - 1].position.x * 100;
obstacle[0].center.y = MergedData.ennemi_3[tmp - 1].position.y * 100;
}
else{
// Robot 2 is defined
tmp = MergedData.robot_2.size();
obstacle[0].center.x = MergedData.robot_2[tmp - 1].position.x * 100;
obstacle[0].center.y = MergedData.robot_2[tmp - 1].position.y * 100;
}

}
else{
auto tmp = MergedData.robot_1.size();
if (MergedData.robot_1[tmp - 1].position.x == 0 && MergedData.robot_1[tmp - 1].position.y == 0){
// Robot 1 is not defined
auto tmp = MergedData.ennemi_3.size();
obstacle[0].center.x = MergedData.ennemi_3[tmp - 1].position.x * 100;
obstacle[0].center.y = MergedData.ennemi_3[tmp - 1].position.y * 100;
}
else{
// Robot 1 is defined
auto tmp = MergedData.robot_1.size();
obstacle[0].center.x = MergedData.robot_1[tmp - 1].position.x * 100;
obstacle[0].center.y = MergedData.robot_1[tmp - 1].position.y * 100;
}
}

auto tmp = MergedData.ennemi_1.size();
obstacle[1].center.x = MergedData.ennemi_1[tmp - 1].position.x * 100;
obstacle[1].center.y = MergedData.ennemi_1[tmp - 1].position.y * 100;

tmp = MergedData.ennemi_2.size();
obstacle[2].center.x = MergedData.ennemi_2[tmp - 1].position.x * 100;
obstacle[2].center.y = MergedData.ennemi_2[tmp - 1].position.y * 100;

#ifndef WORLD_OF_SILENCE
RCLCPP_INFO(this->get_logger(), "Obstacle 1 : (%d, %d)", obstacle[0].center.x, obstacle[0].center.y);
RCLCPP_INFO(this->get_logger(), "Obstacle 2 : (%d, %d)", obstacle[1].center.x, obstacle[1].center.y);
RCLCPP_INFO(this->get_logger(), "Obstacle 3 : (%d, %d)", obstacle[2].center.x, obstacle[2].center.y);
#endif

this->obstacle_processing(obstacle);
}

void Nav_node::obstacle_processing(Circle obstacle[3]){
void Nav_node::obstacle_processing(std::vector<cdf_msgs::msg::CircleObstacle> obstacle){
/*
Given the three obstacles, this function will place circle on the map
*/

// Try to see if obstacles are different
bool variation = false;
for (int i = 0; i < 3; i++){

int i_len = obstacle.size();
int j_len = this->obstacles.size();
for (int i = 0; i < i_len; i++){
variation = true;
for (int j = 0; j < 3; j++){
for (int j = 0; j < j_len; j++){
if (abs(obstacle[i].radius - this->obstacles[j].radius) > this->variation_obs\
&& abs(obstacle[i].center.x - this->obstacles[j].center.x) > this->variation_obs\
&& abs(obstacle[i].center.y - this->obstacles[j].center.y) > this->variation_obs){
Expand Down Expand Up @@ -282,6 +250,31 @@ void Nav_node::load_map_file(std::string map_file){
#endif
}

void Nav_node::obstacles_callback(const cdf_msgs::msg::Obstacles msg){
/*
This function gather the msg from the cluster detector and process all the
obstacles to add them to the map.
*/
std::vector<cdf_msgs::msg::CircleObstacle> obstacle;
cdf_msgs::msg::CircleObstacle tmp_circle;

int range_msg = msg.circles.size();
for (int i = 0; i < range_msg; i++){
tmp_circle = to_absolute_coordinate(msg.circles[i], this->robot_position);

// If the obstacle is in the playground we can add it
if (tmp_circle.center.x > 0 &&
tmp_circle.center.x < 300 &&
tmp_circle.center.y > 0 &&
tmp_circle.center.y < 200){
obstacle.push_back(tmp_circle);
}
}

this->obstacle_processing(obstacle);
}


void Nav_node::robot_data_callback(const cdf_msgs::msg::RobotData msg){
/*
Get odometry information from the RobotData topic
Expand All @@ -293,6 +286,7 @@ void Nav_node::robot_data_callback(const cdf_msgs::msg::RobotData msg){
// Update the robot position
this->robot_position.x = (this->robot_data.position).x * 100;
this->robot_position.y = (this->robot_data.position).y * 100;
this->robot_position.theta = (this->robot_data.position).z;

#ifndef WORLD_OF_SILENCE
RCLCPP_INFO(this->get_logger(), "Robot position : (%d, %d)", this->robot_position.x, this->robot_position.y);
Expand Down

0 comments on commit cef0f17

Please sign in to comment.