no local ekf => use odom offset

EPFLXplore · Sep 5, 2024 · 85046a8 · 85046a8
1 parent def2739
commit 85046a8
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 10 deletions.
diff --git a/navigation/path_planning/config/nav2_params_real.yaml b/navigation/path_planning/config/nav2_params_real.yaml
@@ -140,7 +140,7 @@ bt_navigator:
     use_sim_time: False
     global_frame: map
     robot_base_frame: base_link
-    odom_topic: /odometry/filtered
+    odom_topic: /odom_with_offset
     bt_loop_duration: 10
     default_server_timeout: 20
     enable_groot_monitoring: True
@@ -233,7 +233,7 @@ controller_server:
     ## Server parameters
     use_sim_time: False
     controller_frequency: 1.0
-    odom_topic: "odometry/filtered"
+    odom_topic: "odom_with_offset"
     # Min velocities to ignore
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5

diff --git a/navigation/path_planning/launch/nav2_real.launch.py b/navigation/path_planning/launch/nav2_real.launch.py
@@ -80,10 +80,10 @@ def launch_setup(context: launch.LaunchContext, *args, **kwargs):
 
     return [
         # Commands
-        # start_nav2_cmd,
+        start_nav2_cmd,
         # start_wheels_control_cmd,
         # Nodes
-        local_robot_localization_node,
+        # local_robot_localization_node,
         global_robot_localization_node,
         odom_offset_node
     ]

diff --git a/navigation/path_planning/path_planning/odom_offset_node.py b/navigation/path_planning/path_planning/odom_offset_node.py
@@ -1,6 +1,7 @@
 #!/usr/bin/python3
 import rclpy
 from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy
 from nav_msgs.msg import Odometry
 import numpy as np
 # import tf_transformations
@@ -26,7 +27,7 @@ def __init__(self):
         #     self.offset_orientation = self.get_parameter('orientation_offset').get_parameter_value().double_array_value
 
         self.odom_subscriber = self.create_subscription(
-            Odometry, "/lio_sam/mapping/odometry", self.odom_callback, 10
+            Odometry, "/lio_sam/mapping/odometry", self.odom_callback, QoSProfile(depth=10, reliability = ReliabilityPolicy.BEST_EFFORT)
         )
         self.odom_publisher = self.create_publisher(Odometry, "/odom_with_offset", 10)
 
@@ -40,18 +41,18 @@ def load_offsets_from_config(self, config_file_path):
         return position_offset, orientation_offset
 
     def odom_callback(self, msg):
-        msg.header.frame_id = "odom"
-        msg.pose.pose.position.x += self.offset_position[0]
-        msg.pose.pose.position.y += self.offset_position[1]
-        msg.pose.pose.position.z += self.offset_position[2]
+        # msg.header.frame_id = "odom"
+        # msg.pose.pose.position.x += 0.0
+        # msg.pose.pose.position.y += 
+        # msg.pose.pose.position.z += self.offset_position[2]
 
         current_orientation = [
             msg.pose.pose.orientation.x,
             msg.pose.pose.orientation.y,
             msg.pose.pose.orientation.z,
             msg.pose.pose.orientation.w,
         ]
-        offset_quaternion = self.offset_orientation
+        # offset_quaternion = self.offset_orientation
 
         # new_orientation = tf_transformations.quaternion_multiply(
         #     current_orientation, offset_quaternion