diff --git a/rove.repos b/rove.repos index c628c9d..8eb12b9 100644 --- a/rove.repos +++ b/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 diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml index 319d890..098d775 100644 --- a/src/rove_navigation/config/rove_nav_params.yaml +++ b/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 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..1c819ed --- /dev/null +++ b/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 \ 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 new file mode 100644 index 0000000..dd40fec --- /dev/null +++ b/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() 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..f1cf0b9 --- /dev/null +++ b/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() diff --git a/src/rove_navigation/rove_navigation/person_following.py b/src/rove_navigation/rove_navigation/person_following.py index 54d905b..2bd8ad5 100644 --- a/src/rove_navigation/rove_navigation/person_following.py +++ b/src/rove_navigation/rove_navigation/person_following.py @@ -3,14 +3,16 @@ 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): super().__init__('navigate_to_person') self.subscription = self.create_subscription( PointStamped, - '/person_position', + '/tracking/point', self.navigate_to_person, 10) @@ -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 a4ae75a..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', @@ -30,6 +31,8 @@ 'person_following = rove_navigation.person_following:main', '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', ], }, ) diff --git a/src/rove_zed/config/zed_body_trck.yaml b/src/rove_zed/config/zed_body_trck.yaml index e421a25..cd62cfa 100644 --- a/src/rove_zed/config/zed_body_trck.yaml +++ b/src/rove_zed/config/zed_body_trck.yaml @@ -7,7 +7,7 @@ body_format: "BODY_18" # 'BODY_18','BODY_34','BODY_38','BODY_70' allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage max_range: 20.0 # [m] Defines a upper depth range for detections - body_kp_selection: "UPPER_BODY" # 'FULL', 'UPPER_BODY' + body_kp_selection: "FULL" # 'FULL', 'UPPER_BODY' enable_body_fitting: false # Defines if the body fitting will be applied enable_tracking: true # Defines if the object detection will track objects across images flow prediction_timeout_s: 3.0 # During this time [sec], the skeleton will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions diff --git a/src/rove_zed/launch/zed_body_trck.launch.py b/src/rove_zed/launch/zed_body_trck.launch.py index 657b8a9..e452a81 100644 --- a/src/rove_zed/launch/zed_body_trck.launch.py +++ b/src/rove_zed/launch/zed_body_trck.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): - camera_model = 'zedm' + camera_model = 'zed2i' pkg_zed_wrapper = get_package_share_directory('zed_wrapper') launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') @@ -22,7 +22,7 @@ def generate_launch_description(): launch_description_source=launch_file_path, launch_arguments=[ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` ['camera_model', camera_model], - ['camera_name', camera_model], + ['camera_name', 'zed'], ['ros_params_override_path', config_override_path] ] )