From 7d197a8d88ff9e76dc67511211f22425e6900364 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 8 Feb 2021 20:21:32 +0100 Subject: [PATCH 01/18] Remove things that needed to be delete in merge of PR #27 #28 --- src/assurancetourix/assurancetourix/src/assurancetourix.cpp | 1 - src/modules/drive/include/drive.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/src/assurancetourix/assurancetourix/src/assurancetourix.cpp b/src/assurancetourix/assurancetourix/src/assurancetourix.cpp index 51cda057..c4d3d373 100644 --- a/src/assurancetourix/assurancetourix/src/assurancetourix.cpp +++ b/src/assurancetourix/assurancetourix/src/assurancetourix.cpp @@ -72,7 +72,6 @@ Assurancetourix::Assurancetourix() : Node("assurancetourix") { timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / refresh_frequency), std::bind(&Assurancetourix::simulation_marker_callback, this)); #endif - //timer_ = this->create_wall_timer(0.3s, std::bind(&Assurancetourix::detect, this)); // to remove for PR RCLCPP_INFO(this->get_logger(), "Assurancetourix has been started"); } diff --git a/src/modules/drive/include/drive.hpp b/src/modules/drive/include/drive.hpp index 598a74b2..1a8bc8fa 100644 --- a/src/modules/drive/include/drive.hpp +++ b/src/modules/drive/include/drive.hpp @@ -10,7 +10,6 @@ #include #endif #include "std_srvs/srv/set_bool.hpp" -#include "std_srvs/srv/trigger.hpp" #include "actuators_srvs/srv/slider.hpp" #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/joint_state.hpp" From e7ff01711f8fee1e436a2c576705179a523b4859 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 8 Feb 2021 20:24:02 +0100 Subject: [PATCH 02/18] Global planner works now at 2Hz, drive's garbage log not showing anymore --- src/modules/robot/behavior_trees/navigate_w_replanning_time.xml | 2 +- src/modules/robot/robot/interfaces.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/robot/behavior_trees/navigate_w_replanning_time.xml b/src/modules/robot/behavior_trees/navigate_w_replanning_time.xml index 7ceb1c0f..27a16821 100644 --- a/src/modules/robot/behavior_trees/navigate_w_replanning_time.xml +++ b/src/modules/robot/behavior_trees/navigate_w_replanning_time.xml @@ -5,7 +5,7 @@ - + diff --git a/src/modules/robot/robot/interfaces.py b/src/modules/robot/robot/interfaces.py index 685c6d3f..de765453 100644 --- a/src/modules/robot/robot/interfaces.py +++ b/src/modules/robot/robot/interfaces.py @@ -40,7 +40,7 @@ def generate_launch_description(): Node( package="drive", executable="drive", - output="screen", + output={'both': 'log'}, parameters=[params], remappings=remappings, ), From 511332235b3e28cd1b4f176a61ed31fbe9551159 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 8 Feb 2021 21:09:21 +0100 Subject: [PATCH 03/18] Local_costmap contains obstacles for teb_local_planner In fact, the planner takes care of the global_costmap and the teb_local_planner only of the local_costmap. Here, the local_costmap is just representing the static map without inflation and gradient_layer that need a specific treatement. --- src/modules/robot/param/robot.in.yml | 30 ++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/src/modules/robot/param/robot.in.yml b/src/modules/robot/param/robot.in.yml index e42f76cd..99d740d5 100644 --- a/src/modules/robot/param/robot.in.yml +++ b/src/modules/robot/param/robot.in.yml @@ -137,18 +137,36 @@ global_costmap: inflate_around_unknown: false always_send_full_costmap: true -local_costmap: #just enough parameters to disable it +local_costmap: local_costmap: ros__parameters: - update_frequency: 0.1 - publish_frequency: 0.1 + lethal_cost_threshold: 253 + trinary_costmap: false + update_frequency: 5.0 + publish_frequency: 5.0 global_frame: map robot_base_frame: base_link use_sim_time: false - rolling_window: true - width: 6 - height: 6 + rolling_window: false resolution: 1.0 + static_map: true + plugins: ["static_layer", "gradient_layer", "inflation_layer"] + static_layer: + plugin: nav2_costmap_2d::StaticLayer + gradient_layer: #draw radians circle around ennemies + plugin: "gradient_costmap_layer_plugin/GradientLayer" + enabled: false + gradient_size: 7 + gradient_factor: 100 + markers_topic: /ennemies_positions_markers + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + enabled: false + inflation_radius: 0.01 + cost_scaling_factor: 5.0 + inflate_unknown: false + inflate_around_unknown: false + always_send_full_costmap: true map_server: ros__parameters: From 0d18afb50b22c44c1648f326c961f0b0be8790e1 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 8 Feb 2021 21:12:33 +0100 Subject: [PATCH 04/18] Tweak of the global_costmap for an acceptable global_planning --- src/modules/robot/param/robot.in.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/robot/param/robot.in.yml b/src/modules/robot/param/robot.in.yml index 99d740d5..3754d9fc 100644 --- a/src/modules/robot/param/robot.in.yml +++ b/src/modules/robot/param/robot.in.yml @@ -112,12 +112,13 @@ controller_server_rclcpp_node: global_costmap: global_costmap: ros__parameters: + lethal_cost_threshold: 253 update_frequency: 5.0 publish_frequency: 5.0 global_frame: map robot_base_frame: base_link use_sim_time: !Var use_sim_time - resolution: 0.02 + resolution: 0.01 plugins: ["static_layer", "gradient_layer", "inflation_layer"] static_layer: #subscribe to map plugin: "nav2_costmap_2d::StaticLayer" @@ -131,8 +132,8 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" enabled: true - inflation_radius: 0.2 - cost_scaling_factor: 5.0 + inflation_radius: 0.3 + cost_scaling_factor: 10.0 inflate_unknown: false inflate_around_unknown: false always_send_full_costmap: true From 9d3cca75470bbaebd436134426594f6454919116 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 8 Feb 2021 21:20:21 +0100 Subject: [PATCH 05/18] Teb_local_planner tweak: - Decreasing controller_frequency which were way too high, - Made the progress checker more accurate on progress detection, - Raise of the min_obstacle_distance and inflation radius to avoid navigation abort due to trajectory collision with an obstacle, - Include static costmap obstacles. Next step: create a node for computing and sending dynamic obstacles to teb_local_planner. --- src/modules/robot/param/robot.in.yml | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/modules/robot/param/robot.in.yml b/src/modules/robot/param/robot.in.yml index 3754d9fc..3c70b1c5 100644 --- a/src/modules/robot/param/robot.in.yml +++ b/src/modules/robot/param/robot.in.yml @@ -56,33 +56,40 @@ cetautomatix: controller_server: ros__parameters: use_sim_time: !Var use_sim_time - controller_frequency: 40.0 + controller_frequency: 20.0 min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 + min_y_velocity_threshold: 0.001 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" goal_checker_plugin: "goal_checker" controller_plugins: ["DynamicFollowPath"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.5 + required_movement_radius: 0.005 movement_time_allowance: 10.0 goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.02 - yaw_goal_tolerance: 0.02 + xy_goal_tolerance: 0.01 + yaw_goal_tolerance: 0.01 stateful: True DynamicFollowPath: plugin: "teb_local_planner::TebLocalPlannerROS" # Robot footprint footprint_model.type: polygon + # Goal tolerance + xy_goal_tolerance: 0.01 + yaw_goal_tolerance: 0.01 + # Obstacles - min_obstacle_dist: 0.01 - inflation_dist: 0.1 + min_obstacle_dist: 0.03 + inflation_dist: 0.25 costmap_converter_plugin: costmap_converter::CostmapToPolygonsDBSMCCH costmap_converter_spin_thread: True costmap_converter_rate: 5 + include_costmap_obstacles: true + costmap_obstacles_behind_robot_dist: 3.0 + include_dynamic_obstacles: false # Homotopy Class Planner enable_homotopy_class_planning: True @@ -103,6 +110,9 @@ controller_server: global_plan_overwrite_orientation: true allow_init_with_backwards_motion: true + # Miscellaneous + map_frame: "map" + controller_server_rclcpp_node: ros__parameters: From 2fe9b94da2148a814490fbe6d3d7fbac62ab525c Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 8 Feb 2021 21:27:19 +0100 Subject: [PATCH 06/18] Hello teb_obstacles node ! The goal of this node is to compute and send to the teb_local_planner the dynamic obstacles. With theses obstacles, the teb_local_planner might be able to make better decisions. The dynamic obstacles are (for the moment in my mind): - Ennemies - The other allie --- src/navigation/teb_obstacles/package.xml | 15 +++++++++ .../teb_obstacles/resource/teb_obstacles | 0 src/navigation/teb_obstacles/setup.cfg | 4 +++ src/navigation/teb_obstacles/setup.py | 33 +++++++++++++++++++ .../teb_obstacles/teb_obstacles/__init__.py | 0 .../teb_obstacles/teb_obstacles.py | 28 ++++++++++++++++ 6 files changed, 80 insertions(+) create mode 100644 src/navigation/teb_obstacles/package.xml create mode 100644 src/navigation/teb_obstacles/resource/teb_obstacles create mode 100644 src/navigation/teb_obstacles/setup.cfg create mode 100644 src/navigation/teb_obstacles/setup.py create mode 100644 src/navigation/teb_obstacles/teb_obstacles/__init__.py create mode 100644 src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py diff --git a/src/navigation/teb_obstacles/package.xml b/src/navigation/teb_obstacles/package.xml new file mode 100644 index 00000000..f1bf23e6 --- /dev/null +++ b/src/navigation/teb_obstacles/package.xml @@ -0,0 +1,15 @@ + + + + teb_obstacles + 0.8.3 + teb_obstacles node compute and send dynamic teb_obstacles + Philéas LAMBERT + ECAM Makers :: CDFR 2021 + + rclpy + + + ament_python + + diff --git a/src/navigation/teb_obstacles/resource/teb_obstacles b/src/navigation/teb_obstacles/resource/teb_obstacles new file mode 100644 index 00000000..e69de29b diff --git a/src/navigation/teb_obstacles/setup.cfg b/src/navigation/teb_obstacles/setup.cfg new file mode 100644 index 00000000..b166fdb2 --- /dev/null +++ b/src/navigation/teb_obstacles/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/teb_obstacles +[install] +install-scripts=$base/lib/teb_obstacles diff --git a/src/navigation/teb_obstacles/setup.py b/src/navigation/teb_obstacles/setup.py new file mode 100644 index 00000000..bdcc7740 --- /dev/null +++ b/src/navigation/teb_obstacles/setup.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 + + +"""Teb_obstacles package.""" + + +from os import path +from setuptools import setup, find_packages + +package_name = 'teb_obstacles' + +setup( + name=package_name, + version='0.8.3', + packages=find_packages(), + data_files=[ + (path.join("share", package_name), ["package.xml"]), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ], + install_requires=['setuptools'], + zip_safe=True, + author="Philéas LAMBERT", + maintainer='Phileas LAMBERT', + maintainer_email='phileas.lambert@gmail.com', + keywords=["ROS2", "teb_obstacles", "CDFR"], + description='teb_obstacles node compute and send dynamic teb_obstacles', + license="ECAM Makers :: CDFR 2021", + entry_points={ + 'console_scripts': [ + 'teb_obstacles = teb_obstacles.teb_obstacles:main' + ], + }, +) diff --git a/src/navigation/teb_obstacles/teb_obstacles/__init__.py b/src/navigation/teb_obstacles/teb_obstacles/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py new file mode 100644 index 00000000..3c2fe773 --- /dev/null +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 + + +"""Teb_obstacles localisation node.""" + + +import rclpy + +from rclpy.node import Node + +class Teb_obstacles(Node): + + def __init__(self): + super().__init__("teb_dynamic_obstacles_node") + self.allie = "obelix" if self.get_namespace().strip("/") == "asterix" else "asterix" + self.get_logger().info('teb_dynamic_obstacles node is ready') + + +def main(args=None): + """Entrypoint.""" + rclpy.init(args=args) + teb_obstacles = Teb_obstacles() + try: + rclpy.spin(teb_obstacles) + except KeyboardInterrupt: + pass + teb_obstacles.destroy_node() + rclpy.shutdown() From 89b0def61e74f3a99fbd6c4557c4be4185bd8d96 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Mon, 8 Feb 2021 21:35:20 +0100 Subject: [PATCH 07/18] teb_obstales node launching with nav --- src/modules/robot/robot/launcher.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/robot/robot/launcher.py b/src/modules/robot/robot/launcher.py index 8aa83290..667cd745 100644 --- a/src/modules/robot/robot/launcher.py +++ b/src/modules/robot/robot/launcher.py @@ -126,6 +126,13 @@ def generate_robot_launch_description(robot_namespace: str, simulation=False): parameters=[params.name], remappings=remappings, ), + Node( + package='teb_obstacles', + executable='teb_obstacles', + output='screen', + parameters=[], + remappings=remappings + ), IncludeLaunchDescription( PythonLaunchDescriptionSource( [nav2_launch_file_dir, "/navigation_launch.py"] From 17db37ab63bb52d3c68ad034c4fe7bc3d82498fa Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 9 Feb 2021 20:45:36 +0100 Subject: [PATCH 08/18] Subscribing + callback to allies and ennemies topics --- .../teb_obstacles/teb_obstacles/teb_obstacles.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index 3c2fe773..e3218b42 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -7,14 +7,23 @@ import rclpy from rclpy.node import Node +from visualization_msgs.msg import MarkerArray class Teb_obstacles(Node): def __init__(self): super().__init__("teb_dynamic_obstacles_node") self.allie = "obelix" if self.get_namespace().strip("/") == "asterix" else "asterix" + self.allies_subscription_ = self.create_subscription( + MarkerArray, '/allies_positions_markers', self.allies_subscription_callback, 10) + self.ennemies_subscription_ = self.create_subscription( + MarkerArray, '/ennemies_positions_markers', self.ennemies_subscription_callback, 10) + self.allies_subscription_ + self.ennemies_subscription_ self.get_logger().info('teb_dynamic_obstacles node is ready') + def allies_subscription_callback(self, msg): + def ennemies_subscription_callback(self, msg): def main(args=None): """Entrypoint.""" From 46a06cf64d44265415b3cf8b35add4d2223def15 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 9 Feb 2021 20:48:26 +0100 Subject: [PATCH 09/18] Initialisation of obstacles array This ObstacleArrayMsg object is the object to send to the topic 'obstacle' in order to tell the teb_local_planner that theses obstacles are dynamic obstacles --- .../teb_obstacles/teb_obstacles/teb_obstacles.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index e3218b42..b200a1bd 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -20,8 +20,23 @@ def __init__(self): MarkerArray, '/ennemies_positions_markers', self.ennemies_subscription_callback, 10) self.allies_subscription_ self.ennemies_subscription_ + self.initObstaclesArray() self.get_logger().info('teb_dynamic_obstacles node is ready') + def initObstaclesArray(self): + """ObstacleArray index 0: allie, index 1-2: ennemies""" + self.obstacles = ObstacleArrayMsg() + self.obstacles.header.frame_id = "map" + self.obstacles.obstacles.append(ObstacleMsg()) + self.obstacles.obstacles.append(ObstacleMsg()) + self.obstacles.obstacles.append(ObstacleMsg()) + for i in range(3): + self.obstacles.obstacles[i].header.frame_id = "map" + self.obstacles.obstacles[i].polygon.points = [Point32()] + self.obstacles.obstacles[i].polygon.points[0].x = -1.0 + self.obstacles.obstacles[i].polygon.points[0].y = -1.0 + self.obstacles.obstacles[i].radius = 0.15 + self.previous_obstacles = copy.deepcopy(self.obstacles) def allies_subscription_callback(self, msg): def ennemies_subscription_callback(self, msg): From 9bb597eaf6842b323045d6a0923bcbfa65818843 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 9 Feb 2021 20:51:46 +0100 Subject: [PATCH 10/18] dictionnary_index_id: The purpose of this dictionnary is to track which marker id of the obstaclesArray is at which index in order to update the array instead of re-create it in case of new markers detection from assurancetourix --- src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index b200a1bd..3fb5b812 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -20,6 +20,7 @@ def __init__(self): MarkerArray, '/ennemies_positions_markers', self.ennemies_subscription_callback, 10) self.allies_subscription_ self.ennemies_subscription_ + self.dictionary_index_id = {"0":0, "1":0, "2":0} self.initObstaclesArray() self.get_logger().info('teb_dynamic_obstacles node is ready') From bfc2dbc584808b445c91c4db38230c236f5195c7 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 9 Feb 2021 21:00:48 +0100 Subject: [PATCH 11/18] ennemies_subscription_callback function of teb_obstacles This function dissociate the predicted markers (id > 10) from the true positions of the ennemie. At first call, it will just put the marker id in a availible slot of the dictionary. Other callbacks will set the dynamic obstacles --- .../teb_obstacles/teb_obstacles.py | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index 3fb5b812..19c087fa 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -7,7 +7,9 @@ import rclpy from rclpy.node import Node +from costmap_converter_msgs.msg import ObstacleArrayMsg, ObstacleMsg from visualization_msgs.msg import MarkerArray +from geometry_msgs.msg import Point32 class Teb_obstacles(Node): @@ -38,8 +40,29 @@ def initObstaclesArray(self): self.obstacles.obstacles[i].polygon.points[0].y = -1.0 self.obstacles.obstacles[i].radius = 0.15 self.previous_obstacles = copy.deepcopy(self.obstacles) + def set_obstacle(self, index, marker): + + def allies_subscription_callback(self, msg): + def ennemies_subscription_callback(self, msg): + """Identity the ennemie marker in assurancetourix marker_array detection + set the dynamic obstacle for teb_local_planner""" + for ennemie_marker in msg.markers: + if ennemie_marker.id <= 10: + in_dict = False + for index in range(1,2): + if self.dictionary_index_id[f"{index}"] == ennemie_marker.id: + self.set_obstacle(index, ennemie_marker) + in_dict = True + if not in_dict: + if self.dictionary_index_id["1"] == 0: + self.dictionary_index_id["1"] = ennemie_marker.id + elif self.dictionary_index_id["2"] == 0: + self.dictionary_index_id["2"] = ennemie_marker.id + else: + self.get_logger().info('obstacleArray index limit, is there 3 ennemies, or is it a bad marker detection') + def main(args=None): """Entrypoint.""" From f0b4212a257fcee760621856798e24ec0a899e69 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 9 Feb 2021 21:02:21 +0100 Subject: [PATCH 12/18] WIP on allies_subscription_callback for teb_obstacles --- .../teb_obstacles/teb_obstacles/teb_obstacles.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index 19c087fa..d6bc7aa7 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -44,6 +44,14 @@ def set_obstacle(self, index, marker): def allies_subscription_callback(self, msg): + """Identity the allie marker in assurancetourix marker_array detection + set the dynamic obstacle for teb_local_planner""" + for allie_marker in msg.markers: + if allie_marker.text.lower() == self.allie: + if self.dictionary_index_id["0"] == 0: + self.dictionary_index_id["0"] = allie_marker.id + self.obstacles.obstacles[0].id = self.dictionary_index_id["0"] + self.set_obstacle(0, allie_marker) def ennemies_subscription_callback(self, msg): """Identity the ennemie marker in assurancetourix marker_array detection From 99d0577d9c20475bf16470404612b8a9f820d6d2 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 9 Feb 2021 21:04:55 +0100 Subject: [PATCH 13/18] Set_obstacles function for teb_obstacles Set the position of the obstacle + compute its x and y linear velocity for the teb_local_planner in order to predict the movement of the dynamic obstacle --- .../teb_obstacles/teb_obstacles.py | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index d6bc7aa7..6e5f5c6d 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -5,6 +5,7 @@ import rclpy +import copy from rclpy.node import Node from costmap_converter_msgs.msg import ObstacleArrayMsg, ObstacleMsg @@ -40,7 +41,26 @@ def initObstaclesArray(self): self.obstacles.obstacles[i].polygon.points[0].y = -1.0 self.obstacles.obstacles[i].radius = 0.15 self.previous_obstacles = copy.deepcopy(self.obstacles) + + def get_diff_time(self, t1, t2): + return float(t1.sec - t2.sec + (t1.nanosec - t2.nanosec)*1e-9) + def set_obstacle(self, index, marker): + self.previous_obstacles.obstacles[index] = copy.deepcopy(self.obstacles.obstacles[index]) + self.obstacles.obstacles[index].header = marker.header + self.obstacles.obstacles[index].polygon.points[0].x = marker.pose.position.x + self.obstacles.obstacles[index].polygon.points[0].y = marker.pose.position.y + + dt = float(self.get_diff_time(marker.header.stamp, self.previous_obstacles.obstacles[index].header.stamp)) + + if dt != 0.0: + self.obstacles.obstacles[index].velocities.twist.linear.x = ( + marker.pose.position.x - self.previous_obstacles.obstacles[index].polygon.points[0].x + ) / dt + + self.obstacles.obstacles[index].velocities.twist.linear.y = ( + marker.pose.position.y - self.previous_obstacles.obstacles[index].polygon.points[0].y + ) / dt def allies_subscription_callback(self, msg): From 35518029089a4a0a969cef279c2b4fb7940ed781 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Tue, 9 Feb 2021 21:08:06 +0100 Subject: [PATCH 14/18] Publishing dynamic obstacles each .5s + including them in teb config --- src/modules/robot/param/robot.in.yml | 2 +- .../teb_obstacles/teb_obstacles/teb_obstacles.py | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/robot/param/robot.in.yml b/src/modules/robot/param/robot.in.yml index 3c70b1c5..aa4a14a0 100644 --- a/src/modules/robot/param/robot.in.yml +++ b/src/modules/robot/param/robot.in.yml @@ -89,7 +89,7 @@ controller_server: costmap_converter_rate: 5 include_costmap_obstacles: true costmap_obstacles_behind_robot_dist: 3.0 - include_dynamic_obstacles: false + include_dynamic_obstacles: true # Homotopy Class Planner enable_homotopy_class_planning: True diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index 6e5f5c6d..afee8920 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -23,8 +23,14 @@ def __init__(self): MarkerArray, '/ennemies_positions_markers', self.ennemies_subscription_callback, 10) self.allies_subscription_ self.ennemies_subscription_ + + self.obstacles_publisher_ = self.create_publisher(ObstacleArrayMsg, 'obstacles', 10) + self.dictionary_index_id = {"0":0, "1":0, "2":0} self.initObstaclesArray() + + self.create_timer(0.5, self.send_obstacles) + self.get_logger().info('teb_dynamic_obstacles node is ready') def initObstaclesArray(self): @@ -91,6 +97,8 @@ def ennemies_subscription_callback(self, msg): else: self.get_logger().info('obstacleArray index limit, is there 3 ennemies, or is it a bad marker detection') + def send_obstacles(self): + self.obstacles_publisher_.publish(self.obstacles) def main(args=None): """Entrypoint.""" From a06bdc96296abb7a1d1501de5e4f83a7be77154f Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 10 Feb 2021 23:41:04 +0100 Subject: [PATCH 15/18] /robot/get_odom_map_tf service that returns the initial map->odom tf --- .../localisation/localisation/localisation_node.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/localisation/localisation/localisation_node.py b/src/modules/localisation/localisation/localisation_node.py index 2b4a2cff..0e9714b5 100644 --- a/src/modules/localisation/localisation/localisation_node.py +++ b/src/modules/localisation/localisation/localisation_node.py @@ -12,6 +12,7 @@ from rcl_interfaces.msg import SetParametersResult from visualization_msgs.msg import MarkerArray from tf2_ros import StaticTransformBroadcaster +from transformix_msgs.srv import TransformixParametersTransformStamped class Localisation(rclpy.node.Node): """Robot localisation node.""" @@ -27,6 +28,8 @@ def __init__(self): self._tf.header.frame_id = 'map' self._tf.child_frame_id = 'odom' self.update_transform() + self.get_initial_tf_srv = self.create_service(TransformixParametersTransformStamped, + 'get_odom_map_tf', self.get_initial_tf_srv_callback) self.subscription_ = self.create_subscription( MarkerArray, '/allies_positions_markers', self.allies_subscription_callback, 10) self.subscription_ # prevent unused variable warning @@ -107,6 +110,7 @@ def update_transform(self): self._tf.transform.rotation.y = float(qy) self._tf.transform.rotation.z = float(qz) self._tf.transform.rotation.w = float(qw) + self._initial_tf = self._tf self._tf_brodcaster.sendTransform(self._tf) def allies_subscription_callback(self, msg): @@ -126,6 +130,11 @@ def allies_subscription_callback(self, msg): self.tf_publisher_.publish(self._tf) self.last_odom_update = self.get_clock().now().to_msg().sec + def get_initial_tf_srv_callback(self, request, response): + self.get_logger().info(f"incoming request for {self.robot} odom -> map tf") + response.transform_stamped = self._initial_tf + return response + def main(args=None): """Entrypoint.""" rclpy.init(args=args) From 991785ba58c0109a48a6a0d5cda945f73adf9727 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 10 Feb 2021 23:43:46 +0100 Subject: [PATCH 16/18] Client of /robot/get_odom_map_tf for future pose transform --- .../teb_obstacles/teb_obstacles.py | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index afee8920..5faf5299 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -11,12 +11,19 @@ from costmap_converter_msgs.msg import ObstacleArrayMsg, ObstacleMsg from visualization_msgs.msg import MarkerArray from geometry_msgs.msg import Point32 +from platform import machine +from transformix_msgs.srv import TransformixParametersTransformStamped class Teb_obstacles(Node): def __init__(self): super().__init__("teb_dynamic_obstacles_node") + self.simulation = True if machine() != "aarch64" else False + self.allie = "obelix" if self.get_namespace().strip("/") == "asterix" else "asterix" + + self.get_allie_odom_transformation() + self.allies_subscription_ = self.create_subscription( MarkerArray, '/allies_positions_markers', self.allies_subscription_callback, 10) self.ennemies_subscription_ = self.create_subscription( @@ -33,6 +40,25 @@ def __init__(self): self.get_logger().info('teb_dynamic_obstacles node is ready') + def get_allie_odom_transformation(self): + if self.simulation: + return + + get_tf_client = self.create_client(TransformixParametersTransformStamped, f'/{self.allie}/get_odom_map_tf') + + if not get_tf_client.wait_for_service(timeout_sec=15.0): + self.get_logger().info(f'No service /{self.allie}/get_odom_map_tf availible, is there ony one robot?') + return + get_tf_request = TransformixParametersTransformStamped.Request() + future = get_tf_client.call_async(get_tf_request) + rclpy.spin_until_future_complete(self, future) + try: + response = future.result() + except Exception as e: + self.get_logger().info( + 'Service call failed %r' % (e,)) + else: + self.odom_map_tf = response.transform_stamped def initObstaclesArray(self): """ObstacleArray index 0: allie, index 1-2: ennemies""" self.obstacles = ObstacleArrayMsg() From dcca6928240df9cf1d3e5d7a62f22a4113054a45 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 10 Feb 2021 23:49:22 +0100 Subject: [PATCH 17/18] Different allie tracking strategy for dynamic obstacle By subscribing to the odometry of the allie, we can get the position of the allie relative to its odom frame. With the odom->map transform, we can have these coordinates in the map frame. Then we can set the allie dynamic obstacle by the same way as for ennemies. Cooldown of 0.3s between each update of allie dynamic obstacle --- .../teb_obstacles/teb_obstacles.py | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index 5faf5299..d98bb576 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -9,7 +9,8 @@ from rclpy.node import Node from costmap_converter_msgs.msg import ObstacleArrayMsg, ObstacleMsg -from visualization_msgs.msg import MarkerArray +from visualization_msgs.msg import MarkerArray, Marker +from nav_msgs.msg import Odometry from geometry_msgs.msg import Point32 from platform import machine from transformix_msgs.srv import TransformixParametersTransformStamped @@ -25,7 +26,7 @@ def __init__(self): self.get_allie_odom_transformation() self.allies_subscription_ = self.create_subscription( - MarkerArray, '/allies_positions_markers', self.allies_subscription_callback, 10) + Odometry, f'/{self.allie}/odom', self.allies_subscription_callback, 10) self.ennemies_subscription_ = self.create_subscription( MarkerArray, '/ennemies_positions_markers', self.ennemies_subscription_callback, 10) self.allies_subscription_ @@ -34,6 +35,9 @@ def __init__(self): self.obstacles_publisher_ = self.create_publisher(ObstacleArrayMsg, 'obstacles', 10) self.dictionary_index_id = {"0":0, "1":0, "2":0} + + self.last_time_allie_callback = self.get_clock().now().to_msg() + self.initObstaclesArray() self.create_timer(0.5, self.send_obstacles) @@ -96,14 +100,17 @@ def set_obstacle(self, index, marker): def allies_subscription_callback(self, msg): - """Identity the allie marker in assurancetourix marker_array detection + """Determine the pose of base_link in map set the dynamic obstacle for teb_local_planner""" - for allie_marker in msg.markers: - if allie_marker.text.lower() == self.allie: - if self.dictionary_index_id["0"] == 0: - self.dictionary_index_id["0"] = allie_marker.id - self.obstacles.obstacles[0].id = self.dictionary_index_id["0"] - self.set_obstacle(0, allie_marker) + if self.get_diff_time(self.get_clock().now().to_msg(), self.last_time_allie_callback) > 0.3: + pose = msg.pose.pose + x = pose.position.x + self.odom_map_tf.transform.translation.x + y = pose.position.y + self.odom_map_tf.transform.translation.y + tmp_marker = Marker() + tmp_marker.pose.position.x = x + tmp_marker.pose.position.y = y + self.set_obstacle(0, tmp_marker) + self.last_time_allie_callback = self.get_clock().now().to_msg() def ennemies_subscription_callback(self, msg): """Identity the ennemie marker in assurancetourix marker_array detection From fbd32156319f9cdbbea03e2c9a6b344c71f89b6d Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 10 Feb 2021 23:51:05 +0100 Subject: [PATCH 18/18] removing useless remapping + commentaries on teb_obstacles functions --- src/modules/robot/robot/launcher.py | 3 +-- src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py | 3 +++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/robot/robot/launcher.py b/src/modules/robot/robot/launcher.py index 667cd745..174b5fab 100644 --- a/src/modules/robot/robot/launcher.py +++ b/src/modules/robot/robot/launcher.py @@ -130,8 +130,7 @@ def generate_robot_launch_description(robot_namespace: str, simulation=False): package='teb_obstacles', executable='teb_obstacles', output='screen', - parameters=[], - remappings=remappings + parameters=[] ), IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py index d98bb576..3e9cec52 100644 --- a/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py +++ b/src/navigation/teb_obstacles/teb_obstacles/teb_obstacles.py @@ -79,9 +79,12 @@ def initObstaclesArray(self): self.previous_obstacles = copy.deepcopy(self.obstacles) def get_diff_time(self, t1, t2): + """Returns the nb of seconds between the two Time object""" return float(t1.sec - t2.sec + (t1.nanosec - t2.nanosec)*1e-9) def set_obstacle(self, index, marker): + """Set the marker as obstacle in ObstacleArrayMsg at the given index, + compute the linear velocities relative to the previous state""" self.previous_obstacles.obstacles[index] = copy.deepcopy(self.obstacles.obstacles[index]) self.obstacles.obstacles[index].header = marker.header self.obstacles.obstacles[index].polygon.points[0].x = marker.pose.position.x