From 803b68d4efb5608aa2bb1c1189181db53a36c663 Mon Sep 17 00:00:00 2001 From: PhileasL Date: Wed, 10 Feb 2021 23:49:22 +0100 Subject: [PATCH] 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