Update manual_topomapping.py with rotation

LCAS · Jul 19, 2024 · 8b1c001 · 8b1c001
1 parent f59c07f
commit 8b1c001
Showing 1 changed file with 13 additions and 3 deletions.
diff --git a/topological_navigation/topological_navigation/scripts/manual_topomapping.py b/topological_navigation/topological_navigation/scripts/manual_topomapping.py
@@ -24,7 +24,7 @@
 from nav_msgs.msg import Odometry
 from geometry_msgs.msg import Pose
 
-from sensor_msgs.msg import Joy
+from sensor_msgs.msg import Joy, Imu
 from std_srvs.srv import Trigger
 from visualization_msgs.msg import Marker, MarkerArray
 
@@ -55,8 +55,9 @@ def __init__(self):
         self.declare_parameter( 'remove_btn'      , Parameter.Type.INTEGER )  #remove_node_button_index
         self.declare_parameter( 'gen_map_btn'     , Parameter.Type.INTEGER )  #generate_tmap_button_index 
         # Topics 
-        self.declare_parameter( 'topic_joy'       , Parameter.Type.STRING  )  #remove_node_button_index
-        self.declare_parameter( 'topic_pose'      , Parameter.Type.STRING  )  #generate_tmap_button_index 
+        self.declare_parameter( 'topic_joy'       , Parameter.Type.STRING  )  
+        self.declare_parameter( 'topic_pose'      , Parameter.Type.STRING  )   
+        self.declare_parameter( 'topic_imu'       , Parameter.Type.STRING  )   
 
 
         self.pointset        = self.get_parameter_or('tmap'            , Parameter('str'   , Parameter.Type.STRING, '')  ).value
@@ -73,6 +74,7 @@ def __init__(self):
         self.gen_map_btn     = self.get_parameter_or('gen_map_btn'     , Parameter('int'   , Parameter.Type.INTEGER, 3)  ).value
         self.topic_joy       = self.get_parameter_or('topic_joy'       , Parameter('str'   , Parameter.Type.STRING, '/joy')  ).value
         self.topic_pose      = self.get_parameter_or('topic_pose'      , Parameter('str'   , Parameter.Type.STRING, '/gps_base/odometry')  ).value
+        self.topic_imu       = self.get_parameter_or('topic_imu'       , Parameter('str'   , Parameter.Type.STRING, '/gps_base/yaw')  ).value
 
 
 
@@ -96,6 +98,7 @@ def __init__(self):
 
         # Subscribers
         self.create_subscription(Joy     , self.topic_joy , self.joy_cb       , 10)
+        self.create_subscription(Imu     , self.topic_pose, self.robot_imu_cb , 10)
         self.create_subscription(Odometry, self.topic_pose, self.robot_pose_cb, 10)
 
         # Publishers
@@ -109,6 +112,8 @@ def __init__(self):
         # Load existing nodes from tmap_dir if there are any
         self.get_tmap_nodes()
 
+        self.robot_imu_msg = None
+
 
     def load_yaml(self, filename):
         with open(filename, 'r') as f:
@@ -149,6 +154,11 @@ def get_tmap_nodes(self):
 
     def robot_pose_cb(self, msg):
         self.robot_pose_msg = msg.pose.pose
+        #Please note, this is an ugly hack, the better way to do it is via topics sync 
+        self.robot_pose_msg.orientation = self.robot_imu_msg
+
+    def robot_imu_cb(self, msg):
+        self.robot_imu_msg = msg.orientation
 
 
     def joy_cb(self, msg):