From 397c38407a76e5490d8121936b33d98fcd8c95eb Mon Sep 17 00:00:00 2001 From: "G.Doisy" Date: Wed, 19 Jan 2022 09:13:24 +0100 Subject: [PATCH 01/22] fix invert logic (#2772) --- nav2_costmap_2d/src/costmap_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 1179454d6f..3f9da4013d 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -69,7 +69,7 @@ void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, boo bool xrange = x > start_x && x < end_x; for (int y = 0; y < static_cast(getSizeInCellsY()); y++) { - if ((xrange && y > start_y && y < end_y) != invert) { + if ((xrange && y > start_y && y < end_y) == invert) { continue; } int index = getIndex(x, y); From 0bd193a2941bf648876ab142794ded514f8f409f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erik=20=C3=96rjehag?= Date: Fri, 21 Jan 2022 22:50:29 +0100 Subject: [PATCH 02/22] Bugfix tf lookup of goal pose in nav2_controller (#2780) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Erik Örjehag --- .../nav2_controller/nav2_controller.hpp | 2 +- nav2_controller/src/nav2_controller.cpp | 23 +++++++++++-------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 8114c53c7c..77e20d93e4 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -243,7 +243,7 @@ class ControllerServer : public nav2_util::LifecycleNode double failure_tolerance_; // Whether we've published the single controller warning yet - geometry_msgs::msg::Pose end_pose_; + geometry_msgs::msg::PoseStamped end_pose_; // Last time the controller generated a valid command rclcpp::Time last_valid_cmd_time_; diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 52f4c05827..e23ea1f7e9 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -417,18 +417,14 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) } controllers_[current_controller_]->setPlan(path); - auto end_pose = path.poses.back(); - end_pose.header.frame_id = path.header.frame_id; - rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); - nav_2d_utils::transformPose( - costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), - end_pose, end_pose, tolerance); + end_pose_ = path.poses.back(); + end_pose_.header.frame_id = path.header.frame_id; goal_checkers_[current_goal_checker_]->reset(); RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", - end_pose.pose.position.x, end_pose.pose.position.y); - end_pose_ = end_pose.pose; + end_pose_.pose.position.x, end_pose_.pose.position.y); + current_path_ = path; } @@ -555,7 +551,16 @@ bool ControllerServer::isGoalReached() nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); - return goal_checkers_[current_goal_checker_]->isGoalReached(pose.pose, end_pose_, velocity); + + geometry_msgs::msg::PoseStamped transformed_end_pose; + rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); + nav_2d_utils::transformPose( + costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), + end_pose_, transformed_end_pose, tolerance); + + return goal_checkers_[current_goal_checker_]->isGoalReached( + pose.pose, transformed_end_pose.pose, + velocity); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) From 2f5d1e67c67d5cb9341bd7fa7c19c25b3ccd886f Mon Sep 17 00:00:00 2001 From: Ekansh Sharma Date: Sat, 5 Feb 2022 01:49:03 +0100 Subject: [PATCH 03/22] Feature to call controller action server to follow path (#2789) * Added call to controller action server with a path to follow ++ Added a new function in robot_navigator,py ++ Added a launch script to test function ++ Updated setup.py to include demo_follow_path.py * Code refactoring * Code refactoring * Code refactoring for consistency * Updated README.md * Resolved executable conflict in setup.py for example_follow_path.py * Code refactoring with ament_flake8 --- nav2_simple_commander/README.md | 5 +- .../launch/follow_path_example_launch.py | 77 ++++++++++++++++ .../example_follow_path.py | 92 +++++++++++++++++++ .../nav2_simple_commander/robot_navigator.py | 24 +++++ nav2_simple_commander/setup.py | 1 + 5 files changed, 197 insertions(+), 2 deletions(-) create mode 100644 nav2_simple_commander/launch/follow_path_example_launch.py create mode 100644 nav2_simple_commander/nav2_simple_commander/example_follow_path.py diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 707e372294..c44d4fea08 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -18,6 +18,7 @@ The methods provided by the basic navigator are shown below, with inputs and exp | goThroughPoses(poses) | Requests the robot to drive through a set of poses (list of `PoseStamped`).| | goToPose(pose) | Requests the robot to drive to a pose (`PoseStamped`). | | followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. | +| followPath(path) | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | | cancelNav() | Cancel an ongoing `goThroughPoses` `goToPose` or `followWaypoints` request.| | isNavComplete() | Checks if navigation is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | | getFeedback() | Gets feedback from navigation task, returns action server feedback object. | @@ -100,8 +101,8 @@ The `nav2_simple_commander` has a few examples to highlight the API functions av - `example_nav_to_pose.py` - Demonstrates the navigate to pose capabilities of the navigator, as well as a number of auxiliary methods. - `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods. -- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods required. - +- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods. +- `example_follow_path.py` - Demonstrates the path following capabilities of the navigator, as well as a number of auxiliary methods. ## Demos The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API: diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py new file mode 100644 index 0000000000..41624dff6f --- /dev/null +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_follow_path', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py new file mode 100644 index 0000000000..fe94f98798 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -0,0 +1,92 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# 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 geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy + + +""" +Basic navigation demo to follow a given path +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # Go to our demos first goal pose + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = -3.0 + goal_pose.pose.position.y = -2.0 + goal_pose.pose.orientation.w = 1.0 + + # Sanity check a valid path exists + path = navigator.getPath(initial_pose, goal_pose) + + # Follow path + navigator.followPath(path) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated distance remaining to goal position: ' + + '{0:.3f}'.format(feedback.distance_to_goal) + + '\nCurrent speed of the robot: ' + + '{0:.3f}'.format(feedback.speed)) + + # Do something depending on the return code + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') + elif result == NavigationResult.CANCELED: + print('Goal was canceled!') + elif result == NavigationResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 80c21cfb58..ce0ee8d01b 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -23,6 +23,7 @@ from lifecycle_msgs.srv import GetState from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose from nav2_msgs.action import FollowWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import FollowPath from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes import rclpy @@ -62,6 +63,7 @@ def __init__(self): 'navigate_through_poses') self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') + self.follow_path_client = ActionClient(self, FollowPath, 'follow_path') self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, 'compute_path_to_pose') self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses, @@ -155,6 +157,28 @@ def followWaypoints(self, poses): self.result_future = self.goal_handle.get_result_async() return True + def followPath(self, path): + """Send a `FollowPath` action request.""" + self.debug("Waiting for 'FollowPath' action server") + while not self.follow_path_client.wait_for_server(timeout_sec=1.0): + self.info("'FollowPath' action server not available, waiting...") + + goal_msg = FollowPath.Goal() + goal_msg.path = path + + self.info('Executing path...') + send_goal_future = self.follow_path_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Follow path was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def cancelNav(self): """Cancel pending navigation request of any type.""" self.info('Canceling current goal.') diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index ca44856c5a..8fbdd00f11 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -28,6 +28,7 @@ 'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main', 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', + 'example_follow_path = nav2_simple_commander.example_follow_path:main', 'demo_picking = nav2_simple_commander.demo_picking:main', 'demo_inspection = nav2_simple_commander.demo_inspection:main', 'demo_security = nav2_simple_commander.demo_security:main', From 38ca8d58c3edfb7ac64596b5e4de406a27aeae2d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 7 Feb 2022 19:53:06 +0100 Subject: [PATCH 04/22] Add recoveries to simple commander (#2792) * fix Typo * add recoveries * add docs * added demo using backup and spin * rename isNavigationComplete to isTaskComplete * rename cancelNav to cancelTask * add prints * fix premature exit * rename NavResult to TaskResult * fix readme order * fix import order * renaming * renaming --- nav2_simple_commander/README.md | 20 ++-- .../launch/recoveries_example_launch.py | 77 +++++++++++++ .../nav2_simple_commander/demo_inspection.py | 13 +-- .../nav2_simple_commander/demo_picking.py | 12 +- .../nav2_simple_commander/demo_recoveries.py | 105 ++++++++++++++++++ .../nav2_simple_commander/demo_security.py | 12 +- .../example_follow_path.py | 10 +- .../example_nav_through_poses.py | 12 +- .../example_nav_to_pose.py | 12 +- .../example_waypoint_follower.py | 12 +- .../nav2_simple_commander/robot_navigator.py | 75 ++++++++++--- nav2_simple_commander/setup.py | 1 + 12 files changed, 295 insertions(+), 66 deletions(-) create mode 100644 nav2_simple_commander/launch/recoveries_example_launch.py create mode 100644 nav2_simple_commander/nav2_simple_commander/demo_recoveries.py diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index c44d4fea08..3a368511f2 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -19,10 +19,12 @@ The methods provided by the basic navigator are shown below, with inputs and exp | goToPose(pose) | Requests the robot to drive to a pose (`PoseStamped`). | | followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. | | followPath(path) | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | -| cancelNav() | Cancel an ongoing `goThroughPoses` `goToPose` or `followWaypoints` request.| -| isNavComplete() | Checks if navigation is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | -| getFeedback() | Gets feedback from navigation task, returns action server feedback object. | -| getResult() | Gets final result of navigation task, to be called after `isNavComplete` returns `True`. Returns action server result object. | +| spin(spin_dist, time_allowance) | Requests the robot to performs an in-place rotation by a given angle. | +| backup(backup_dist, backup_speed, time_allowance) | Requests the robot to back up by a given distance. | +| cancelTask() | Cancel an ongoing task request.| +| isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | +| getFeedback() | Gets feedback from task, returns action server feedback object. | +| getResult() | Gets final result of task, to be called after `isTaskComplete` returns `True`. Returns action server result object. | | getPath(start, goal) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | | getPathThroughPoses(start, goals) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | | changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | @@ -50,17 +52,17 @@ nav.setInitialPose(init_pose) nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()` ... nav.goToPose(goal_pose) -while not nav.isNavComplete(): +while not nav.isTaskComplete(): feedback = nav.getFeedback() if feedback.navigation_duration > 600: - nav.cancelNav() + nav.cancelTask() ... result = nav.getResult() -if result == NavigationResult.SUCCEEDED: +if result == TaskResult.SUCCEEDED: print('Goal succeeded!') -elif result == NavigationResult.CANCELED: +elif result == TaskResult.CANCELED: print('Goal was canceled!') -elif result == NavigationResult.FAILED: +elif result == TaskResult.FAILED: print('Goal failed!') ``` diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py new file mode 100644 index 0000000000..0d50500034 --- /dev/null +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_recoveries', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 111e54dc9d..429e11b68f 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -16,7 +16,7 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy """ @@ -72,7 +72,7 @@ def main(): # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) # Simply the current waypoint ID for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -80,18 +80,17 @@ def main(): str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points))) result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Inspection of shelves complete! Returning to start...') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Inspection of shelving was canceled. Returning to start...') - exit(1) - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Inspection of shelving failed! Returning to start...') # go back to start initial_pose.header.stamp = navigator.get_clock().now().to_msg() navigator.goToPose(initial_pose) - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 8cfcb2aec3..6a7a177acb 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -82,7 +82,7 @@ def main(): # (e.x. queue up future tasks or detect person for fine-tuned positioning) # Simply print information for workers on the robot's ETA for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -92,7 +92,7 @@ def main(): + ' seconds.') result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Got product from ' + request_item_location + '! Bringing product to shipping destination (' + request_destination + ')...') shipping_destination = PoseStamped() @@ -104,16 +104,16 @@ def main(): shipping_destination.pose.orientation.w = 0.0 navigator.goToPose(shipping_destination) - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print(f'Task at {request_item_location} was canceled. Returning to staging point...') initial_pose.header.stamp = navigator.get_clock().now().to_msg() navigator.goToPose(initial_pose) - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print(f'Task at {request_item_location} failed!') exit(-1) - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py new file mode 100644 index 0000000000..0313f956ff --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -0,0 +1,105 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# 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 geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + + +""" +Basic recoveries demo. In this demonstration, the robot navigates +to a dead-end where recoveries such as backup and spin are used +to get out of it. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = 6.13 + goal_pose.pose.position.y = 1.90 + goal_pose.pose.orientation.w = 1.0 + + navigator.goToPose(goal_pose) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print( + f'Estimated time of arrival to destination is: \ + {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9}' + ) + + # Robot hit a dead end, back it up + print('Robot hit a dead end, backing up...') + navigator.backup(backup_dist=0.5, backup_speed=0.1) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print(f'Distance traveled: {feedback.distance_traveled}') + + # Turn it around + print('Spinning robot around...') + navigator.spin(spin_dist=3.14) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print(f'Spin angle traveled: {feedback.angular_distance_traveled}') + + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Dead end confirmed! Returning to start...') + elif result == TaskResult.CANCELED: + print('Recovery was canceled. Returning to start...') + elif result == TaskResult.FAILED: + print('Recovering from dead end failed! Returning to start...') + + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + navigator.goToPose(initial_pose) + while not navigator.isTaskComplete(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index 8180bc36b4..f727cc8d72 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -16,7 +16,7 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -75,7 +75,7 @@ def main(): # Do something during our route (e.x. AI detection on camera images for anomalies) # Simply print ETA for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -86,18 +86,18 @@ def main(): # Some failure mode, must stop since the robot is clearly stuck if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0): print('Navigation has exceeded timeout of 180s, canceling request.') - navigator.cancelNav() + navigator.cancelTask() # If at end of route, reverse the route to restart security_route.reverse() result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Route complete! Restarting...') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Security route was canceled, exiting.') exit(1) - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Security route failed! Restarting from other side...') exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py index fe94f98798..084d06f08b 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy @@ -56,7 +56,7 @@ def main(): navigator.followPath(path) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -74,11 +74,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py index f4523b8974..58c47c75e1 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -89,7 +89,7 @@ def main(): navigator.goThroughPoses(goal_poses) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -106,7 +106,7 @@ def main(): # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some navigation request change to demo preemption if Duration.from_msg(feedback.navigation_time) > Duration(seconds=35.0): @@ -121,11 +121,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py index cd302e418c..58dee813c9 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -68,7 +68,7 @@ def main(): navigator.goToPose(goal_pose) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -85,7 +85,7 @@ def main(): # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some navigation request change to demo preemption if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): @@ -94,11 +94,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py index 8ed0fc3d16..9d02341a66 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -90,7 +90,7 @@ def main(): navigator.followWaypoints(goal_poses) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -107,7 +107,7 @@ def main(): # Some navigation timeout to demo cancellation if now - nav_start > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some follow waypoints request change to demo preemption if now - nav_start > Duration(seconds=35.0): @@ -124,11 +124,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index ce0ee8d01b..2a7bfff430 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -18,12 +18,14 @@ import time from action_msgs.msg import GoalStatus +from builtin_interfaces.msg import Duration +from geometry_msgs.msg import Point from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState +from nav2_msgs.action import BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import FollowWaypoints, NavigateThroughPoses, NavigateToPose -from nav2_msgs.action import FollowPath +from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes import rclpy @@ -33,8 +35,8 @@ from rclpy.qos import QoSProfile, QoSReliabilityPolicy -class NavigationResult(Enum): - UKNOWN = 0 +class TaskResult(Enum): + UNKNOWN = 0 SUCCEEDED = 1 CANCELED = 2 FAILED = 3 @@ -68,6 +70,8 @@ def __init__(self): 'compute_path_to_pose') self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses, 'compute_path_through_poses') + self.spin_client = ActionClient(self, Spin, 'spin') + self.backup_client = ActionClient(self, BackUp, 'backup') self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, 'amcl_pose', self._amclPoseCallback, @@ -157,6 +161,47 @@ def followWaypoints(self, poses): self.result_future = self.goal_handle.get_result_async() return True + def spin(self, spin_dist=1.57, time_allowance=10): + self.debug("Waiting for 'Spin' action server") + while not self.spin_client.wait_for_server(timeout_sec=1.0): + self.info("'Spin' action server not available, waiting...") + goal_msg = Spin.Goal() + goal_msg.target_yaw = spin_dist + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Spinning to angle {goal_msg.target_yaw}....') + send_goal_future = self.spin_client.send_goal_async(goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Spin request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): + self.debug("Waiting for 'Backup' action server") + while not self.backup_client.wait_for_server(timeout_sec=1.0): + self.info("'Backup' action server not available, waiting...") + goal_msg = BackUp.Goal() + goal_msg.target = Point(x=float(backup_dist)) + goal_msg.speed = backup_speed + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....') + send_goal_future = self.backup_client.send_goal_async(goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Backup request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def followPath(self, path): """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -179,16 +224,16 @@ def followPath(self, path): self.result_future = self.goal_handle.get_result_async() return True - def cancelNav(self): - """Cancel pending navigation request of any type.""" - self.info('Canceling current goal.') + def cancelTask(self): + """Cancel pending task request of any type.""" + self.info('Canceling current task.') if self.result_future: future = self.goal_handle.cancel_goal_async() rclpy.spin_until_future_complete(self, future) return - def isNavComplete(self): - """Check if the navigation request of any type is complete yet.""" + def isTaskComplete(self): + """Check if the task request of any type is complete yet.""" if not self.result_future: # task was cancelled or completed return True @@ -196,13 +241,13 @@ def isNavComplete(self): if self.result_future.result(): self.status = self.result_future.result().status if self.status != GoalStatus.STATUS_SUCCEEDED: - self.debug(f'Goal with failed with status code: {self.status}') + self.debug(f'Task with failed with status code: {self.status}') return True else: # Timed out, still processing, not complete yet return False - self.debug('Goal succeeded!') + self.debug('Task succeeded!') return True def getFeedback(self): @@ -212,13 +257,13 @@ def getFeedback(self): def getResult(self): """Get the pending action result message.""" if self.status == GoalStatus.STATUS_SUCCEEDED: - return NavigationResult.SUCCEEDED + return TaskResult.SUCCEEDED elif self.status == GoalStatus.STATUS_ABORTED: - return NavigationResult.FAILED + return TaskResult.FAILED elif self.status == GoalStatus.STATUS_CANCELED: - return NavigationResult.CANCELED + return TaskResult.CANCELED else: - return NavigationResult.UNKNOWN + return TaskResult.UNKNOWN def waitUntilNav2Active(self): """Block until the full navigation system is up and running.""" diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index 8fbdd00f11..ab19a26e47 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -32,6 +32,7 @@ 'demo_picking = nav2_simple_commander.demo_picking:main', 'demo_inspection = nav2_simple_commander.demo_inspection:main', 'demo_security = nav2_simple_commander.demo_security:main', + 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', ], }, ) From 319c8ac2c3896c1fc23f177bb3906c95f0d9cfd2 Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Mon, 7 Feb 2022 19:01:54 -0500 Subject: [PATCH 05/22] added planner_id (#2806) --- .../nav2_simple_commander/robot_navigator.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 2a7bfff430..009314442c 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -273,7 +273,7 @@ def waitUntilNav2Active(self): self.info('Nav2 is ready for use!') return - def getPath(self, start, goal): + def getPath(self, start, goal, planner_id=None): """Send a `ComputePathToPose` action request.""" self.debug("Waiting for 'ComputePathToPose' action server") while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0): @@ -282,6 +282,8 @@ def getPath(self, start, goal): goal_msg = ComputePathToPose.Goal() goal_msg.goal = goal goal_msg.start = start + if planner_id is not None: + goal_msg.planner_id = planner_id self.info('Getting path...') send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg) From 0ecfa57fe87d579a54bc084a22bd7de921155b68 Mon Sep 17 00:00:00 2001 From: janx8r <95029851+janx8r@users.noreply.github.com> Date: Thu, 10 Feb 2022 13:35:17 +0100 Subject: [PATCH 06/22] Fixing the issue #2781: raytraceLine with same start and end point (#2784) * Fixing the issue #2781: raytraceLine with same start and end point no longer causes segmentation fault * Some whitespace modifications to make the code pass release_test * Add testcase for raytraceLine the same point Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> --- .../include/nav2_voxel_grid/voxel_grid.hpp | 22 ++++++++++----- .../test/voxel_grid_bresenham_3d.cpp | 27 +++++++++++++++++++ 2 files changed, 43 insertions(+), 6 deletions(-) diff --git a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp index 9e5ea94101..baff10d684 100644 --- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp +++ b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp @@ -232,12 +232,22 @@ class VoxelGrid if ((unsigned int)(dist) < min_length) { return; } - double scale = std::min(1.0, max_length / dist); - - // Updating starting point to the point at distance min_length from the initial point - double min_x0 = x0 + (x1 - x0) / dist * min_length; - double min_y0 = y0 + (y1 - y0) / dist * min_length; - double min_z0 = z0 + (z1 - z0) / dist * min_length; + double scale, min_x0, min_y0, min_z0; + if (dist > 0.0) { + scale = std::min(1.0, max_length / dist); + + // Updating starting point to the point at distance min_length from the initial point + min_x0 = x0 + (x1 - x0) / dist * min_length; + min_y0 = y0 + (y1 - y0) / dist * min_length; + min_z0 = z0 + (z1 - z0) / dist * min_length; + } else { + // dist can be 0 if [x0, y0, z0]==[x1, y1, z1]. + // In this case only this voxel should be processed. + scale = 1.0; + min_x0 = x0; + min_y0 = y0; + min_z0 = z0; + } int dx = int(x1) - int(min_x0); // NOLINT int dy = int(y1) - int(min_y0); // NOLINT diff --git a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp index 10ff23dd87..49aa5830f0 100644 --- a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp +++ b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp @@ -49,6 +49,10 @@ class TestVoxel ASSERT_TRUE(off < size_); data_[off] = val; } + inline unsigned int operator()(unsigned int off) + { + return data_[off]; + } private: uint32_t * data_; @@ -122,6 +126,29 @@ TEST(voxel_grid, bresenham3DBoundariesCheck) } } +TEST(voxel_grid, bresenham3DSamePoint) +{ + const int sz_x = 60; + const int sz_y = 60; + const int sz_z = 2; + const unsigned int max_length = 60; + const unsigned int min_length = 0; + nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z); + TestVoxel tv(vg.getData(), sz_x, sz_y); + + // Initial point + const double x0 = 2.2; + const double y0 = 3.8; + const double z0 = 0.4; + + unsigned int offset = int(y0) * sz_x + int(x0); + unsigned int val_before = tv(offset); + // Same point to check + vg.raytraceLine(tv, x0, y0, z0, x0, y0, z0, max_length, min_length); + unsigned int val_after = tv(offset); + ASSERT_FALSE(val_before == val_after); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 996adce7d051e80ae9e633d3735c4d7f557a71c3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 12 Feb 2022 23:49:50 +0100 Subject: [PATCH 07/22] Add optional node names to wait (#2811) * add option to specify navigator and localizer to wait for * add docs for waituntilNav2Active * wait for pose only for amcl * revert order --- nav2_simple_commander/README.md | 2 +- .../nav2_simple_commander/robot_navigator.py | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 3a368511f2..3174f89a21 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -33,7 +33,7 @@ The methods provided by the basic navigator are shown below, with inputs and exp | clearGlobalCostmap() | Clears the global costmap. | | getGlobalCostmap() | Returns the global costmap, `nav2_msgs/Costmap` | | getLocalCostmap() | Returns the local costmap, `nav2_msgs/Costmap` | -| waitUntilNav2Active() | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. | +| waitUntilNav2Active(navigator='bt_navigator, localizer='amcl') | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. Custom navigator and localizer nodes can be specified | | lifecycleStartup() | Sends a request to all lifecycle management servers to bring them into the active state, to be used if autostart is `false` and you want this program to control Nav2's lifecycle. | | lifecycleShutdown() | Sends a request to all lifecycle management servers to shut them down. | diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 009314442c..e3071d1b5b 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -265,11 +265,12 @@ def getResult(self): else: return TaskResult.UNKNOWN - def waitUntilNav2Active(self): + def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" - self._waitForNodeToActivate('amcl') - self._waitForInitialPose() - self._waitForNodeToActivate('bt_navigator') + self._waitForNodeToActivate(localizer) + if localizer == 'amcl': + self._waitForInitialPose() + self._waitForNodeToActivate(navigator) self.info('Nav2 is ready for use!') return From b45f62627fca56d4a5974c5adda779c8844f1d60 Mon Sep 17 00:00:00 2001 From: Andrii Maistruk <71632363+andriimaistruk@users.noreply.github.com> Date: Tue, 15 Feb 2022 20:56:39 +0200 Subject: [PATCH 08/22] remove resizing on update message callback (#2818) --- nav2_costmap_2d/plugins/static_layer.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 37a5cef248..44598ff0a6 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -317,11 +317,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u costmap_[index] = interpretValue(update->data[di++]); } } - - x_ = update->x; - y_ = update->y; - width_ = update->width; - height_ = update->height; + has_updated_data_ = true; } From 51c67cad08219d8db5cb711fc63b7f4977542e4f Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Sun, 20 Feb 2022 23:58:01 +0000 Subject: [PATCH 09/22] restrict test_msgs to test_dependency (#2827) Signed-off-by: Alberto Soragna --- nav2_behavior_tree/package.xml | 1 + nav2_behavior_tree/test/plugins/action/CMakeLists.txt | 4 +++- nav2_util/CMakeLists.txt | 1 - nav2_util/package.xml | 2 +- nav2_util/test/CMakeLists.txt | 3 ++- 5 files changed, 7 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index d94d419de1..b2f60f12ac 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -51,6 +51,7 @@ ament_lint_common ament_lint_auto ament_cmake_gtest + test_msgs ament_cmake diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 1f69b5fce3..a482c94673 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -1,5 +1,7 @@ +find_package(test_msgs REQUIRED) + ament_add_gtest(test_bt_action_node test_bt_action_node.cpp) -ament_target_dependencies(test_bt_action_node ${dependencies}) +ament_target_dependencies(test_bt_action_node ${dependencies} test_msgs) ament_add_gtest(test_action_spin_action test_spin_action.cpp) target_link_libraries(test_action_spin_action nav2_spin_action_bt_node) diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 930034f09c..a3999f2aed 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -12,7 +12,6 @@ find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(rclcpp_action REQUIRED) -find_package(test_msgs REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(bondcpp REQUIRED) diff --git a/nav2_util/package.xml b/nav2_util/package.xml index edc8822ed4..5abbe72399 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -24,7 +24,6 @@ bondcpp bond rclcpp_action - test_msgs rclcpp_lifecycle launch launch_testing_ament_cmake @@ -38,6 +37,7 @@ launch launch_testing_ament_cmake std_srvs + test_msgs action_msgs launch_testing_ros ament_cmake_pytest diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 4d3b5511dc..42e6efebb0 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -3,7 +3,8 @@ ament_add_gtest(test_execution_timer test_execution_timer.cpp) ament_add_gtest(test_node_utils test_node_utils.cpp) target_link_libraries(test_node_utils ${library_name}) -find_package(std_srvs) +find_package(std_srvs REQUIRED) +find_package(test_msgs REQUIRED) ament_add_gtest(test_service_client test_service_client.cpp) ament_target_dependencies(test_service_client std_srvs) From 36a0104becfd81d2ca59ea70eba2d17f9bffcbab Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Tue, 1 Mar 2022 01:20:18 +0000 Subject: [PATCH 10/22] remove unused odometry smoother in bt navigator (#2829) * remove unused odometry smoother in bt navigator Signed-off-by: Alberto Soragna * reorganize bt navigator to shared odom_smoother object with servers Signed-off-by: Alberto Soragna --- .../include/nav2_bt_navigator/bt_navigator.hpp | 2 +- .../include/nav2_bt_navigator/navigator.hpp | 13 ++++++++++--- .../navigators/navigate_through_poses.hpp | 6 ++++-- .../navigators/navigate_to_pose.hpp | 6 ++++-- nav2_bt_navigator/src/bt_navigator.cpp | 10 +++++----- .../src/navigators/navigate_through_poses.cpp | 5 +++-- .../src/navigators/navigate_to_pose.cpp | 5 +++-- 7 files changed, 30 insertions(+), 17 deletions(-) diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 076a4c8f37..b83a84fce3 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -89,7 +89,7 @@ class BtNavigator : public nav2_util::LifecycleNode nav2_bt_navigator::NavigatorMuxer plugin_muxer_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; // Metrics for feedback std::string robot_frame_; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp index 68abf0830f..f1b36808c6 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp @@ -135,13 +135,15 @@ class Navigator * @param feedback_utils Some utilities useful for navigators to have * @param plugin_muxer The muxing object to ensure only one navigator * can be active at a time + * @param odom_smoother Object to get current smoothed robot's speed * @return bool If successful */ bool on_configure( rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, const std::vector & plugin_lib_names, const FeedbackUtils & feedback_utils, - nav2_bt_navigator::NavigatorMuxer * plugin_muxer) + nav2_bt_navigator::NavigatorMuxer * plugin_muxer, + std::shared_ptr odom_smoother) { auto node = parent_node.lock(); logger_ = node->get_logger(); @@ -173,7 +175,7 @@ class Navigator blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT - return configure(parent_node) && ok; + return configure(parent_node, odom_smoother) && ok; } /** @@ -284,7 +286,12 @@ class Navigator /** * @param Method to configure resources. */ - virtual bool configure(rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/) {return true;} + virtual bool configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/ + std::shared_ptr /*odom_smoother*/) + { + return true; + } /** * @brief Method to cleanup resources. diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index d86736bad1..eb0b204515 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -51,9 +51,11 @@ class NavigateThroughPosesNavigator /** * @brief A configure state transition to configure navigator's state * @param node Weakptr to the lifecycle node + * @param odom_smoother Object to get current smoothed robot's speed */ bool configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + rclcpp_lifecycle::LifecycleNode::WeakPtr node, + std::shared_ptr odom_smoother) override; /** * @brief Get action name for this navigator @@ -106,7 +108,7 @@ class NavigateThroughPosesNavigator std::string path_blackboard_id_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 9773ef65c3..00dfeaffdb 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -50,9 +50,11 @@ class NavigateToPoseNavigator /** * @brief A configure state transition to configure navigator's state * @param node Weakptr to the lifecycle node + * @param odom_smoother Object to get current smoothed robot's speed */ bool configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + rclcpp_lifecycle::LifecycleNode::WeakPtr node, + std::shared_ptr odom_smoother) override; /** * @brief A cleanup state transition to remove memory allocated @@ -122,7 +124,7 @@ class NavigateToPoseNavigator std::string path_blackboard_id_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 0bdfed8382..f50a31cdce 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -107,21 +107,21 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) feedback_utils.robot_frame = robot_frame_; feedback_utils.transform_tolerance = transform_tolerance_; + // Odometry smoother object for getting current speed + odom_smoother_ = std::make_shared(shared_from_this(), 0.3, odom_topic_) + if (!pose_navigator_->on_configure( - shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_)) + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)) { return nav2_util::CallbackReturn::FAILURE; } if (!poses_navigator_->on_configure( - shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_)) + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)) { return nav2_util::CallbackReturn::FAILURE; } - // Odometry smoother object for getting current speed - odom_smoother_ = std::make_unique(shared_from_this(), 0.3, odom_topic_); - return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 20e7d1b139..f07d851e2d 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -24,7 +24,8 @@ namespace nav2_bt_navigator bool NavigateThroughPosesNavigator::configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + std::shared_ptr odom_smoother) { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); @@ -36,7 +37,7 @@ NavigateThroughPosesNavigator::configure( path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); // Odometry smoother object for getting current speed - odom_smoother_ = std::make_unique(node, 0.3); + odom_smoother_ = odom_smoother; return true; } diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index de0422e103..4bc13de1e1 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -24,7 +24,8 @@ namespace nav2_bt_navigator bool NavigateToPoseNavigator::configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + std::shared_ptr odom_smoother) { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); @@ -36,7 +37,7 @@ NavigateToPoseNavigator::configure( path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); // Odometry smoother object for getting current speed - odom_smoother_ = std::make_unique(node, 0.3); + odom_smoother_ = odom_smoother; self_client_ = rclcpp_action::create_client(node, getName()); From 09643c88acc5ee899bd690c9ad0d4780dafa46e1 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 2 Mar 2022 00:13:29 +0100 Subject: [PATCH 11/22] Add all action options (#2834) * added options * update docs --- nav2_simple_commander/README.md | 14 +++++------ .../nav2_simple_commander/robot_navigator.py | 24 ++++++++++++------- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 3174f89a21..177ce667f9 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -15,18 +15,18 @@ The methods provided by the basic navigator are shown below, with inputs and exp | Robot Navigator Method | Description | | --------------------------------- | -------------------------------------------------------------------------- | | setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. | -| goThroughPoses(poses) | Requests the robot to drive through a set of poses (list of `PoseStamped`).| -| goToPose(pose) | Requests the robot to drive to a pose (`PoseStamped`). | +| goThroughPoses(poses, behavior_tree='') | Requests the robot to drive through a set of poses (list of `PoseStamped`).| +| goToPose(pose, behavior_tree='') | Requests the robot to drive to a pose (`PoseStamped`). | | followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. | -| followPath(path) | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | -| spin(spin_dist, time_allowance) | Requests the robot to performs an in-place rotation by a given angle. | -| backup(backup_dist, backup_speed, time_allowance) | Requests the robot to back up by a given distance. | +| followPath(path, controller_id='', goal_checker_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| spin(spin_dist=1.57, time_allowance=10) | Requests the robot to performs an in-place rotation by a given angle. | +| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10) | Requests the robot to back up by a given distance. | | cancelTask() | Cancel an ongoing task request.| | isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | | getFeedback() | Gets feedback from task, returns action server feedback object. | | getResult() | Gets final result of task, to be called after `isTaskComplete` returns `True`. Returns action server result object. | -| getPath(start, goal) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | -| getPathThroughPoses(start, goals) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | +| getPath(start, goal, planner_id='', use_start=False) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| getPathThroughPoses(start, goals, planner_id='', use_start=False) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | | changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | | clearAllCostmaps() | Clears both the global and local costmaps. | | clearLocalCostmap() | Clears the local costmap. | diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index e3071d1b5b..4bcf8093d6 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -93,7 +93,7 @@ def setInitialPose(self, initial_pose): self.initial_pose = initial_pose self._setInitialPose() - def goThroughPoses(self, poses): + def goThroughPoses(self, poses, behavior_tree=''): """Send a `NavThroughPoses` action request.""" self.debug("Waiting for 'NavigateThroughPoses' action server") while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0): @@ -101,6 +101,7 @@ def goThroughPoses(self, poses): goal_msg = NavigateThroughPoses.Goal() goal_msg.poses = poses + goal_msg.behavior_tree = behavior_tree self.info(f'Navigating with {len(goal_msg.poses)} goals....') send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg, @@ -115,7 +116,7 @@ def goThroughPoses(self, poses): self.result_future = self.goal_handle.get_result_async() return True - def goToPose(self, pose): + def goToPose(self, pose, behavior_tree=''): """Send a `NavToPose` action request.""" self.debug("Waiting for 'NavigateToPose' action server") while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0): @@ -123,6 +124,7 @@ def goToPose(self, pose): goal_msg = NavigateToPose.Goal() goal_msg.pose = pose + goal_msg.behavior_tree = behavior_tree self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' + str(pose.pose.position.y) + '...') @@ -202,7 +204,7 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True - def followPath(self, path): + def followPath(self, path, controller_id='', goal_checker_id=''): """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") while not self.follow_path_client.wait_for_server(timeout_sec=1.0): @@ -210,6 +212,8 @@ def followPath(self, path): goal_msg = FollowPath.Goal() goal_msg.path = path + goal_msg.controller_id = controller_id + goal_msg.goal_checker_id = goal_checker_id self.info('Executing path...') send_goal_future = self.follow_path_client.send_goal_async(goal_msg, @@ -274,17 +278,17 @@ def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): self.info('Nav2 is ready for use!') return - def getPath(self, start, goal, planner_id=None): + def getPath(self, start, goal, planner_id='', use_start=False): """Send a `ComputePathToPose` action request.""" self.debug("Waiting for 'ComputePathToPose' action server") while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathToPose' action server not available, waiting...") goal_msg = ComputePathToPose.Goal() - goal_msg.goal = goal goal_msg.start = start - if planner_id is not None: - goal_msg.planner_id = planner_id + goal_msg.goal = goal + goal_msg.planner_id = planner_id + goal_msg.use_start = use_start self.info('Getting path...') send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg) @@ -304,15 +308,17 @@ def getPath(self, start, goal, planner_id=None): return self.result_future.result().result.path - def getPathThroughPoses(self, start, goals): + def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): """Send a `ComputePathThroughPoses` action request.""" self.debug("Waiting for 'ComputePathThroughPoses' action server") while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathThroughPoses' action server not available, waiting...") goal_msg = ComputePathThroughPoses.Goal() - goal_msg.goals = goals goal_msg.start = start + goal_msg.goals = goals + goal_msg.planner_id = planner_id + goal_msg.use_start = use_start self.info('Getting path...') send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) From 4219a1431ca9b152cbb4d0b17cb61e856bc9c5d7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 10 Mar 2022 12:21:16 -0800 Subject: [PATCH 12/22] Adding theta* to the main packages list --- navigation2/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/navigation2/package.xml b/navigation2/package.xml index ed2d8537e1..5e55723049 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -33,6 +33,7 @@ nav2_smac_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller + nav2_theta_star_planner ament_cmake From a4d69060e3083f9d881aac54f2dc969e9bfe065a Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Mon, 21 Mar 2022 16:47:12 +0100 Subject: [PATCH 13/22] Fix: bt_navigator crashes on lc transitions (#2848) * fix empty address access on halt all actions * fix unsafe declaration of parameters * restore odom smoother * fix styling issues * add missing semicolumn --- .../src/behavior_tree_engine.cpp | 4 ++++ .../src/navigators/navigate_through_poses.cpp | 24 +++++++++++++------ .../src/navigators/navigate_to_pose.cpp | 24 +++++++++++++------ 3 files changed, 38 insertions(+), 14 deletions(-) diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index e689cae927..076223d286 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -108,6 +108,10 @@ BehaviorTreeEngine::resetGrootMonitor() void BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node) { + if (!root_node) { + return; + } + // this halt signal should propagate through the entire tree. root_node->halt(); diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index f07d851e2d..462b1da6c5 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -29,11 +29,17 @@ NavigateThroughPosesNavigator::configure( { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - node->declare_parameter("goals_blackboard_id", std::string("goals")); + + if (!node->has_parameter("goals_blackboard_id")) { + node->declare_parameter("goals_blackboard_id", std::string("goals")); + } + goals_blackboard_id_ = node->get_parameter("goals_blackboard_id").as_string(); + if (!node->has_parameter("path_blackboard_id")) { node->declare_parameter("path_blackboard_id", std::string("path")); } + path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); // Odometry smoother object for getting current speed @@ -48,12 +54,16 @@ NavigateThroughPosesNavigator::getDefaultBTFilepath( { std::string default_bt_xml_filename; auto node = parent_node.lock(); - std::string pkg_share_dir = - ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - node->declare_parameter( - "default_nav_through_poses_bt_xml", - pkg_share_dir + - "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml"); + + if (!node->has_parameter("default_nav_through_poses_bt_xml")) { + std::string pkg_share_dir = + ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + node->declare_parameter( + "default_nav_through_poses_bt_xml", + pkg_share_dir + + "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml"); + } + node->get_parameter("default_nav_through_poses_bt_xml", default_bt_xml_filename); return default_bt_xml_filename; diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 4bc13de1e1..cf1d1970fc 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -29,11 +29,17 @@ NavigateToPoseNavigator::configure( { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - node->declare_parameter("goal_blackboard_id", std::string("goal")); + + if (!node->has_parameter("goal_blackboard_id")) { + node->declare_parameter("goal_blackboard_id", std::string("goal")); + } + goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string(); + if (!node->has_parameter("path_blackboard_id")) { node->declare_parameter("path_blackboard_id", std::string("path")); } + path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); // Odometry smoother object for getting current speed @@ -54,12 +60,16 @@ NavigateToPoseNavigator::getDefaultBTFilepath( { std::string default_bt_xml_filename; auto node = parent_node.lock(); - std::string pkg_share_dir = - ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - node->declare_parameter( - "default_nav_to_pose_bt_xml", - pkg_share_dir + - "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"); + + if (!node->has_parameter("default_nav_to_pose_bt_xml")) { + std::string pkg_share_dir = + ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + node->declare_parameter( + "default_nav_to_pose_bt_xml", + pkg_share_dir + + "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"); + } + node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename); return default_bt_xml_filename; From 11b19793bc35b9fc5a2f217953db646d1e0147b6 Mon Sep 17 00:00:00 2001 From: Hossein Sheikhi Darani <64957461+HosseinSheikhi@users.noreply.github.com> Date: Mon, 21 Mar 2022 08:50:23 -0700 Subject: [PATCH 14/22] fix-collision checker must capture lethal before unknow (#2854) --- nav2_costmap_2d/src/footprint_collision_checker.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 51c1fe7841..f51ec286be 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -94,13 +94,13 @@ double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { point_cost = pointCost(line.getX(), line.getY()); // Score the current point - if (line_cost < point_cost) { - line_cost = point_cost; + // if in collision, no need to continue + if (point_cost == static_cast(LETHAL_OBSTACLE)) { + return point_cost; } - // if in collision, no need to continue - if (line_cost == static_cast(LETHAL_OBSTACLE)) { - return line_cost; + if (line_cost < point_cost) { + line_cost = point_cost; } } From 9d01514bf7208aeb87ff7d818efc9e1df90f4a5b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 23 Mar 2022 10:28:08 -0700 Subject: [PATCH 15/22] Removing old unused function and comment (#2863) --- nav2_core/include/nav2_core/recovery.hpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index a800b2d21f..42646824be 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/recovery.hpp @@ -65,15 +65,6 @@ class Recovery * @brief Method to deactive recovery and any threads involved in execution. */ virtual void deactivate() = 0; - - /** - * @brief Method Execute recovery behavior - * @param name The name of this planner - * @return true if successful, false is failed to execute fully - */ - // TODO(stevemacenski) evaluate design for recoveries to not host - // their own servers and utilize a recovery server exposed action. - // virtual bool executeRecovery() = 0; }; } // namespace nav2_core From c8d498d8c7899b9968073d8b222b9be9db60c38d Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Mon, 4 Apr 2022 13:52:53 -0400 Subject: [PATCH 16/22] Better Costmap Error Message (#2884) * Better Costmap Error Message * PR Feedback --- nav2_costmap_2d/plugins/obstacle_layer.cpp | 8 ++++++-- nav2_costmap_2d/plugins/voxel_layer.cpp | 8 ++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 0e33aafb3b..071543b8c8 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -569,8 +569,12 @@ ObstacleLayer::raytraceFreespace( if (!worldToMap(ox, oy, x0, y0)) { RCLCPP_WARN( logger_, - "Sensor origin at (%.2f, %.2f) is out of map bounds. The costmap cannot raytrace for it.", - ox, oy); + "Sensor origin at (%.2f, %.2f) is out of map bounds (%.2f, %.2f) to (%.2f, %.2f). " + "The costmap cannot raytrace for it.", + ox, oy, + origin_x_, origin_y_, + origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY(), + ); return; } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 635f4babb0..0e6fad2de1 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -318,8 +318,12 @@ void VoxelLayer::raytraceFreespace( if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { RCLCPP_WARN( logger_, - "Sensor origin: (%.2f, %.2f, %.2f), out of map bounds. The costmap can't raytrace for it.", - ox, oy, oz); + "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). " + "The costmap cannot raytrace for it.", + ox, oy, oz, + ox + getSizeInMetersX(), oy + getSizeInMetersY(), oz + getSizeInMetersZ(), + origin_x_, origin_y_, origin_z_); + return; } From b154456be05c631fd88b9ba05725afc4a1db8b34 Mon Sep 17 00:00:00 2001 From: "G.Doisy" Date: Thu, 7 Apr 2022 20:20:16 +0200 Subject: [PATCH 17/22] add getRobotRadius() in costmap_2d_ros (#2896) --- .../include/nav2_costmap_2d/costmap_2d_ros.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index babf691fda..838f20724e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -294,6 +294,14 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ bool getUseRadius() {return use_radius_;} + /** + * @brief Get the costmap's robot_radius_ parameter, corresponding to + * raidus of the robot footprint when it is defined as as circle + * (i.e. when use_radius_ == true). + * @return robot_radius_ + */ + double getRobotRadius() {return robot_radius_;} + protected: rclcpp::Node::SharedPtr client_node_; From 974512a487776aea2cbeb5d63c725a9603fe2b3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Andr=C3=A9s=20=C3=81lvarez=20Restrepo?= Date: Thu, 7 Apr 2022 18:28:36 -0400 Subject: [PATCH 18/22] Add clock time to costmaps (#2899) --- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index fde37a3815..906a9f0324 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -126,7 +126,7 @@ void Costmap2DPublisher::prepareGrid() grid_ = std::make_unique(); grid_->header.frame_id = global_frame_; - grid_->header.stamp = rclcpp::Time(); + grid_->header.stamp = clock_->now(); grid_->info.resolution = grid_resolution; From e983b7d38c05e115b2917e61ca8364cb5561876b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 11 Apr 2022 17:10:03 -0700 Subject: [PATCH 19/22] update goal checker plugin to plugins (#2909) --- nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml | 2 +- nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml | 2 +- nav2_regulated_pure_pursuit_controller/README.md | 2 +- nav2_rotation_shim_controller/README.md | 2 +- nav2_system_tests/src/costmap_filters/keepout_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/speed_global_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/speed_local_params.yaml | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index bfedd2ce78..8eb299a25a 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -108,7 +108,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 2193065bba..cbbb149c6b 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -108,7 +108,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 84a40e8a07..fa3ff9638d 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -83,7 +83,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] progress_checker: diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 3c03186452..15185bfa96 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -47,7 +47,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] progress_checker: diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 666a1b9378..26a4c66d17 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -99,7 +99,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 71fa2a02a1..21be55c9e8 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -100,7 +100,7 @@ controller_server: min_theta_velocity_threshold: 0.001 speed_limit_topic: "/speed_limit" progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 1dd66e8fc6..d103568a39 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -100,7 +100,7 @@ controller_server: min_theta_velocity_threshold: 0.001 speed_limit_topic: "/speed_limit" progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters From 6471778ee59bfa32ee2b5dc32929657270f94cc2 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Sat, 16 Apr 2022 01:43:33 +0200 Subject: [PATCH 20/22] Allow usage of std::string in searchAndGetParam() (#2918) * Allow usage of std::string in searchAndGetParam() Removed also old TODO related to legacy ROS API * Fix --- .../nav_2d_utils/include/nav_2d_utils/parameters.hpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index 5951bd09ef..c3c30428fe 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp @@ -64,15 +64,7 @@ param_t searchAndGetParam( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & param_name, const param_t & default_value) { - // TODO(crdelsey): Handle searchParam - // std::string resolved_name; - // if (nh->searchParam(param_name, resolved_name)) - // { - // param_t value = 0; - // nh->param(resolved_name, value, default_value); - // return value; - // } - param_t value = 0; + param_t value; nav2_util::declare_parameter_if_not_declared( nh, param_name, rclcpp::ParameterValue(default_value)); From c71224454fb4e12d50228c00a76ce12765352fb9 Mon Sep 17 00:00:00 2001 From: "M. Mostafa Farzan" Date: Wed, 27 Apr 2022 22:12:34 +0430 Subject: [PATCH 21/22] Clean up action clients in nav2_simple_commander (#2924) * Clean up action clients in nav2_simple_commander * Add camelCase version * Add docs --- nav2_simple_commander/README.md | 1 + .../nav2_simple_commander/robot_navigator.py | 15 +++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 177ce667f9..a8f0b4de45 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -36,6 +36,7 @@ The methods provided by the basic navigator are shown below, with inputs and exp | waitUntilNav2Active(navigator='bt_navigator, localizer='amcl') | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. Custom navigator and localizer nodes can be specified | | lifecycleStartup() | Sends a request to all lifecycle management servers to bring them into the active state, to be used if autostart is `false` and you want this program to control Nav2's lifecycle. | | lifecycleShutdown() | Sends a request to all lifecycle management servers to shut them down. | +| destroyNode() | Releases the resources used by the object. | A general template for building applications is as follows: diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 4bcf8093d6..24307b97d8 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -87,6 +87,21 @@ def __init__(self): self.get_costmap_global_srv = self.create_client(GetCostmap, '/global_costmap/get_costmap') self.get_costmap_local_srv = self.create_client(GetCostmap, '/local_costmap/get_costmap') + def destroyNode(self): + self.destroy_node() + + def destroy_node(self): + self.nav_through_poses_client.destroy() + self.nav_to_pose_client.destroy() + self.follow_waypoints_client.destroy() + self.follow_path_client.destroy() + self.compute_path_to_pose_client.destroy() + self.compute_path_through_poses_client.destroy() + self.smoother_client.destroy() + self.spin_client.destroy() + self.backup_client.destroy() + super().destroy_node() + def setInitialPose(self, initial_pose): """Set the initial pose to the localization system.""" self.initial_pose_received = False From 7378f9f7830d1355d230b49f6b1f532f53688c5c Mon Sep 17 00:00:00 2001 From: mrmara <48493979+mrmara@users.noreply.github.com> Date: Thu, 5 May 2022 19:11:07 +0200 Subject: [PATCH 22/22] Added mutex to prevent SEGFAULT on map change in AMCL (#2933) * Added mutex to prevent SEGFAULT on map change * Removing redundant mutex Co-authored-by: Antonio Marangi --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 3 +-- nav2_amcl/src/amcl_node.cpp | 9 +++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 723a243a4a..65b40ddb9f 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -130,7 +130,7 @@ class AmclNode : public nav2_util::LifecycleNode bool first_map_only_{true}; std::atomic first_map_received_{false}; amcl_hyp_t * initial_pose_hyp_; - std::recursive_mutex configuration_mutex_; + std::recursive_mutex mutex_; rclcpp::Subscription::ConstSharedPtr map_sub_; #if NEW_UNIFORM_SAMPLING static std::vector> free_space_indices; @@ -238,7 +238,6 @@ class AmclNode : public nav2_util::LifecycleNode */ static pf_vector_t uniformPoseGenerator(void * arg); pf_t * pf_{nullptr}; - std::mutex pf_mutex_; bool pf_init_; pf_vector_t pf_odom_pose_; int resample_count_{0}; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index c2a46b4f87..8be37dbeb1 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -504,7 +504,7 @@ AmclNode::globalLocalizationCallback( const std::shared_ptr/*req*/, std::shared_ptr/*res*/) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO(get_logger(), "Initializing with uniform distribution"); @@ -530,7 +530,7 @@ AmclNode::nomotionUpdateCallback( void AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO(get_logger(), "initialPoseReceived"); @@ -565,6 +565,7 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha void AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg) { + std::lock_guard cfl(mutex_); // In case the client sent us a pose estimate in the past, integrate the // intervening odometric change. geometry_msgs::msg::TransformStamped tx_odom; @@ -629,7 +630,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg) void AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(mutex_); // Since the sensor data is continually being published by the simulator or robot, // we don't want our callbacks to fire until we're in the active state @@ -1166,7 +1167,7 @@ AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) void AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg) { - std::lock_guard cfl(configuration_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO( get_logger(), "Received a %d X %d map @ %.3f m/pix",