Teb tweaking

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");
 }
 

src/modules/drive/include/drive.hpp

@@ -10,7 +10,6 @@
 #include <webots/PositionSensor.hpp>
 #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"

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)

src/modules/robot/behavior_trees/navigate_w_replanning_time.xml

@@ -5,7 +5,7 @@
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <PipelineSequence name="NavigateWithReplanning">
-      <RateController hz="1.0">
+      <RateController hz="2.0">
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
       </RateController>
       <FollowPath path="{path}"  controller_id="DynamicFollowPath"/>

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: true
 
       # 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:
@@ -112,12 +122,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,24 +142,42 @@ 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
 
-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:

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,
                     ),

src/modules/robot/robot/launcher.py

@@ -126,6 +126,12 @@ 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=[]
+                    ),
                     IncludeLaunchDescription(
                         PythonLaunchDescriptionSource(
                             [nav2_launch_file_dir, "/navigation_launch.py"]

src/navigation/teb_obstacles/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>teb_obstacles</name>
+  <version>0.8.3</version>
+  <description>teb_obstacles node compute and send dynamic teb_obstacles</description>
+  <maintainer email="phileas.lambert@gmail.com">Philéas LAMBERT</maintainer>
+  <license>ECAM Makers :: CDFR 2021</license>
+
+  <exec_depend>rclpy</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/navigation/teb_obstacles/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/teb_obstacles
+[install]
+install-scripts=$base/lib/teb_obstacles

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'
+        ],
+    },
+)