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