Add filtering by topic name to log subscriber.

Some other changes, gitignore, added poses to rviz config

Signed-off-by: Michał Pełka <[email protected]>

kraken_nav/.gitignore

@@ -0,0 +1,2 @@
+.vscode
+__pycache__

kraken_nav/o3de_kraken_nav/launch/config/config_multi.rviz

@@ -567,6 +567,86 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /apple_kraken_shiny_4/plan
       Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_1/goal_pose
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_2/goal_pose
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_3/goal_pose
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_4/goal_pose
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/global_kraken_orchestration.py

@@ -14,6 +14,7 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
 
         GlobalOrchestrationSM.__init__(self)
         KrakenOrchestrationNode.__init__(self)
+        self.logger = KrakenOrchestrationNode.get_logger(self)
 
         spin_thread = Thread(target=rclpy.spin, args=(self,))
         spin_thread.start()
@@ -23,10 +24,11 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
 
     def get_next_pose(self):
         if self.current_index >= len(self.poses):
-            print('Invalid index!')
+            self.logger.error('Invalid index!')
             return PoseStamped()
         
         pose = self.poses[self.current_index]
+        self.logger.info("Current goal id is %d " % self.current_index)
         self.current_index += 1
         return pose
 
@@ -35,7 +37,7 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
         rclpy.shutdown()
 
     def on_enter_waiting(self):
-        print('Has the simulation started?')
+        self.logger.info('Has the simulation started?')
         self.send_state("Wait for connection")
         # Perform a check whether the simulation has started or not.
         if (self.sim_started):
@@ -45,23 +47,23 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
             self.await_start()
 
     def on_enter_spawning(self):
-        print('Spawning the robot.')
+        self.logger.info('Spawning the robot.')
         self.send_state("Spawning the robot")
         if self.spawn_kraken():
             self.spawned()
         else:
-            print('Robot spawning failed.')
+            self.logger.info('Robot spawning failed.')
 
     def on_spawned(self):
-        print('Robot spawned successfully. Fetching info.')
+        self.logger.info('Robot spawned successfully. Fetching info.')
         pose = self.get_spawn_point_pose()
         self.poses = self.get_plan_poses(pose)
         # Give nav stack a few seconds before we start first goal point
-        print('Waiting for robot sensors to latch...')
+        self.logger.info('Waiting for robot sensors to latch...')
         time.sleep(3)
         
     def on_enter_gathering(self):
-        print('Is the robot in a gathering position?')
+        self.logger.info('Is the robot in a gathering position?')
         # Perform a check whether the robot is in a gathering position or not.
         if (self.can_gather):
             self.gather()
@@ -69,13 +71,13 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
             self.navigate()
 
     def on_gather(self):
-        print('Gathering the apples.')
+        self.logger.info('Gathering the apples.')
         # Instruct the robot to start gathering apples.
         self.has_finished = not self.has_finished
         pass
 
     def on_enter_finishing(self):
-        print('Has the robot completed its job?')
+        self.logger.info('Has the robot completed its job?')
         # Perform a check whether the robot has finished its job or not.
         if (self.current_index >= len(self.poses)):
             self.finished()
@@ -84,7 +86,7 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
 
     def on_navigate(self):
         self.send_state("Navigating")
-        print('Navigating the robot to a closest gathering position?')
+        self.logger.info('Navigating the robot to a closest gathering position?')
         self.can_gather = False
         # Instruct the robot to navigate to the closest gathering position.
         self.navigate_to_pose(self.get_next_pose())
@@ -96,21 +98,21 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
         self.can_gather = True
 
     def on_gather(self):
-        print("Requesting gathering...")
+        self.logger.info("Requesting gathering...")
         self.send_state("Gather %.1f "% self.apple_progress)
         trigger_res = self.trigger_apple_gathering()
-        print(trigger_res.message)
+        self.logger.info(trigger_res.message)
 
         if trigger_res.success:
             while not self.gathering_done:
                 self.send_state("Gather %.0f %%"% self.apple_progress)
                 time.sleep(0.2)
             
-            print("Done gathering apples.")
+            self.logger.info("Done gathering apples.")
         else:
-            print("Freeing up the AppleKraken...")
+            self.logger.info("Freeing up the AppleKraken...")
             cancel_res = self.cancel_apple_gathering()
-            print(cancel_res.message)
+            self.logger.info(cancel_res.message)
             time.sleep(2)
 
     def on_finish(self):
@@ -119,7 +121,7 @@ class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
 
     def on_finished(self):
         self.send_state("Completed")
-        print('The robot has completed its job.')
+        self.logger.info('The robot has completed its job.')
         # The robot has finished its job.
         pass
 

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/orchestration_node.py

@@ -29,7 +29,7 @@ class KrakenOrchestrationNode(Node):
             self.get_logger().info('Get plan service not available waiting again...')
 
     def apple_gathering_done_callback(self, request, response):
-        print("Received done client msg.")
+        self.get_logger().info("Received done client msg.")
         self.gathering_done = True
         return Empty.Response()
 
@@ -101,12 +101,10 @@ class KrakenOrchestrationNode(Node):
         self.init_apple_subscriptions()
 
     def log_cb(self, msg):
-        if msg.msg == "Goal succeeded" and self.kraken_name in str(self.kraken_name):
-            print("Navigation point reached.")
+        if msg.msg == "Goal succeeded" and self.kraken_name in str(msg.name):
+            self.get_logger().info("Navigation point reached.")
             self.goal_pose_reached = True
 
-
-
     def spawn_kraken(self):
         req = SpawnEntity.Request()
         req.name = self.kraken_name[:len(self.kraken_name)-2]