Added connection lost behavior

rove.repos

@@ -1,4 +1,8 @@
 repositories:
+  capra_pose_tracking:
+    type: git
+    url: https://github.com/clubcapra/capra-pose-tracking
+    version: master
   ovis_ros2:
     type: git
     url: https://github.com/clubcapra/ovis_ros2

src/rove_navigation/config/rove_nav_params.yaml

@@ -129,7 +129,7 @@ controller_server:
       simulate_ahead_time: 1.0
       plugin: "dwb_core::DWBLocalPlanner"
       debug_trajectory_details: true
-      min_vel_x: 0.0
+      min_vel_x: -0.4
       min_vel_y: 0.0
       max_vel_x: 0.4
       max_vel_y: 0.0

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,15 @@
+# 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'
+REACH_A = 'REACH_A'
+REACH_B = 'REACH_B'
+
+# 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

@@ -0,0 +1,158 @@
+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 Header
+from nav_msgs.msg import Odometry, Path
+from nav2_msgs.action import NavigateThroughPoses
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+
+# Node to handle when the robot loses connection with internet, stores waypoints to fall back to to try to get back connection
+class LostNetworkFallback(Node):
+
+    def __init__(self):        
+        self.DISTANCE_BETWEEN_FALLBACKS = 0.2 # meters
+        self.NB_MAX_FAILED_CONNS = 5
+        self.PING_RATE = 1.0 # seconds
+        self.PING_TIMEOUT = 2 # seconds
+        self.ADDRESS_TO_PING = '8.8.8.8'
+        self.PORT_TO_PING = 53
+
+        self.ONLINE = 'ONLINE'
+        self.OFFLINE = 'OFFLINE'
+
+        self.state = self.OFFLINE
+        self.nbFailedConns = 0
+        self.fallbackIndex = 0
+
+        super().__init__('lost_network_fallback')
+        self.get_logger().info('Initiating LostNetworkFallback node')
+        self.create_timer(self.PING_RATE, self.fallback_behavior)
+        self.odometry_subscription = self.create_subscription(
+            Odometry, '/odometry/local', self.odometry_callback, 10)
+
+        # Thread to execute goal pose posting (because send_goal_async()'s callback was called before reaching actual goal pose)
+        self.connection_thread = Thread(target=self.check_connection)
+        self.connection_thread.start()
+
+        # Navigator object for nav2
+        self.navigator = BasicNavigator()
+
+        # list of potential fallback positions, ascending order
+        self.fallback_path = Path()
+        self.fallback_path.header = Header(frame_id='map')
+
+        self.current_position = PoseStamped()
+        self.current_position.header.frame_id = 'map'
+        self.current_position.header.stamp = self.navigator.get_clock().now().to_msg()
+        self.current_position.pose.position.x = 0.0
+        self.current_position.pose.position.y = 0.0
+        self.current_position.pose.position.z = 0.0
+        self.current_position.pose.orientation.w = 1.0
+        self.fallback_path.poses.append(self.current_position)
+
+        self.ini_pos = False
+        self.poses_remaining = 0
+
+
+    # Check connection to google.com TODO: check connection to UI user instead
+    # Gets called on a different thread
+    def check_connection(self):
+        while True:
+            start = time.time()
+            try:
+                socket.setdefaulttimeout(self.PING_TIMEOUT)
+                socket.socket(socket.AF_INET, socket.SOCK_STREAM).connect((self.ADDRESS_TO_PING, self.PORT_TO_PING))
+                # if this following code runs, the connection was established
+                self.nbFailedConns = 0
+                if self.state != self.ONLINE:
+                    self.get_logger().info('Connection re-established to server! terminating fallback state')
+                self.state = self.ONLINE
+            except socket.error as e:
+                self.nbFailedConns += 1
+                if self.nbFailedConns >= self.NB_MAX_FAILED_CONNS:
+                    self.state = self.OFFLINE
+                    if self.nbFailedConns == self.NB_MAX_FAILED_CONNS: 
+                        self.get_logger().warn(f'Connection Error!!! ping to server failed {self.nbFailedConns} '+
+                                           'times in a row, activating fallback state, the robot will try to retrace its steps.')
+            delay = time.time() - start
+            if(delay < self.PING_RATE): time.sleep(delay)
+
+    # 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 remaining: '+ str(self.poses_remaining))
+            self.navigate_poses()              
+
+        # self.get_logger().warn(f'Navigating to fallback at ({self.fallback_path[self.fallbackIndex].__str__()})')
+
+        # self.fallbackIndex+=1
+        # if self.fallbackIndex < len(self.fallback_path.poses):
+        #     self.get_logger().warn(f'Fallback {self.fallbackIndex} reached. Will attempt to reach next one if still offline.')
+
+        # if self.fallbackIndex == len(self.fallback_path.poses):
+        #     self.get_logger().error(f'Last fallback reached, waiting and hoping for connection to come back :(')
+
+    def navigate_poses(self):
+        self.navigator.goThroughPoses(self.fallback_path.poses)
+        i = 0
+        while not self.navigator.isTaskComplete():
+
+            if self.state == self.ONLINE:
+                self.fallbackIndex = 0
+                self.navigator.cancelTask()
+                self.fallback_path[self.fallbackIndex:]
+                return
+
+            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) > Duration(seconds=600.0):
+                    self.navigator.cancelTask()
+            i+=1
+
+        # Do something depending on the return code
+        result = self.navigator.getResult()
+        if result == TaskResult.SUCCEEDED:
+            print('Goal succeeded!')
+        elif result == TaskResult.FAILED:
+            self.get_logger().error(f'Couldn\'t proceed through fallbacks, reason unknown')
+        else :
+            self.get_logger().warn(f'nav2 navigator task returned with unknown TaskResult')
+
+    def odometry_callback(self, msg : Odometry):
+        self.current_position = PoseStamped()
+        self.current_position.pose.position = msg.pose.pose.position
+        self.current_position.pose.orientation = msg.pose.pose.orientation
+        self.current_position.header.frame_id = 'map'
+        self.current_position.header.stamp = self.navigator.get_clock().now().to_msg()
+
+        if not self.ini_pos:
+            self.navigator.setInitialPose(self.current_position)
+            self.ini_pos = True
+        # If robot at self.DISTANCE_BETWEEN_FALLBACKS distance or more from last fallback, add new fallback point 
+        elif(self.state == self.ONLINE):
+            dist = [self.current_position.pose.position.x - self.fallback_path.poses[0].pose.position.x,
+                    self.current_position.pose.position.y - self.fallback_path.poses[0].pose.position.y,
+                    self.current_position.pose.position.z - self.fallback_path.poses[0].pose.position.z]
+
+            if(np.linalg.norm(dist) >= self.DISTANCE_BETWEEN_FALLBACKS):
+                self.fallback_path.poses.insert(0, self.current_position)
+                self.get_logger().info(f'New fallback position added: {self.fallback_path.poses[0].__str__()} total fallback points: {len(self.fallback_path.poses)}')            
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LostNetworkFallback()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rove_navigation/rove_navigation/mule_behavior.py

@@ -0,0 +1,165 @@
+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.goal_update_pub = self.create_publisher(
+            PoseStamped,
+            '/goal_pose',
+            10)
+        self.subscription_person_position = self.create_subscription(
+            String,
+            '/navigate_to_pose/_action/status',
+            self.state_listener,
+            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.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.REACH_A
+
+                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.REACH_A:
+                self.get_logger().info(f'Going to point A coordonates {self.startpoint.pose.position}')
+                self.goal_point = self.startPoint
+                self.navigate_to_goal_point()
+                self.change_state('REWIND')
+            if self.state == consts.REACH_B:
+                self.get_logger().info(f'Going to point B coordonates {self.endpoint.pose.position}')
+                self.goal_point = self.endpoint
+                self.navigate_to_goal_point()
+                self.change_state('REWIND')
+
+                # 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')
+
+    def navigate_to_goal_point(self):
+        # Log the navigation target for debugging
+        self.get_logger().info(f'Navigating to goal : {self.goal_point}')
+        # Publish the goal
+        self.goal_update_pub.publish(self.goal_point)
+
+    # # 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()