Merge pull request #31 from robotique-ecam/odometry_readjustement

Odometry readjustement

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/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):
@@ -28,13 +29,17 @@ 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
         self.tf_publisher_ = self.create_publisher(
             TransformStamped, "adjust_odometry", 10
         )
@@ -114,6 +119,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):
@@ -136,6 +142,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."""

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,41 @@ 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 +111,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 +123,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 +143,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/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,31 @@
+#!/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"],
+    },
+)