diff --git a/src/rove_navigation/launch/mule_behaviour.launch.py b/src/rove_navigation/launch/mule_behaviour.launch.py new file mode 100644 index 0000000..6a5c559 --- /dev/null +++ b/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 + ]) diff --git a/src/rove_navigation/rove_navigation/behavior/mule_constants.py b/src/rove_navigation/rove_navigation/behavior/mule_constants.py new file mode 100644 index 0000000..07d0752 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/rove_navigation/rove_navigation/lost_connection.py b/src/rove_navigation/rove_navigation/lost_connection.py index 7365528..dd40fec 100644 --- a/src/rove_navigation/rove_navigation/lost_connection.py +++ b/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__()})') diff --git a/src/rove_navigation/rove_navigation/mule_behavior.py b/src/rove_navigation/rove_navigation/mule_behavior.py new file mode 100644 index 0000000..b378c9b --- /dev/null +++ b/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() diff --git a/src/rove_navigation/rove_navigation/person_following.py b/src/rove_navigation/rove_navigation/person_following.py index 54d905b..053657e 100644 --- a/src/rove_navigation/rove_navigation/person_following.py +++ b/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) diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index e838a8d..a1245aa 100644 --- a/src/rove_navigation/setup.py +++ b/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', ], }, )