fixing review comments

ros-navigation · Aug 12, 2024 · 783af26 · 783af26
1 parent 267bd58
commit 783af26
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 8 deletions.
diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py
@@ -110,6 +110,7 @@ def initialPoseCallback(self, msg):
             self.initial_pose = msg.pose.pose
             self.t_map_to_odom.transform.translation.x = self.initial_pose.position.x
             self.t_map_to_odom.transform.translation.y = self.initial_pose.position.y
+            self.t_map_to_odom.transform.translation.z = 0.0
             self.t_map_to_odom.transform.rotation = self.initial_pose.orientation
             self.t_odom_to_base_link.transform.translation = Vector3()
             self.t_odom_to_base_link.transform.rotation = Quaternion()
@@ -131,7 +132,7 @@ def initialPoseCallback(self, msg):
         t_map_to_base_link.child_frame_id = 'base_link'
         t_map_to_base_link.transform.translation.x = self.initial_pose.position.x
         t_map_to_base_link.transform.translation.y = self.initial_pose.position.y
-        t_map_to_base_link.transform.translation.z = self.initial_pose.position.z
+        t_map_to_base_link.transform.translation.z = 0.0
         t_map_to_base_link.transform.rotation = self.initial_pose.orientation
         mat_map_to_base_link = transformStampedToMatrix(t_map_to_base_link)
         mat_odom_to_base_link = transformStampedToMatrix(self.t_odom_to_base_link)

diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py
@@ -25,14 +25,13 @@
 
 def addYawToQuat(quaternion, yaw_to_add):
     q = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
-    _, _, yaw = tf_transformations.euler_from_quaternion(q)
-    new_yaw = yaw + yaw_to_add
-    new_quaternion_array = tf_transformations.quaternion_from_euler(0.0, 0.0, new_yaw)
+    q2 = tf_transformations.quaternion_from_euler(0.0, 0.0, yaw_to_add)
+    q_new = tf_transformations.quaternion_multiply(q, q2)
     new_quaternion = Quaternion()
-    new_quaternion.x = new_quaternion_array[0]
-    new_quaternion.y = new_quaternion_array[1]
-    new_quaternion.z = new_quaternion_array[2]
-    new_quaternion.w = new_quaternion_array[3]
+    new_quaternion.x = q_new[0]
+    new_quaternion.y = q_new[1]
+    new_quaternion.z = q_new[2]
+    new_quaternion.w = q_new[3]
     return new_quaternion