APPARENTLY CANT CALL NAVIGATOR IN MULTITHREAD :)

src/rove_navigation/launch/mule_behaviour.launch.py

@@ -0,0 +1,33 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, GroupAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.actions import Node
+from launch_ros.actions import SetRemap
+
+def generate_launch_description():
+
+    mule_behavior = Node(
+            package="rove_navigation",
+            executable='mule_behavior',
+            name='mule_behavior',
+            output='screen',
+        )
+
+    person_following = Node(
+            package="rove_navigation",
+            executable='person_following',
+            name='navigate_to_person',
+            output='screen',
+        )
+
+    return LaunchDescription([
+        #person_following_node,
+        mule_behavior,
+        person_following
+    ])

src/rove_navigation/rove_navigation/behavior/mule_constants.py

@@ -0,0 +1,13 @@
+# states
+START = 'START' # At the beginning, exiting this state sets the startpoint
+FOLLOWING = 'FOLLOWING' # make person_following behaviour active
+REWIND = 'REWIND' # Initiates the rewind phase
+PROCESSED_REWIND = 'PROCESSED_REWIND' # Continues the rewind phase until another TPOSE is detected
+PAUSE = 'PAUSE' # Freezes the robot in place (TODO: nice to have, make it still oriente itself towards the target, but not move)
+END = 'END'
+
+# poses
+FIGURE_IDLE = "IDLE" # Continue state
+FIGURE_TPOSE = "TPOSE" # Start the follow
+FIGURE_BUCKET = "BUCKET" # Pause/UnPause
+FIGURE_UP = "UP" # start the rewind phase

src/rove_navigation/rove_navigation/lost_connection.py

@@ -87,7 +87,7 @@ def check_connection(self):
     # gets called on a ros timer (main thread)
     def fallback_behavior(self):
         if self.state==self.OFFLINE and self.poses_remaining > 0:
-            self.get_logger().info('nb poses remaingin: '+ str(self.poses_remaining))
+            self.get_logger().info('nb poses remaining: '+ str(self.poses_remaining))
             self.navigate_poses()              
 
         # self.get_logger().warn(f'Navigating to fallback at ({self.fallback_path[self.fallbackIndex].__str__()})')

src/rove_navigation/rove_navigation/mule_behavior.py

@@ -0,0 +1,147 @@
+import time
+import socket
+from threading import Thread
+
+from sympy import Point
+import rclpy
+from rclpy.duration import Duration
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+import numpy as np
+from std_msgs.msg import String, Header
+from nav_msgs.msg import Odometry, Path
+from nav2_msgs.action import NavigateThroughPoses
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+import rove_navigation.behavior.mule_constants as consts
+
+# Node to handle when the robot loses connection with internet, stores waypoints to fall back to to try to get back connection
+class MuleBehavior(Node):
+    def __init__(self):        
+        super().__init__('mule_behavior')
+        self.subscription_person_position = self.create_subscription(
+            String,
+            '/tracking/state',
+            self.state_listener,
+            10)
+        self.pub_mule_state = self.create_publisher(
+            String,
+            '/mule_state',
+            10)
+        self.odom_subscriber = self.create_subscription(
+            Odometry,
+            '/odometry/local',
+            self.odom_listener,
+            10)
+
+        # nav variables
+        self.startPoint: PoseStamped
+        self.endpoint: PoseStamped
+        self.goal_point: PoseStamped
+        self.currentPos: PoseStamped
+        self.navigator = BasicNavigator()
+
+
+        self.state = ''
+        self.change_state(consts.START)
+        self.navigate_to_goal_point() # returns only when goal reached/cancelled/failed
+
+        self.get_logger().info('Initiating MuleBehavior node')
+
+        # Thread to execute goal pose posting for rewind
+        self.connection_thread = Thread(target=self.rewind_behavior)
+        self.connection_thread.start()
+
+    def change_state(self, state):
+        if(state != self.state):
+            self.state = state
+            self.pub_mule_state.publish(String(data=state))
+
+    # Subscribed to /odometry/local
+    def odom_listener(self, msg:Odometry):
+        self.currentPos = PoseStamped()
+        self.currentPos.header.frame_id = msg.header.frame_id
+        self.currentPos.header.stamp = msg.header.stamp
+        self.currentPos.pose = msg.pose.pose
+
+    # Subscribed to /tracking/state, handles state/figure changes
+    def state_listener(self, msg:String):
+        if self.state != msg.data:
+            returnState = msg.data
+            match msg.data:
+                case consts.FIGURE_TPOSE:
+                    if self.state == consts.START:
+                        self.startPoint = self.currentPos
+                        self.navigator.setInitialPose(self.startPoint)
+                        self.get_logger().info(f'Mule behavior engaged, point A noted at coordonates {self.startPoint.pose.position}')
+                    returnState = consts.FOLLOWING                     
+
+                case consts.FIGURE_IDLE:
+                    if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND:
+                        returnState=self.state
+
+                case consts.FIGURE_UP:
+                    if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND:
+                        if self.state == consts.PAUSE: returnState = consts.FOLLOWING
+                        else: returnState = consts.PAUSE
+
+                case consts.FIGURE_BUCKET:
+                    if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND:
+                        self.endpoint = self.currentPos
+                        returnState = consts.REWIND
+
+                case _:
+                    self.get_logger().error(f'Unimplemented pose returned: {msg.data}')     
+
+            self.change_state(returnState)
+
+    # runs on a thread continuously
+    def rewind_behavior(self):
+        while self.state != consts.END:
+            if self.state == consts.REWIND:
+                self.get_logger().info(f'Rewind phase engaged, point B noted at coordonates {self.endpoint.pose.position}')
+                self.change_state(consts.PROCESSED_REWIND)
+                self.goal_point = self.startPoint
+            while self.state == consts.PROCESSED_REWIND:
+                self.navigate_to_goal_point() # returns only when goal reached/cancelled/failed
+                result = self.navigator.getResult()
+                goal = 'A'
+                nextgoal = 'B'
+                if self.goal_point == self.startPoint:
+                    goal = 'B'
+                    nextgoal = 'A'
+                if result == TaskResult.SUCCEEDED:
+                    self.get_logger().info(f'Goal {goal} reached! Initiating nav towards point {nextgoal}')
+                    self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint
+                elif result == TaskResult.FAILED:
+                    self.get_logger().error(f'Failed to reach {goal}! Initiating nav towards point {nextgoal}')
+                    self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint
+                elif result == TaskResult.CANCELED:
+                    self.get_logger().error(f'Rewinding to point {goal} was cancelled! Initiating nav towards point {nextgoal}')
+                else :
+                    self.get_logger().warn(f'nav2 navigator task returned with unknown TaskResult')
+
+    # Tells navigator to go to current goal Point and processes feedback
+    def navigate_to_goal_point(self):
+        i = 0
+        self.navigator.goToPose(self.goal_point)
+        while not self.navigator.isTaskComplete():
+            if self.state != consts.PROCESSED_REWIND:
+                self.navigator.cancelTask()
+
+            feedback:NavigateThroughPoses.Feedback = self.navigator.getFeedback()
+            self.poses_remaining = feedback.number_of_poses_remaining
+            if feedback and i % 5 == 0:
+                # navigation timeout
+                if Duration.from_msg(feedback.navigation_time) > 1.5 * Duration(seconds=feedback.estimated_time_remaining):
+                    self.navigator.cancelTask()
+            i+=1
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MuleBehavior()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rove_navigation/rove_navigation/person_following.py

@@ -3,7 +3,9 @@
 from geometry_msgs.msg import PointStamped, PoseStamped
 from tf2_ros import Buffer, TransformListener
 from tf2_geometry_msgs import PointStamped
+from std_msgs.msg import String
 import math
+import rove_navigation.behavior.mule_constants as consts
 
 class NavigateToPersonNode(Node):
     def __init__(self, truncate_distance):
@@ -18,37 +20,46 @@ def __init__(self, truncate_distance):
             PoseStamped,
             '/goal_pose',
             10)
-
+        self.pub_mule_state = self.create_subscription(
+            String,
+            '/mule_state',
+            self.mule_state_listener,
+            10)
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
+        self.curr_phase = consts.START
 
         # Distance to truncate from the target position
         self.truncate_distance = truncate_distance
+
+    def mule_state_listener(self, msg: String):
+        self.curr_phase = msg
 
     def navigate_to_person(self, msg):
-        # Ensure that the transformation is available
-        now = rclpy.time.Time()
-        if self.tf_buffer.can_transform('map', msg.header.frame_id, now):
-            point_transformed = self.tf_buffer.transform(msg, 'map', timeout=rclpy.duration.Duration(seconds=1))
-            goal_pose = PoseStamped()
-            goal_pose.header.frame_id = "map"
-            goal_pose.header.stamp = self.get_clock().now().to_msg()
-
-            # Calculate the angle for the truncated goal using atan2
-            angle = math.atan2(point_transformed.point.y, point_transformed.point.x)
-
-            # Calculate the position truncating the specified distance from the transformed position
-            goal_pose.pose.position.x = point_transformed.point.x - self.truncate_distance * math.cos(angle)
-            goal_pose.pose.position.y = point_transformed.point.y - self.truncate_distance * math.sin(angle)
-            goal_pose.pose.position.z = 0.0  # Normally zero for ground robots
-
-            goal_pose.pose.orientation.w = 1.0  # No rotation about the z-axis
-
-            # Log the navigation target for debugging
-            self.get_logger().info(f'Navigating to truncated goal: {goal_pose.pose.position.x}, {goal_pose.pose.position.y}')
-
-            # Publish the goal
-            self.goal_update_pub.publish(goal_pose)
+        if self.curr_phase == consts.FOLLOWING:
+            # Ensure that the transformation is available
+            now = rclpy.time.Time()
+            if self.tf_buffer.can_transform('map', msg.header.frame_id, now):
+                point_transformed = self.tf_buffer.transform(msg, 'map', timeout=rclpy.duration.Duration(seconds=1))
+                goal_pose = PoseStamped()
+                goal_pose.header.frame_id = "map"
+                goal_pose.header.stamp = self.get_clock().now().to_msg()
+
+                # Calculate the angle for the truncated goal using atan2
+                angle = math.atan2(point_transformed.point.y, point_transformed.point.x)
+
+                # Calculate the position truncating the specified distance from the transformed position
+                goal_pose.pose.position.x = point_transformed.point.x - self.truncate_distance * math.cos(angle)
+                goal_pose.pose.position.y = point_transformed.point.y - self.truncate_distance * math.sin(angle)
+                goal_pose.pose.position.z = 0.0  # Normally zero for ground robots
+
+                goal_pose.pose.orientation.w = 1.0  # No rotation about the z-axis
+
+                # Log the navigation target for debugging
+                self.get_logger().info(f'Navigating to truncated goal: {goal_pose.pose.position.x}, {goal_pose.pose.position.y}')
+
+                # Publish the goal
+                self.goal_update_pub.publish(goal_pose)
 
 def main(args=None):
     rclpy.init(args=args)

src/rove_navigation/setup.py

@@ -22,6 +22,7 @@
     description='TODO: Package description',
     license='TODO: License declaration',
     tests_require=['pytest'],
+    package_dir={'': 'src'},
     entry_points={
         'console_scripts': [
             'navigate_to_person = rove_navigation.navigate_to_person:main',
@@ -31,6 +32,7 @@
             'frontier_publisher = rove_navigation.frontier_publisher:main',
             'exploration = rove_navigation.exploration:main',
             'lost_connection = rove_navigation.lost_connection:main',
+            'mule_behavior = rove_navigation.mule_behavior:main',
         ],
     },
 )