From 9f464646f39b118f85d76fbd5d50de8adaf56d53 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Fri, 21 Jun 2024 20:13:51 -0400 Subject: [PATCH 1/4] merge behavior and nav package --- src/rove_behavior/bt_xml/patrolling.xml | 32 ------ src/rove_behavior/package.xml | 18 --- src/rove_behavior/resource/rove_behavior | 0 src/rove_behavior/rove_behavior/__init__.py | 0 src/rove_behavior/rove_behavior/navigation.py | 105 ------------------ .../rove_behavior/square_behavior.py | 74 ------------ src/rove_behavior/setup.cfg | 4 - src/rove_behavior/setup.py | 31 ------ src/rove_behavior/test/test_copyright.py | 25 ----- src/rove_behavior/test/test_flake8.py | 25 ----- src/rove_behavior/test/test_pep257.py | 23 ---- src/rove_bringup/launch/autonomy.launch.py | 2 +- src/rove_bringup/launch/sim.launch.py | 2 +- .../config/follow_dynamic_point.xml | 24 ++++ .../launch/navigation.launch.py | 20 +++- .../rove_navigation}/green_person_tracker.py | 1 + .../rove_navigation/navigate_to_person.py | 29 +++++ src/rove_navigation/setup.py | 2 + src/rove_slam/launch/velodyne_3d.launch.py | 2 +- 19 files changed, 77 insertions(+), 342 deletions(-) delete mode 100644 src/rove_behavior/bt_xml/patrolling.xml delete mode 100644 src/rove_behavior/package.xml delete mode 100644 src/rove_behavior/resource/rove_behavior delete mode 100644 src/rove_behavior/rove_behavior/__init__.py delete mode 100644 src/rove_behavior/rove_behavior/navigation.py delete mode 100644 src/rove_behavior/rove_behavior/square_behavior.py delete mode 100644 src/rove_behavior/setup.cfg delete mode 100644 src/rove_behavior/setup.py delete mode 100644 src/rove_behavior/test/test_copyright.py delete mode 100644 src/rove_behavior/test/test_flake8.py delete mode 100644 src/rove_behavior/test/test_pep257.py create mode 100644 src/rove_navigation/config/follow_dynamic_point.xml rename src/{rove_behavior/rove_behavior => rove_navigation/rove_navigation}/green_person_tracker.py (99%) mode change 100644 => 100755 create mode 100644 src/rove_navigation/rove_navigation/navigate_to_person.py diff --git a/src/rove_behavior/bt_xml/patrolling.xml b/src/rove_behavior/bt_xml/patrolling.xml deleted file mode 100644 index 130e797..0000000 --- a/src/rove_behavior/bt_xml/patrolling.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/rove_behavior/package.xml b/src/rove_behavior/package.xml deleted file mode 100644 index e497557..0000000 --- a/src/rove_behavior/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - rove_behavior - 0.0.0 - behavior package - vscode - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/rove_behavior/resource/rove_behavior b/src/rove_behavior/resource/rove_behavior deleted file mode 100644 index e69de29..0000000 diff --git a/src/rove_behavior/rove_behavior/__init__.py b/src/rove_behavior/rove_behavior/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/rove_behavior/rove_behavior/navigation.py b/src/rove_behavior/rove_behavior/navigation.py deleted file mode 100644 index 9a8e00c..0000000 --- a/src/rove_behavior/rove_behavior/navigation.py +++ /dev/null @@ -1,105 +0,0 @@ - -import py_trees -import transforms3d - -from action_msgs.msg import GoalStatus -from rclpy.action import ActionClient -from nav2_msgs.action import NavigateToPose - - -class GetLocationFromQueue(py_trees.behaviour.Behaviour): - """Gets a location name from the queue""" - - def __init__(self, name, location_dict): - super(GetLocationFromQueue, self).__init__(name) - self.location_dict = location_dict - self.bb = py_trees.blackboard.Blackboard() - - def update(self): - """Checks for the status of the navigation action""" - loc_list = self.bb.get("loc_list") - if len(loc_list) == 0: - self.logger.info("No locations available") - return py_trees.common.Status.FAILURE - else: - target_location = loc_list.pop() - self.logger.info(f"Selected location {target_location}") - target_pose = self.location_dict[target_location] - self.bb.set("target_pose", target_pose) - return py_trees.common.Status.SUCCESS - - def terminate(self, new_status): - self.logger.info(f"Terminated with status {new_status}") - - -class GoToPose(py_trees.behaviour.Behaviour): - """Wrapper behavior around the `move_base` action client""" - - def __init__(self, name, pose, node): - super(GoToPose, self).__init__(name) - self.pose = pose - self.client = None - self.node = node - self.bb = py_trees.blackboard.Blackboard() - - def initialise(self): - """Sends the initial navigation action goal""" - # Check if there is a pose available in the blackboard - try: - target_pose = self.bb.get("target_pose") - if target_pose is not None: - self.pose = target_pose - except: - pass - - self.client = ActionClient(self.node, NavigateToPose, "/navigate_to_pose") - self.client.wait_for_server() - - self.goal_status = None - x, y, theta = self.pose - self.logger.info(f"Going to [x: {x}, y: {y}, theta: {theta}] ...") - self.goal = self.create_move_base_goal(x, y, theta) - self.send_goal_future = self.client.send_goal_async(self.goal) - self.send_goal_future.add_done_callback(self.goal_callback) - - def goal_callback(self, future): - res = future.result() - if res is None or not res.accepted: - return - future = res.get_result_async() - future.add_done_callback(self.goal_result_callback) - - def goal_result_callback(self, future): - # If there is a result, we consider navigation completed and save the - # result code to be checked in the `update()` method. - self.goal_status = future.result().status - - def update(self): - """Checks for the status of the navigation action""" - # If there is a result, we can check the status of the action directly. - # Otherwise, the action is still running. - if self.goal_status is not None: - if self.goal_status == GoalStatus.STATUS_SUCCEEDED: - return py_trees.common.Status.SUCCESS - else: - return py_trees.common.Status.FAILURE - return py_trees.common.Status.RUNNING - - def terminate(self, new_status): - self.logger.info(f"Terminated with status {new_status}") - self.client = None - self.bb.set("target_pose", None) - - def create_move_base_goal(self, x, y, theta): - """Creates a MoveBaseGoal message from a 2D navigation pose""" - goal = NavigateToPose.Goal() - goal.pose.header.frame_id = "map" - goal.pose.header.stamp = self.node.get_clock().now().to_msg() - goal.pose.pose.position.x = x - goal.pose.pose.position.y = y - quat = transforms3d.euler.euler2quat(0, 0, theta) - goal.pose.pose.orientation.w = quat[0] - goal.pose.pose.orientation.x = quat[1] - goal.pose.pose.orientation.y = quat[2] - goal.pose.pose.orientation.z = quat[3] - return goal \ No newline at end of file diff --git a/src/rove_behavior/rove_behavior/square_behavior.py b/src/rove_behavior/rove_behavior/square_behavior.py deleted file mode 100644 index ae25d39..0000000 --- a/src/rove_behavior/rove_behavior/square_behavior.py +++ /dev/null @@ -1,74 +0,0 @@ -#!/usr/bin/env python3 - -""" -Test script for running a simple behavior tree for autonomous navigation. -Taken from https://github.com/sea-bass/turtlebot3_behavior_demos/blob/main/tb3_autonomy/scripts/autonomy_node.py - - -Example usage: - ros2 run rove_behavior autonomy_node -""" - -import os -import yaml -import random -import rclpy -from rclpy.node import Node -import time -import py_trees -import py_trees_ros -from py_trees.common import OneShotPolicy -from ament_index_python.packages import get_package_share_directory - -from rove_behavior.navigation import GoToPose, GetLocationFromQueue - - -class AutonomyBehavior(Node): - def __init__(self): - super().__init__("autonomy_node") - self.get_logger().info("AutonomyBehavior node has been initialized.") - - # Defines object locations as [x, y, theta] - self.locations = { - "location1": [0.0, 0.0, -1.571], - "location2": [2.0, 0.0, 1.571], - "location3": [2.0, 2.0, 0.0], - "location4": [0.0, 2.0, 3.142], - } - - self.loc_list = list(self.locations.keys()) - - # Create and setup the behavior tree - self.tree = self.create_naive_tree() - - - def create_naive_tree(self): - """Create behavior tree with explicit nodes for each location.""" - seq = py_trees.composites.Sequence(name="navigation", memory=True) - root = py_trees.decorators.OneShot( - name="root", child=seq, policy=OneShotPolicy.ON_SUCCESSFUL_COMPLETION - ) - tree = py_trees_ros.trees.BehaviourTree(root, unicode_tree_debug=False) - tree.setup(timeout=15.0, node=self) - - for loc in self.loc_list: - pose = self.locations[loc] - seq.add_child(GoToPose(f"go_to_{loc}", pose, self)) - - return tree - - def execute(self, period=0.5): - """Executes the behavior tree at the specified period.""" - self.tree.tick_tock(period_ms=period * 1000.0) - rclpy.spin(self.tree.node) - rclpy.shutdown() - - -def main(args=None): - rclpy.init(args=args) - behavior = AutonomyBehavior() - behavior.execute() - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/src/rove_behavior/setup.cfg b/src/rove_behavior/setup.cfg deleted file mode 100644 index 8d354c1..0000000 --- a/src/rove_behavior/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rove_behavior -[install] -install_scripts=$base/lib/rove_behavior diff --git a/src/rove_behavior/setup.py b/src/rove_behavior/setup.py deleted file mode 100644 index 6638a05..0000000 --- a/src/rove_behavior/setup.py +++ /dev/null @@ -1,31 +0,0 @@ -from setuptools import find_packages, setup -from glob import glob -import os - -package_name = 'rove_behavior' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name), glob('launch/*')), - (os.path.join('share', package_name), glob('config/*')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='vscode', - maintainer_email='capra@ens.etsmtl.ca', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'square_behavior = rove_behavior.square_behavior:main', - 'green_person_tracker = rove_behavior.green_person_tracker:main', - ], - }, -) diff --git a/src/rove_behavior/test/test_copyright.py b/src/rove_behavior/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/src/rove_behavior/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/rove_behavior/test/test_flake8.py b/src/rove_behavior/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/rove_behavior/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/rove_behavior/test/test_pep257.py b/src/rove_behavior/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/rove_behavior/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py index 650994f..24ab0fa 100644 --- a/src/rove_bringup/launch/autonomy.launch.py +++ b/src/rove_bringup/launch/autonomy.launch.py @@ -55,5 +55,5 @@ def generate_launch_description(): return LaunchDescription([ slam, slam3d, - nav, + #nav, ]) diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py index f9e06fc..73dcebc 100644 --- a/src/rove_bringup/launch/sim.launch.py +++ b/src/rove_bringup/launch/sim.launch.py @@ -84,7 +84,7 @@ def generate_launch_description(): # fake human tracker human_tracker = Node( - package='rove_behavior', + package='rove_navigation', executable='green_person_tracker', name='green_person_tracker', output='screen', diff --git a/src/rove_navigation/config/follow_dynamic_point.xml b/src/rove_navigation/config/follow_dynamic_point.xml new file mode 100644 index 0000000..34ebd18 --- /dev/null +++ b/src/rove_navigation/config/follow_dynamic_point.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py index 53857fd..531dd64 100644 --- a/src/rove_navigation/launch/navigation.launch.py +++ b/src/rove_navigation/launch/navigation.launch.py @@ -7,6 +7,8 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + def generate_launch_description(): @@ -18,15 +20,29 @@ def generate_launch_description(): nav2_config_path = os.path.join(pkg_rove_navigation, 'config', 'rove_nav_params.yaml') + + bt_xml_path = PathJoinSubstitution( + [pkg_rove_navigation, 'config', 'follow_dynamic_point.xml'] + ) + + navigation_to_person_node = Node( + package="rove_navigation", + executable='navigate_to_person', + name='navigation_to_person', + output='screen', + ) + nav = IncludeLaunchDescription( PythonLaunchDescriptionSource(navigation_launch_path), launch_arguments={ 'use_sim_time': 'true', - 'params_file': nav2_config_path + 'params_file': nav2_config_path, + 'bt_xml_filename': bt_xml_path, }.items() ) return LaunchDescription([ - nav + nav, + navigation_to_person_node, ]) diff --git a/src/rove_behavior/rove_behavior/green_person_tracker.py b/src/rove_navigation/rove_navigation/green_person_tracker.py old mode 100644 new mode 100755 similarity index 99% rename from src/rove_behavior/rove_behavior/green_person_tracker.py rename to src/rove_navigation/rove_navigation/green_person_tracker.py index c843e04..f63dda9 --- a/src/rove_behavior/rove_behavior/green_person_tracker.py +++ b/src/rove_navigation/rove_navigation/green_person_tracker.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import rclpy from rclpy.node import Node from sensor_msgs.msg import Image diff --git a/src/rove_navigation/rove_navigation/navigate_to_person.py b/src/rove_navigation/rove_navigation/navigate_to_person.py new file mode 100644 index 0000000..f85e53b --- /dev/null +++ b/src/rove_navigation/rove_navigation/navigate_to_person.py @@ -0,0 +1,29 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PointStamped, PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import math + +class NavigateToPersonNode(Node): + def __init__(self): + super().__init__('navigate_to_person') + self.subscription = self.create_subscription( + PointStamped, + '/person_position', + self.navigate_to_person, + 10) + self.navigator = BasicNavigator() + + def navigate_to_person(self, msg): + goal_pose = PoseStamped() + goal_pose.header.frame_id = "map" + self.navigator.goToPose(goal_pose) + +def main(args=None): + rclpy.init(args=args) + node = NavigateToPersonNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index 871b4ee..f7b3339 100644 --- a/src/rove_navigation/setup.py +++ b/src/rove_navigation/setup.py @@ -24,6 +24,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'navigate_to_person = rove_navigation.navigate_to_person:main', + 'green_person_tracker = rove_navigation.green_person_tracker:main', ], }, ) diff --git a/src/rove_slam/launch/velodyne_3d.launch.py b/src/rove_slam/launch/velodyne_3d.launch.py index f0636e2..cb66d24 100644 --- a/src/rove_slam/launch/velodyne_3d.launch.py +++ b/src/rove_slam/launch/velodyne_3d.launch.py @@ -118,5 +118,5 @@ def generate_launch_description(): icp_odometry_node, point_cloud_assembler_node, rtabmap_node, - rtabmap_viz_node + #rtabmap_viz_node ]) From 3dae26fb4d4f5692631bd3887350780a029d22ff Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 22 Jun 2024 03:55:47 +0200 Subject: [PATCH 2/4] person following --- .../config/follow_dynamic_point.xml | 17 ++++------- .../config/rove_nav_params.yaml | 1 + .../rove_navigation/green_person_tracker.py | 17 +++++++++-- .../rove_navigation/navigate_to_person.py | 29 +++++++++++++++++-- 4 files changed, 48 insertions(+), 16 deletions(-) diff --git a/src/rove_navigation/config/follow_dynamic_point.xml b/src/rove_navigation/config/follow_dynamic_point.xml index 34ebd18..bbc531f 100644 --- a/src/rove_navigation/config/follow_dynamic_point.xml +++ b/src/rove_navigation/config/follow_dynamic_point.xml @@ -1,24 +1,19 @@ - - + + - + - + - + - \ No newline at end of file + diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml index c290fad..f05b786 100644 --- a/src/rove_navigation/config/rove_nav_params.yaml +++ b/src/rove_navigation/config/rove_nav_params.yaml @@ -54,6 +54,7 @@ bt_navigator: robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 + default_nav_to_pose_bt_xml: ./src/rove_navigation/config/follow_dynamic_point.xml default_server_timeout: 20 enable_groot_monitoring: true groot_zmq_publisher_port: 1666 diff --git a/src/rove_navigation/rove_navigation/green_person_tracker.py b/src/rove_navigation/rove_navigation/green_person_tracker.py index f63dda9..b8a709c 100755 --- a/src/rove_navigation/rove_navigation/green_person_tracker.py +++ b/src/rove_navigation/rove_navigation/green_person_tracker.py @@ -6,6 +6,8 @@ from cv_bridge import CvBridge import cv2 import numpy as np +from tf2_ros import Buffer, TransformListener +from tf2_geometry_msgs import do_transform_point class Tracker(Node): def __init__(self): @@ -39,6 +41,10 @@ def __init__(self): self.cx = 640 # Optical center x coordinate self.cy = 360 # Optical center y coordinate + # TF2 Buffer and Listener + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + def depth_callback(self, msg): try: self.depth_image = self.bridge.imgmsg_to_cv2(msg, '32FC1') @@ -102,8 +108,15 @@ def publish_position(self, x, y, depth, header): point.point.x = x_cam point.point.y = y_cam point.point.z = z_cam - self.position_pub.publish(point) - self.get_logger().info(f"Person (green square) position: x={point.point.x}, y={point.point.y}, z={point.point.z}") + + # Transform the point to the base_link frame + try: + transform = self.tf_buffer.lookup_transform('base_link', header.frame_id, rclpy.time.Time()) + point_transformed = do_transform_point(point, transform) + self.position_pub.publish(point_transformed) + self.get_logger().info(f"Person (green square) position in base_link: x={point_transformed.point.x}, y={point_transformed.point.y}, z={point_transformed.point.z}") + except Exception as e: + self.get_logger().error(f"Failed to transform point: {str(e)}") def main(args=None): rclpy.init(args=args) diff --git a/src/rove_navigation/rove_navigation/navigate_to_person.py b/src/rove_navigation/rove_navigation/navigate_to_person.py index f85e53b..69d0441 100644 --- a/src/rove_navigation/rove_navigation/navigate_to_person.py +++ b/src/rove_navigation/rove_navigation/navigate_to_person.py @@ -2,6 +2,8 @@ from rclpy.node import Node from geometry_msgs.msg import PointStamped, PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator +from tf2_ros import Buffer, TransformListener +from tf2_geometry_msgs import PointStamped # This is necessary for the transform import math class NavigateToPersonNode(Node): @@ -13,11 +15,32 @@ def __init__(self): self.navigate_to_person, 10) self.navigator = BasicNavigator() + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) def navigate_to_person(self, msg): - goal_pose = PoseStamped() - goal_pose.header.frame_id = "map" - self.navigator.goToPose(goal_pose) + + # Ensure that the transformation is available + now = rclpy.time.Time() + if self.tf_buffer.can_transform('map', msg.header.frame_id, now): + # Properly use tf2_geometry_msgs do_transform function + 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() + + # Set the transformed position as the goal + goal_pose.pose.position.x = point_transformed.point.x + goal_pose.pose.position.y = point_transformed.point.y + 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 transformed goal: {goal_pose.pose.position.x} {goal_pose.pose.position.y}...') + + # Command the robot to navigate to the goal + self.navigator.goToPose(goal_pose) def main(args=None): rclpy.init(args=args) From a16d9dce03ca309a24654c121b74029c04dbf186 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 22 Jun 2024 13:02:14 +0200 Subject: [PATCH 3/4] change to behavior to publisher --- .../config/follow_dynamic_point.xml | 30 +++++++++---------- .../config/rove_nav_params.yaml | 1 - .../launch/navigation.launch.py | 3 +- .../rove_navigation/navigate_to_person.py | 12 ++++++-- 4 files changed, 24 insertions(+), 22 deletions(-) diff --git a/src/rove_navigation/config/follow_dynamic_point.xml b/src/rove_navigation/config/follow_dynamic_point.xml index bbc531f..183d36a 100644 --- a/src/rove_navigation/config/follow_dynamic_point.xml +++ b/src/rove_navigation/config/follow_dynamic_point.xml @@ -1,19 +1,17 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + - + \ No newline at end of file diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml index f05b786..c290fad 100644 --- a/src/rove_navigation/config/rove_nav_params.yaml +++ b/src/rove_navigation/config/rove_nav_params.yaml @@ -54,7 +54,6 @@ bt_navigator: robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 - default_nav_to_pose_bt_xml: ./src/rove_navigation/config/follow_dynamic_point.xml default_server_timeout: 20 enable_groot_monitoring: true groot_zmq_publisher_port: 1666 diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py index 531dd64..d11f583 100644 --- a/src/rove_navigation/launch/navigation.launch.py +++ b/src/rove_navigation/launch/navigation.launch.py @@ -38,11 +38,10 @@ def generate_launch_description(): launch_arguments={ 'use_sim_time': 'true', 'params_file': nav2_config_path, - 'bt_xml_filename': bt_xml_path, }.items() ) return LaunchDescription([ nav, - navigation_to_person_node, + #navigation_to_person_node, ]) diff --git a/src/rove_navigation/rove_navigation/navigate_to_person.py b/src/rove_navigation/rove_navigation/navigate_to_person.py index 69d0441..2c327b9 100644 --- a/src/rove_navigation/rove_navigation/navigate_to_person.py +++ b/src/rove_navigation/rove_navigation/navigate_to_person.py @@ -14,7 +14,12 @@ def __init__(self): '/person_position', self.navigate_to_person, 10) - self.navigator = BasicNavigator() + + self.goal_update_pub = self.create_publisher( + PoseStamped, + '/goal_update', + 10) + self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) @@ -39,8 +44,9 @@ def navigate_to_person(self, msg): # Log the navigation target for debugging self.get_logger().info(f'Navigating to transformed goal: {goal_pose.pose.position.x} {goal_pose.pose.position.y}...') - # Command the robot to navigate to the goal - self.navigator.goToPose(goal_pose) + # Publish the goal + self.goal_update_pub.publish(goal_pose) + def main(args=None): rclpy.init(args=args) From bae6538395ecefeafe3be95273610ebded6105a1 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 22 Jun 2024 18:19:13 +0200 Subject: [PATCH 4/4] square walk + 1 m distance person following --- src/rove_bringup/launch/autonomy.launch.py | 2 +- src/rove_bringup/launch/sim.launch.py | 25 ++-- src/rove_description/worlds/actor.sdf | 54 ++++++--- src/rove_navigation/config/bt_waypoints.xml | 25 ---- .../config/follow_dynamic_point.xml | 17 --- .../launch/navigation.launch.py | 8 +- .../rove_navigation/behavior/__init__.py | 0 .../rove_navigation/behavior/navigation.py | 110 ++++++++++++++++++ .../behavior_square_pattroling.py | 75 ++++++++++++ .../rove_navigation/green_person_tracker.py | 8 +- ...igate_to_person.py => person_following.py} | 29 ++--- src/rove_navigation/setup.py | 2 + 12 files changed, 267 insertions(+), 88 deletions(-) delete mode 100644 src/rove_navigation/config/bt_waypoints.xml delete mode 100644 src/rove_navigation/config/follow_dynamic_point.xml create mode 100644 src/rove_navigation/rove_navigation/behavior/__init__.py create mode 100644 src/rove_navigation/rove_navigation/behavior/navigation.py create mode 100644 src/rove_navigation/rove_navigation/behavior_square_pattroling.py rename src/rove_navigation/rove_navigation/{navigate_to_person.py => person_following.py} (66%) diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py index 24ab0fa..650994f 100644 --- a/src/rove_bringup/launch/autonomy.launch.py +++ b/src/rove_bringup/launch/autonomy.launch.py @@ -55,5 +55,5 @@ def generate_launch_description(): return LaunchDescription([ slam, slam3d, - #nav, + nav, ]) diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py index 73dcebc..52eb91e 100644 --- a/src/rove_bringup/launch/sim.launch.py +++ b/src/rove_bringup/launch/sim.launch.py @@ -8,6 +8,7 @@ from launch.substitutions import Command from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue +from math import pi def generate_launch_description(): @@ -56,18 +57,22 @@ def generate_launch_description(): output='screen', ) + yaw = -pi / 2 + # Spawn robot spawn_rove = Node( - package='ros_gz_sim', - executable='create', - arguments=['-name', 'rove', - '-topic', 'robot_description', - '-x', '-2', - '-y', '0', - '-z', '0.1', - ], - output='screen', - ) + package='ros_gz_sim', + executable='create', + arguments=[ + '-name', 'rove', + '-topic', 'robot_description', + '-x', '0', + '-y', '0', + '-z', '0.1', + '-Y', str(yaw), + ], + output='screen', +) # Takes the description and joint angles as inputs and publishes # the 3D poses of the robot links diff --git a/src/rove_description/worlds/actor.sdf b/src/rove_description/worlds/actor.sdf index ce620a2..886fe60 100644 --- a/src/rove_description/worlds/actor.sdf +++ b/src/rove_description/worlds/actor.sdf @@ -32,28 +32,54 @@ \ No newline at end of file diff --git a/src/rove_navigation/config/bt_waypoints.xml b/src/rove_navigation/config/bt_waypoints.xml deleted file mode 100644 index 5f51ed0..0000000 --- a/src/rove_navigation/config/bt_waypoints.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/rove_navigation/config/follow_dynamic_point.xml b/src/rove_navigation/config/follow_dynamic_point.xml deleted file mode 100644 index 183d36a..0000000 --- a/src/rove_navigation/config/follow_dynamic_point.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py index d11f583..2cd2fb3 100644 --- a/src/rove_navigation/launch/navigation.launch.py +++ b/src/rove_navigation/launch/navigation.launch.py @@ -25,10 +25,10 @@ def generate_launch_description(): [pkg_rove_navigation, 'config', 'follow_dynamic_point.xml'] ) - navigation_to_person_node = Node( + person_following_node = Node( package="rove_navigation", - executable='navigate_to_person', - name='navigation_to_person', + executable='person_following', + name='person_following', output='screen', ) @@ -43,5 +43,5 @@ def generate_launch_description(): return LaunchDescription([ nav, - #navigation_to_person_node, + person_following_node, ]) diff --git a/src/rove_navigation/rove_navigation/behavior/__init__.py b/src/rove_navigation/rove_navigation/behavior/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rove_navigation/rove_navigation/behavior/navigation.py b/src/rove_navigation/rove_navigation/behavior/navigation.py new file mode 100644 index 0000000..0587ff1 --- /dev/null +++ b/src/rove_navigation/rove_navigation/behavior/navigation.py @@ -0,0 +1,110 @@ + +import py_trees +import transforms3d + +from action_msgs.msg import GoalStatus +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose + +import numpy as np +from geometry_msgs.msg import PointStamped + +from rclpy.node import Node + + +class GetLocationFromQueue(py_trees.behaviour.Behaviour): + """Gets a location name from the queue""" + + def __init__(self, name, location_dict): + super(GetLocationFromQueue, self).__init__(name) + self.location_dict = location_dict + self.bb = py_trees.blackboard.Blackboard() + + def update(self): + """Checks for the status of the navigation action""" + loc_list = self.bb.get("loc_list") + if len(loc_list) == 0: + self.logger.info("No locations available") + return py_trees.common.Status.FAILURE + else: + target_location = loc_list.pop() + self.logger.info(f"Selected location {target_location}") + target_pose = self.location_dict[target_location] + self.bb.set("target_pose", target_pose) + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + self.logger.info(f"Terminated with status {new_status}") + + +class GoToPose(py_trees.behaviour.Behaviour): + """Wrapper behavior around the `move_base` action client""" + + def __init__(self, name, pose, node): + super(GoToPose, self).__init__(name) + self.pose = pose + self.client = None + self.node = node + self.bb = py_trees.blackboard.Blackboard() + + def initialise(self): + """Sends the initial navigation action goal""" + # Check if there is a pose available in the blackboard + try: + target_pose = self.bb.get("target_pose") + if target_pose is not None: + self.pose = target_pose + except: + pass + + self.client = ActionClient(self.node, NavigateToPose, "/navigate_to_pose") + self.client.wait_for_server() + + self.goal_status = None + x, y, theta = self.pose + self.logger.info(f"Going to [x: {x}, y: {y}, theta: {theta}] ...") + self.goal = self.create_move_base_goal(x, y, theta) + self.send_goal_future = self.client.send_goal_async(self.goal) + self.send_goal_future.add_done_callback(self.goal_callback) + + def goal_callback(self, future): + res = future.result() + if res is None or not res.accepted: + return + future = res.get_result_async() + future.add_done_callback(self.goal_result_callback) + + def goal_result_callback(self, future): + # If there is a result, we consider navigation completed and save the + # result code to be checked in the `update()` method. + self.goal_status = future.result().status + + def update(self): + """Checks for the status of the navigation action""" + # If there is a result, we can check the status of the action directly. + # Otherwise, the action is still running. + if self.goal_status is not None: + if self.goal_status == GoalStatus.STATUS_SUCCEEDED: + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.FAILURE + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + self.logger.info(f"Terminated with status {new_status}") + self.client = None + self.bb.set("target_pose", None) + + def create_move_base_goal(self, x, y, theta): + """Creates a MoveBaseGoal message from a 2D navigation pose""" + goal = NavigateToPose.Goal() + goal.pose.header.frame_id = "map" + goal.pose.header.stamp = self.node.get_clock().now().to_msg() + goal.pose.pose.position.x = x + goal.pose.pose.position.y = y + quat = transforms3d.euler.euler2quat(0, 0, theta) + goal.pose.pose.orientation.w = quat[0] + goal.pose.pose.orientation.x = quat[1] + goal.pose.pose.orientation.y = quat[2] + goal.pose.pose.orientation.z = quat[3] + return goal \ No newline at end of file diff --git a/src/rove_navigation/rove_navigation/behavior_square_pattroling.py b/src/rove_navigation/rove_navigation/behavior_square_pattroling.py new file mode 100644 index 0000000..2a14e52 --- /dev/null +++ b/src/rove_navigation/rove_navigation/behavior_square_pattroling.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + +""" +Test script for running a simple behavior tree for autonomous navigation. +Taken from https://github.com/sea-bass/turtlebot3_behavior_demos/blob/main/tb3_autonomy/scripts/autonomy_node.py + + +Example usage: + ros2 run rove_behavior autonomy_node +""" + +import os +import yaml +import random +import rclpy +from rclpy.node import Node +import time +import py_trees +import py_trees_ros +from py_trees.common import OneShotPolicy +from ament_index_python.packages import get_package_share_directory + +from rove_navigation.behavior.navigation import GoToPose, GetLocationFromQueue + + +class AutonomyBehavior(Node): + def __init__(self): + super().__init__("autonomy_node") + self.get_logger().info("AutonomyBehavior node has been initialized.") + + # Defines object locations as [x, y, theta] + self.locations = { + "location1": [0.0, 0.0, -1.571], + "location2": [2.0, 0.0, 1.571], + "location3": [2.0, 2.0, 0.0], + "location4": [0.0, 2.0, 3.142], + } + + self.loc_list = list(self.locations.keys()) + + # Create and setup the behavior tree + self.tree = self.create_naive_tree() + + + def create_naive_tree(self): + """Create behavior tree with explicit nodes for each location.""" + seq = py_trees.composites.Sequence(name="navigation", memory=True) + root = py_trees.decorators.OneShot( + name="root", child=seq, policy=OneShotPolicy.ON_SUCCESSFUL_COMPLETION + ) + tree = py_trees_ros.trees.BehaviourTree(root, unicode_tree_debug=False) + tree.setup(timeout=15.0, node=self) + + for loc in self.loc_list: + pose = self.locations[loc] + seq.add_child(GoToPose(f"go_to_{loc}", pose, self)) + self.get_logger().info(f"Added GoToPose node for {loc} with pose {pose}") + + return tree + + def execute(self, period=0.5): + """Executes the behavior tree at the specified period.""" + self.tree.tick_tock(period_ms=period * 1000.0) + rclpy.spin(self.tree.node) + rclpy.shutdown() + + +def main(args=None): + rclpy.init(args=args) + behavior = AutonomyBehavior() + behavior.execute() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/rove_navigation/rove_navigation/green_person_tracker.py b/src/rove_navigation/rove_navigation/green_person_tracker.py index b8a709c..cb8fa9f 100755 --- a/src/rove_navigation/rove_navigation/green_person_tracker.py +++ b/src/rove_navigation/rove_navigation/green_person_tracker.py @@ -36,10 +36,10 @@ def __init__(self): self.get_logger().info("Node started!") # Camera intrinsic parameters - self.fx = 700 # Focal length in x axis - self.fy = 700 # Focal length in y axis - self.cx = 640 # Optical center x coordinate - self.cy = 360 # Optical center y coordinate + self.fx = 363 # Focal length in x axis + self.fy = 363 # Focal length in y axis + self.cx = 672 # Optical center x coordinate + self.cy = 188 # Optical center y coordinate # TF2 Buffer and Listener self.tf_buffer = Buffer() diff --git a/src/rove_navigation/rove_navigation/navigate_to_person.py b/src/rove_navigation/rove_navigation/person_following.py similarity index 66% rename from src/rove_navigation/rove_navigation/navigate_to_person.py rename to src/rove_navigation/rove_navigation/person_following.py index 2c327b9..54d905b 100644 --- a/src/rove_navigation/rove_navigation/navigate_to_person.py +++ b/src/rove_navigation/rove_navigation/person_following.py @@ -1,56 +1,59 @@ import rclpy from rclpy.node import Node from geometry_msgs.msg import PointStamped, PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator from tf2_ros import Buffer, TransformListener -from tf2_geometry_msgs import PointStamped # This is necessary for the transform +from tf2_geometry_msgs import PointStamped import math class NavigateToPersonNode(Node): - def __init__(self): + def __init__(self, truncate_distance): super().__init__('navigate_to_person') self.subscription = self.create_subscription( PointStamped, '/person_position', self.navigate_to_person, 10) - + self.goal_update_pub = self.create_publisher( PoseStamped, - '/goal_update', + '/goal_pose', 10) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - def navigate_to_person(self, msg): + # Distance to truncate from the target position + self.truncate_distance = truncate_distance + 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): - # Properly use tf2_geometry_msgs do_transform function 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() - # Set the transformed position as the goal - goal_pose.pose.position.x = point_transformed.point.x - goal_pose.pose.position.y = point_transformed.point.y + # 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 transformed goal: {goal_pose.pose.position.x} {goal_pose.pose.position.y}...') + 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) - node = NavigateToPersonNode() + # You can adjust the truncate distance here + node = NavigateToPersonNode(truncate_distance=1.5) # for example, truncate 1.5 meters rclpy.spin(node) rclpy.shutdown() diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index f7b3339..9e8d29f 100644 --- a/src/rove_navigation/setup.py +++ b/src/rove_navigation/setup.py @@ -26,6 +26,8 @@ 'console_scripts': [ 'navigate_to_person = rove_navigation.navigate_to_person:main', 'green_person_tracker = rove_navigation.green_person_tracker:main', + 'behavior_square_pattroling = rove_navigation.behavior_square_pattroling:main', + 'person_following = rove_navigation.person_following:main', ], }, )