diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 2ded6e5136e..1851c84f7b9 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.3.4 + 1.3.5

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 505a7895c51..357903df7dc 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -194,6 +194,10 @@ class BtActionNode : public BT::ActionNodeBase // reset the flag to send the goal or not, allowing the user the option to set it in on_tick should_send_goal_ = true; + // Clear the input and output messages to make sure we have no leftover from previous calls + goal_ = typename ActionT::Goal(); + result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); + // user defined callback, may modify "should_send_goal_". on_tick(); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 746ef610494..a774db43842 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -138,6 +138,9 @@ class BtServiceNode : public BT::ActionNodeBase // allowing the user the option to set it in on_tick should_send_request_ = true; + // Clear the input request to make sure we have no leftover from previous calls + request_ = std::make_shared(); + // user defined callback, may modify "should_send_request_". on_tick(); diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 6c152f16c0a..7db46aede36 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.3.4 + 1.3.5 Nav2 behavior tree wrappers, nodes, and utilities Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 0b74a044629..133d45e1ec7 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -102,7 +102,7 @@ class DriveOnHeading : public TimedBehavior RCLCPP_WARN( this->logger_, "Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading"); - return ResultStatus{Status::FAILED, ActionT::Result::NONE}; + return ResultStatus{Status::FAILED, ActionT::Result::TIMEOUT}; } geometry_msgs::msg::PoseStamped current_pose; diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index af5c5abe15a..f2dfdae3e43 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.3.4 + 1.3.5 Nav2 behavior server Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index ae826003a2e..7f61c9c6c0c 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.3.4 + 1.3.5 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/doc/parallel_w_recovery.png b/nav2_bt_navigator/doc/parallel_w_recovery.png index e67f3f77a53..a05c53d86b1 100644 Binary files a/nav2_bt_navigator/doc/parallel_w_recovery.png and b/nav2_bt_navigator/doc/parallel_w_recovery.png differ diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 54ffcb6e6ee..d3bc7924c4d 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.3.4 + 1.3.5 Nav2 BT Navigator Server Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 29220e934bf..94c5bc5610d 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.3.4 + 1.3.5 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 1d92b77acc7..c49482cec59 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.3.4 + 1.3.5 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 8d9e3e5113a..5cf083fe56f 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.3.4 + 1.3.5 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index e0595725030..192550dad01 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.3.4 + 1.3.5 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 3ee8d8a5595..7b1db907f65 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.3.4 + 1.3.5 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 1ff54d88299..3a76f806262 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.3.4 + 1.3.5 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index eb3711eb476..6b45c77e089 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -2,7 +2,7 @@ opennav_docking - 1.3.4 + 1.3.5 A Task Server for robot charger docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index c0e45ee4223..ec2a1de9ab9 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -144,7 +144,10 @@ bool Controller::isTrajectoryCollisionFree( } // Generate path - for (double t = 0; t < projection_time_; t += simulation_time_step_) { + double distance = std::numeric_limits::max(); + unsigned int max_iter = static_cast(ceil(projection_time_ / simulation_time_step_)); + + do{ // Apply velocities to calculate next pose next_pose.pose = control_law_->calculateNextPose( simulation_time_step_, target_pose, next_pose.pose, backward); @@ -177,7 +180,10 @@ bool Controller::isTrajectoryCollisionFree( trajectory_pub_->publish(trajectory); return false; } - } + + // Check if we reach the goal + distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose); + }while(distance > 1e-2 && trajectory.poses.size() < max_iter); trajectory_pub_->publish(trajectory); diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 5b76f6095ce..4874d771201 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -19,12 +19,18 @@ from action_msgs.msg import GoalStatus from geometry_msgs.msg import TransformStamped, Twist from launch import LaunchDescription +# from launch.actions import SetEnvironmentVariable from launch_ros.actions import Node import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.markers +import launch_testing.util from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot import pytest import rclpy -from rclpy.action import ActionClient, ActionServer +from rclpy.action.client import ActionClient +from rclpy.action.server import ActionServer from sensor_msgs.msg import BatteryState from tf2_ros import TransformBroadcaster @@ -32,10 +38,18 @@ # This test can be run standalone with: # python3 -u -m pytest test_docking_server.py -s +# If python3-flaky is installed, you can run the test multiple times to +# try to identify flaky ness. +# python3 -u -m pytest --force-flaky --min-passes 3 --max-runs 5 -s -v test_docking_server.py + @pytest.mark.rostest +# @pytest.mark.flaky +# @pytest.mark.flaky(max_runs=5, min_passes=3) def generate_test_description(): return LaunchDescription([ + # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + # SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), Node( package='opennav_docking', executable='opennav_docking', @@ -74,21 +88,26 @@ class TestDockingServer(unittest.TestCase): @classmethod def setUpClass(cls): rclpy.init() - # Latest odom -> base_link - cls.x = 0.0 - cls.y = 0.0 - cls.theta = 0.0 - # Track charge state - cls.is_charging = False - # Latest command velocity - cls.command = Twist() - cls.node = rclpy.create_node('test_docking_server') @classmethod def tearDownClass(cls): - cls.node.destroy_node() rclpy.shutdown() + def setUp(self): + # Create a ROS node for tests + # Latest odom -> base_link + self.x = 0.0 + self.y = 0.0 + self.theta = 0.0 + # Track charge state + self.is_charging = False + # Latest command velocity + self.command = Twist() + self.node = rclpy.create_node('test_docking_server') + + def tearDown(self): + self.node.destroy_node() + def command_velocity_callback(self, msg): self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z)) self.command = msg @@ -175,12 +194,19 @@ def test_docking_server(self): 'navigate_to_pose', self.nav_execute_callback) - # Spin once so that TF is published - rclpy.spin_once(self.node, timeout_sec=0.1) + # Publish transform + self.publish() + + # Run for 1 seconds to allow tf to propogate + for _ in range(10): + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) # Test docking action self.action_result = [] - self.dock_action_client.wait_for_server(timeout_sec=5.0) + assert self.dock_action_client.wait_for_server(timeout_sec=5.0), \ + 'dock_robot service not available' + goal = DockRobot.Goal() goal.use_dock_id = True goal.dock_id = 'test_dock' @@ -188,11 +214,12 @@ def test_docking_server(self): goal, feedback_callback=self.action_feedback_callback) rclpy.spin_until_future_complete(self.node, future) self.goal_handle = future.result() - assert self.goal_handle.accepted + assert self.goal_handle is not None, 'goal_handle should not be None' + assert self.goal_handle.accepted, 'goal_handle not accepted' result_future_original = self.goal_handle.get_result_async() # Run for 2 seconds - for i in range(20): + for _ in range(20): rclpy.spin_once(self.node, timeout_sec=0.1) time.sleep(0.1) @@ -201,7 +228,8 @@ def test_docking_server(self): goal, feedback_callback=self.action_feedback_callback) rclpy.spin_until_future_complete(self.node, future) self.goal_handle = future.result() - assert self.goal_handle.accepted + assert self.goal_handle is not None, 'goal_handle should not be None' + assert self.goal_handle.accepted, 'goal_handle not accepted' result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self.node, result_future) self.action_result.append(result_future.result()) @@ -216,7 +244,7 @@ def test_docking_server(self): self.node.get_logger().info('Goal preempted') # Run for 0.5 seconds - for i in range(5): + for _ in range(5): rclpy.spin_once(self.node, timeout_sec=0.1) time.sleep(0.1) @@ -230,7 +258,8 @@ def test_docking_server(self): goal, feedback_callback=self.action_feedback_callback) rclpy.spin_until_future_complete(self.node, future) self.goal_handle = future.result() - assert self.goal_handle.accepted + assert self.goal_handle is not None, 'goal_handle should not be None' + assert self.goal_handle.accepted, 'goal_handle not accepted' result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self.node, result_future) self.action_result.append(result_future.result()) @@ -247,10 +276,19 @@ def test_docking_server(self): future = self.undock_action_client.send_goal_async(goal) rclpy.spin_until_future_complete(self.node, future) self.goal_handle = future.result() - assert self.goal_handle.accepted + assert self.goal_handle is not None, 'goal_handle should not be None' + assert self.goal_handle.accepted, 'goal_handle not accepted' result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self.node, result_future) self.action_result.append(result_future.result()) self.assertEqual(self.action_result[3].status, GoalStatus.STATUS_SUCCEEDED) self.assertTrue(self.action_result[3].result.success) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + + def test_exit_code(self, proc_info): + # Check that all processes in the launch exit with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 39a2a3cbf74..074d44f2a9a 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -2,7 +2,7 @@ opennav_docking_bt - 1.3.4 + 1.3.5 A set of BT nodes and XMLs for docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 895d4f68cdb..7d46b360f0f 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -2,7 +2,7 @@ opennav_docking_core - 1.3.4 + 1.3.5 A set of headers for plugins core to the opennav docking framework Steve Macenski Apache-2.0 diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index cf0df0bda41..fd60e9e89de 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.3.4 + 1.3.5 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 3ac03b05692..2586220ae80 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.3.4 + 1.3.5 DWB core interfaces package Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 6e2976b0ecd..e4561d0e4ca 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.3.4 + 1.3.5 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp index b556a06be15..e2dd85d15b1 100644 --- a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp +++ b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp @@ -242,7 +242,9 @@ TEST(ObstacleFootprint, LineCost) costmap_ros->getCostmap()->setCost(4, 3, 100); costmap_ros->getCostmap()->setCost(4, 4, 100); - ASSERT_EQ(critic->lineCost(3, 3, 0, 50), 50); // all 50 + auto max_y_in_grid_coordinates = costmap_ros->getCostmap()->getSizeInCellsY() - 1; + ASSERT_EQ(max_y_in_grid_coordinates, 49); + ASSERT_EQ(critic->lineCost(3, 3, 0, max_y_in_grid_coordinates), 50); // all 50 ASSERT_EQ(critic->lineCost(4, 4, 0, 10), 100); // all 100 ASSERT_EQ(critic->lineCost(0, 50, 3, 3), 100); // pass 50 and 100 } diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 753fe239c07..482cf9c1792 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.3.4 + 1.3.5 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index d795771b147..3d651305ff8 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.3.4 + 1.3.5 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index c57410066e3..eff7c399786 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.3.4 + 1.3.5 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index bcbaee02e00..5d0f43cbe82 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.3.4 + 1.3.5 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index bf0f68c58a2..3d13cb5ea1b 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.3.4 + 1.3.5 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 1a77a3adfc6..8669aafeedb 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -107,33 +107,23 @@ class GracefulController : public nav2_core::Controller void setSpeedLimit(const double & speed_limit, const bool & percentage) override; protected: - /** - * @brief Get motion target point. - * @param motion_target_dist Optimal motion target distance - * @param path Current global path - * @return Motion target point - */ - geometry_msgs::msg::PoseStamped getMotionTarget( - const double & motion_target_dist, - const nav_msgs::msg::Path & path); - /** * @brief Simulate trajectory calculating in every step the new velocity command based on * a new curvature value and checking for collisions. * - * @param robot_pose Robot pose - * @param motion_target Motion target point + * @param motion_target Motion target point (in costmap local frame?) * @param costmap_transform Transform between global and local costmap * @param trajectory Simulated trajectory + * @param cmd_vel Initial command velocity during simulation * @param backward Flag to indicate if the robot is moving backward * @return true if the trajectory is collision free, false otherwise */ bool simulateTrajectory( - const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::PoseStamped & motion_target, const geometry_msgs::msg::TransformStamped & costmap_transform, nav_msgs::msg::Path & trajectory, - const bool & backward); + geometry_msgs::msg::TwistStamped & cmd_vel, + bool backward); /** * @brief Rotate the robot to face the motion target with maximum angular velocity. @@ -141,8 +131,7 @@ class GracefulController : public nav2_core::Controller * @param angle_to_target Angle to the motion target * @return geometry_msgs::msg::Twist Velocity command */ - geometry_msgs::msg::Twist rotateToTarget( - const double & angle_to_target); + geometry_msgs::msg::Twist rotateToTarget(double angle_to_target); /** * @brief Checks if the robot is in collision @@ -153,6 +142,21 @@ class GracefulController : public nav2_core::Controller */ bool inCollision(const double & x, const double & y, const double & theta); + /** + * @brief Compute the distance to each pose in a path + * @param poses Poses to compute distances with + * @param distances Computed distances + */ + void computeDistanceAlongPath( + const std::vector & poses, + std::vector & distances); + + /** + * @brief Control law requires proper orientations, not all planners provide them + * @param path Path to add orientations into, if required + */ + void validateOrientations(std::vector & path); + std::shared_ptr tf_buffer_; std::string plugin_name_; std::shared_ptr costmap_ros_; @@ -164,9 +168,12 @@ class GracefulController : public nav2_core::Controller double goal_dist_tolerance_; bool goal_reached_; + // True from the time a new path arrives until we have completed an initial rotation + bool do_initial_rotation_; + std::shared_ptr> transformed_plan_pub_; std::shared_ptr> local_plan_pub_; - std::shared_ptr> + std::shared_ptr> motion_target_pub_; std::shared_ptr> slowdown_pub_; diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp index 7594524cf0f..dd4d4df09ce 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -33,7 +33,8 @@ namespace nav2_graceful_controller struct Parameters { double transform_tolerance; - double motion_target_dist; + double min_lookahead; + double max_lookahead; double max_robot_pose_search_dist; double k_phi; double k_delta; @@ -44,12 +45,14 @@ struct Parameters double v_linear_max_initial; double v_angular_max; double v_angular_max_initial; + double v_angular_min_in_place; double slowdown_radius; bool initial_rotation; - double initial_rotation_min_angle; - bool final_rotation; + double initial_rotation_tolerance; + bool prefer_final_rotation; double rotation_scaling_factor; bool allow_backward; + double in_place_collision_resolution; }; /** diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp index 788656bd64b..65aea70d87d 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp @@ -15,22 +15,11 @@ #ifndef NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_ #define NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_ -#include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" namespace nav2_graceful_controller { -/** - * @brief Create a PointStamped message of the motion target for - * debugging / visualization porpuses. - * - * @param motion_target Motion target in PoseStamped format - * @return geometry_msgs::msg::PointStamped Motion target in PointStamped format - */ -geometry_msgs::msg::PointStamped createMotionTargetMsg( - const geometry_msgs::msg::PoseStamped & motion_target); - /** * @brief Create a flat circle marker of radius slowdown_radius around the motion target for * debugging / visualization porpuses. diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index d84e35ab80b..747f0864ef5 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.3.4 + 1.3.5 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index f8610a55c90..aba04ce4ced 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + +#include "angles/angles.h" #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_graceful_controller/graceful_controller.hpp" @@ -58,7 +62,7 @@ void GracefulController::configure( // Publishers transformed_plan_pub_ = node->create_publisher("transformed_global_plan", 1); local_plan_pub_ = node->create_publisher("local_plan", 1); - motion_target_pub_ = node->create_publisher("motion_target", 1); + motion_target_pub_ = node->create_publisher("motion_target", 1); slowdown_pub_ = node->create_publisher("slowdown", 1); RCLCPP_INFO(logger_, "Configured Graceful Motion Controller: %s", plugin_name_.c_str()); @@ -111,6 +115,9 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( { std::lock_guard param_lock(param_handler_->getMutex()); + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + // Update for the current goal checker's state geometry_msgs::msg::Pose pose_tolerance; geometry_msgs::msg::Twist velocity_tolerance; @@ -126,64 +133,15 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( control_law_->setSlowdownRadius(params_->slowdown_radius); control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); - // Transform path to robot base frame and publish it + // Transform path to robot base frame auto transformed_plan = path_handler_->transformGlobalPlan( pose, params_->max_robot_pose_search_dist); - transformed_plan_pub_->publish(transformed_plan); - - // Get the particular point on the path at the motion target distance and publish it - auto motion_target = getMotionTarget(params_->motion_target_dist, transformed_plan); - auto motion_target_point = nav2_graceful_controller::createMotionTargetMsg(motion_target); - motion_target_pub_->publish(motion_target_point); - // Publish marker for slowdown radius around motion target for debugging / visualization - auto slowdown_marker = nav2_graceful_controller::createSlowdownMarker( - motion_target, - params_->slowdown_radius); - slowdown_pub_->publish(slowdown_marker); + // Add proper orientations to plan, if needed + validateOrientations(transformed_plan.poses); - // Compute distance to goal as the path's integrated distance to account for path curvatures - double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(transformed_plan); - - // If the distance to the goal is less than the motion target distance, i.e. - // the 'motion target' is the goal, then we skip the motion target orientation by pointing - // it in the same orientation that the last segment of the path - double angle_to_target = atan2(motion_target.pose.position.y, motion_target.pose.position.x); - if (params_->final_rotation && dist_to_goal < params_->motion_target_dist) { - geometry_msgs::msg::PoseStamped stl_pose = - transformed_plan.poses[transformed_plan.poses.size() - 2]; - geometry_msgs::msg::PoseStamped goal_pose = transformed_plan.poses.back(); - double dx = goal_pose.pose.position.x - stl_pose.pose.position.x; - double dy = goal_pose.pose.position.y - stl_pose.pose.position.y; - double yaw = std::atan2(dy, dx); - motion_target.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); - } - - // Flip the orientation of the motion target if the robot is moving backwards - bool reversing = false; - if (params_->allow_backward && motion_target.pose.position.x < 0.0) { - reversing = true; - motion_target.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( - tf2::getYaw(motion_target.pose.orientation) + M_PI); - } - - // Compute velocity command: - // 1. Check if we are close enough to the goal to do a final rotation in place - // 2. Check if we must do a rotation in place before moving - // 3. Calculate the new velocity command using the smooth control law - geometry_msgs::msg::TwistStamped cmd_vel; - cmd_vel.header = pose.header; - if (params_->final_rotation && (dist_to_goal < goal_dist_tolerance_ || goal_reached_)) { - goal_reached_ = true; - double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); - cmd_vel.twist = rotateToTarget(angle_to_goal); - } else if (params_->initial_rotation && // NOLINT - fabs(angle_to_target) > params_->initial_rotation_min_angle) - { - cmd_vel.twist = rotateToTarget(angle_to_target); - } else { - cmd_vel.twist = control_law_->calculateRegularVelocity(motion_target.pose, reversing); - } + // Publish plan for visualization + transformed_plan_pub_->publish(transformed_plan); // Transform local frame to global frame to use in collision checking geometry_msgs::msg::TransformStamped costmap_transform; @@ -196,24 +154,107 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( logger_, "Could not transform %s to %s: %s", costmap_ros_->getBaseFrameID().c_str(), costmap_ros_->getGlobalFrameID().c_str(), ex.what()); - return cmd_vel; + throw ex; } - // Generate and publish local plan for debugging / visualization - nav_msgs::msg::Path local_plan; - if (!simulateTrajectory(pose, motion_target, costmap_transform, local_plan, reversing)) { - throw nav2_core::NoValidControl("Collision detected in the trajectory"); + // Compute distance to goal as the path's integrated distance to account for path curvatures + double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(transformed_plan); + + // If we've reached the XY goal tolerance, just rotate + if (dist_to_goal < goal_dist_tolerance_ || goal_reached_) { + goal_reached_ = true; + double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + // Check for collisions between our current pose and goal pose + size_t num_steps = fabs(angle_to_goal) / params_->in_place_collision_resolution; + // Need to check at least the end pose + num_steps = std::max(static_cast(1), num_steps); + bool collision_free = true; + for (size_t i = 1; i <= num_steps; ++i) { + double step = static_cast(i) / static_cast(num_steps); + double yaw = step * angle_to_goal; + geometry_msgs::msg::PoseStamped next_pose; + next_pose.header.frame_id = costmap_ros_->getBaseFrameID(); + next_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + geometry_msgs::msg::PoseStamped costmap_pose; + tf2::doTransform(next_pose, costmap_pose, costmap_transform); + if (inCollision( + costmap_pose.pose.position.x, costmap_pose.pose.position.y, + tf2::getYaw(costmap_pose.pose.orientation))) + { + collision_free = false; + break; + } + } + // Compute velocity if rotation is possible + if (collision_free) { + cmd_vel.twist = rotateToTarget(angle_to_goal); + return cmd_vel; + } + // Else, fall through and see if we should follow control law longer } - local_plan.header = transformed_plan.header; - local_plan_pub_->publish(local_plan); - return cmd_vel; + // Precompute distance to candidate poses + std::vector target_distances; + computeDistanceAlongPath(transformed_plan.poses, target_distances); + + // Work back from the end of plan to find valid target pose + for (int i = transformed_plan.poses.size() - 1; i >= 0; --i) { + // Underlying control law needs a single target pose, which should: + // * Be as far away as possible from the robot (for smoothness) + // * But no further than the max_lookahed_ distance + // * Be feasible to reach in a collision free manner + geometry_msgs::msg::PoseStamped target_pose = transformed_plan.poses[i]; + double dist_to_target = target_distances[i]; + + // Continue if target_pose is too far away from robot + if (dist_to_target > params_->max_lookahead) {continue;} + + if (dist_to_goal < params_->max_lookahead) { + if (params_->prefer_final_rotation) { + // Avoid unstability and big sweeping turns at the end of paths by + // ignoring final heading + double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x); + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + } + } else if (dist_to_target < params_->min_lookahead) { + // Make sure target is far enough away to avoid instability + break; + } + + // Flip the orientation of the motion target if the robot is moving backwards + bool reversing = false; + if (params_->allow_backward && target_pose.pose.position.x < 0.0) { + reversing = true; + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(target_pose.pose.orientation) + M_PI); + } + + // Actually simulate our path + nav_msgs::msg::Path local_plan; + if (simulateTrajectory(target_pose, costmap_transform, local_plan, cmd_vel, reversing)) { + // Successfully simulated to target_pose - compute velocity at this moment + // Publish the selected target_pose + motion_target_pub_->publish(target_pose); + // Publish marker for slowdown radius around motion target for debugging / visualization + auto slowdown_marker = nav2_graceful_controller::createSlowdownMarker( + target_pose, params_->slowdown_radius); + slowdown_pub_->publish(slowdown_marker); + // Publish the local plan + local_plan.header = transformed_plan.header; + local_plan_pub_->publish(local_plan); + // Successfully found velocity command + return cmd_vel; + } + } + + throw nav2_core::NoValidControl("Collision detected in trajectory"); } void GracefulController::setPlan(const nav_msgs::msg::Path & path) { path_handler_->setPlan(path); goal_reached_ = false; + do_initial_rotation_ = true; } void GracefulController::setSpeedLimit( @@ -240,58 +281,66 @@ void GracefulController::setSpeedLimit( } } -geometry_msgs::msg::PoseStamped GracefulController::getMotionTarget( - const double & motion_target_dist, - const nav_msgs::msg::Path & transformed_plan) -{ - // Find the first pose which is at a distance greater than the motion target distance - auto goal_pose_it = std::find_if( - transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { - return std::hypot(ps.pose.position.x, ps.pose.position.y) >= motion_target_dist; - }); - - // If the pose is not far enough, take the last pose - if (goal_pose_it == transformed_plan.poses.end()) { - goal_pose_it = std::prev(transformed_plan.poses.end()); - } - - return *goal_pose_it; -} - bool GracefulController::simulateTrajectory( - const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::PoseStamped & motion_target, const geometry_msgs::msg::TransformStamped & costmap_transform, - nav_msgs::msg::Path & trajectory, const bool & backward) + nav_msgs::msg::Path & trajectory, + geometry_msgs::msg::TwistStamped & cmd_vel, + bool backward) { - // Check for collision before moving - if (inCollision( - robot_pose.pose.position.x, robot_pose.pose.position.y, - tf2::getYaw(robot_pose.pose.orientation))) - { - return false; - } + trajectory.poses.clear(); - // First pose + // First pose is robot current pose geometry_msgs::msg::PoseStamped next_pose; next_pose.header.frame_id = costmap_ros_->getBaseFrameID(); next_pose.pose.orientation.w = 1.0; - trajectory.poses.push_back(next_pose); + + // Should we simulate rotation initially? + bool sim_initial_rotation = do_initial_rotation_ && params_->initial_rotation; + double angle_to_target = + std::atan2(motion_target.pose.position.y, motion_target.pose.position.x); + if (fabs(angle_to_target) < params_->initial_rotation_tolerance) { + sim_initial_rotation = false; + do_initial_rotation_ = false; + } double distance = std::numeric_limits::max(); double resolution_ = costmap_ros_->getCostmap()->getResolution(); double dt = (params_->v_linear_max > 0.0) ? resolution_ / params_->v_linear_max : 0.0; // Set max iter to avoid infinite loop - unsigned int max_iter = 2 * sqrt( - motion_target.pose.position.x * motion_target.pose.position.x + - motion_target.pose.position.y * motion_target.pose.position.y) / resolution_; + unsigned int max_iter = 3 * + std::hypot(motion_target.pose.position.x, motion_target.pose.position.y) / resolution_; // Generate path do{ - // Apply velocities to calculate next pose - next_pose.pose = control_law_->calculateNextPose( - dt, motion_target.pose, next_pose.pose, backward); + if (sim_initial_rotation) { + // Compute rotation velocity + double next_pose_yaw = tf2::getYaw(next_pose.pose.orientation); + auto cmd = rotateToTarget(angle_to_target - next_pose_yaw); + + // If this is first iteration, this is our current target velocity + if (trajectory.poses.empty()) {cmd_vel.twist = cmd;} + + // Are we done simulating initial rotation? + if (fabs(angle_to_target - next_pose_yaw) < params_->initial_rotation_tolerance) { + sim_initial_rotation = false; + } + + // Forward simulate rotation command + next_pose_yaw += cmd_vel.twist.angular.z * dt; + next_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(next_pose_yaw); + } else { + // If this is first iteration, this is our current target velocity + if (trajectory.poses.empty()) { + cmd_vel.twist = control_law_->calculateRegularVelocity( + motion_target.pose, next_pose.pose, backward); + } + + // Apply velocities to calculate next pose + next_pose.pose = control_law_->calculateNextPose( + dt, motion_target.pose, next_pose.pose, backward); + } // Add the pose to the trajectory for visualization trajectory.poses.push_back(next_pose); @@ -313,11 +362,13 @@ bool GracefulController::simulateTrajectory( return true; } -geometry_msgs::msg::Twist GracefulController::rotateToTarget(const double & angle_to_target) +geometry_msgs::msg::Twist GracefulController::rotateToTarget(double angle_to_target) { geometry_msgs::msg::Twist vel; vel.linear.x = 0.0; vel.angular.z = params_->rotation_scaling_factor * angle_to_target * params_->v_angular_max; + vel.angular.z = std::copysign(1.0, vel.angular.z) * std::max(abs(vel.angular.z), + params_->v_angular_min_in_place); return vel; } @@ -357,6 +408,46 @@ bool GracefulController::inCollision(const double & x, const double & y, const d return false; } +void GracefulController::computeDistanceAlongPath( + const std::vector & poses, + std::vector & distances) +{ + distances.resize(poses.size()); + // Do the first pose from robot + double d = std::hypot(poses[0].pose.position.x, poses[0].pose.position.y); + distances[0] = d; + // Compute remaining poses + for (size_t i = 1; i < poses.size(); ++i) { + d += nav2_util::geometry_utils::euclidean_distance(poses[i - 1].pose, poses[i].pose); + distances[i] = d; + } +} + +void GracefulController::validateOrientations( + std::vector & path) +{ + // We never change the orientation of the first & last pose + // So we need at least three poses to do anything here + if (path.size() < 3) {return;} + + // Check if we actually need to add orientations + double initial_yaw = tf2::getYaw(path[1].pose.orientation); + for (size_t i = 2; i < path.size() - 1; ++i) { + double this_yaw = tf2::getYaw(path[i].pose.orientation); + if (angles::shortest_angular_distance(this_yaw, initial_yaw) > 1e-6) {return;} + } + + // For each pose, point at the next one + // NOTE: control loop will handle reversing logic + for (size_t i = 0; i < path.size() - 1; ++i) { + // Get relative yaw angle + double dx = path[i + 1].pose.position.x - path[i].pose.position.x; + double dy = path[i + 1].pose.position.y - path[i].pose.position.y; + double yaw = std::atan2(dy, dx); + path[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + } +} + } // namespace nav2_graceful_controller // Register this controller as a nav2_core plugin diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 3d3389ec1d9..f8ba2aa075c 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -38,13 +38,15 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( - node, plugin_name_ + ".motion_target_dist", rclcpp::ParameterValue(0.6)); + node, plugin_name_ + ".min_lookahead", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x / 2.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(3.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(2.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.2)); + declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(2.0)); + declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.4)); declare_parameter_if_not_declared(node, plugin_name_ + ".lambda", rclcpp::ParameterValue(2.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".v_linear_min", rclcpp::ParameterValue(0.1)); @@ -52,21 +54,26 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".v_linear_max", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".v_angular_max", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".v_angular_min_in_place", rclcpp::ParameterValue(0.25)); declare_parameter_if_not_declared( node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".initial_rotation_min_angle", rclcpp::ParameterValue(0.75)); + node, plugin_name_ + ".initial_rotation_tolerance", rclcpp::ParameterValue(0.75)); declare_parameter_if_not_declared( - node, plugin_name_ + ".final_rotation", rclcpp::ParameterValue(true)); + node, plugin_name_ + ".prefer_final_rotation", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".in_place_collision_resolution", rclcpp::ParameterValue(0.1)); node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); - node->get_parameter(plugin_name_ + ".motion_target_dist", params_.motion_target_dist); + node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead); + node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead); node->get_parameter( plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist); if (params_.max_robot_pose_search_dist < 0.0) { @@ -85,13 +92,17 @@ ParameterHandler::ParameterHandler( params_.v_linear_max_initial = params_.v_linear_max; node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max); params_.v_angular_max_initial = params_.v_angular_max; + node->get_parameter( + plugin_name_ + ".v_angular_min_in_place", params_.v_angular_min_in_place); node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius); node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation); node->get_parameter( - plugin_name_ + ".initial_rotation_min_angle", params_.initial_rotation_min_angle); - node->get_parameter(plugin_name_ + ".final_rotation", params_.final_rotation); + plugin_name_ + ".initial_rotation_tolerance", params_.initial_rotation_tolerance); + node->get_parameter(plugin_name_ + ".prefer_final_rotation", params_.prefer_final_rotation); node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor); node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward); + node->get_parameter( + plugin_name_ + ".in_place_collision_resolution", params_.in_place_collision_resolution); if (params_.initial_rotation && params_.allow_backward) { RCLCPP_WARN( @@ -126,8 +137,10 @@ ParameterHandler::dynamicParametersCallback(std::vector param if (type == ParameterType::PARAMETER_DOUBLE) { if (name == plugin_name_ + ".transform_tolerance") { params_.transform_tolerance = parameter.as_double(); - } else if (name == plugin_name_ + ".motion_target_dist") { - params_.motion_target_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".min_lookahead") { + params_.min_lookahead = parameter.as_double(); + } else if (name == plugin_name_ + ".max_lookahead") { + params_.max_lookahead = parameter.as_double(); } else if (name == plugin_name_ + ".k_phi") { params_.k_phi = parameter.as_double(); } else if (name == plugin_name_ + ".k_delta") { @@ -144,12 +157,16 @@ ParameterHandler::dynamicParametersCallback(std::vector param } else if (name == plugin_name_ + ".v_angular_max") { params_.v_angular_max = parameter.as_double(); params_.v_angular_max_initial = params_.v_angular_max; + } else if (name == plugin_name_ + ".v_angular_min_in_place") { + params_.v_angular_min_in_place = parameter.as_double(); } else if (name == plugin_name_ + ".slowdown_radius") { params_.slowdown_radius = parameter.as_double(); - } else if (name == plugin_name_ + ".initial_rotation_min_angle") { - params_.initial_rotation_min_angle = parameter.as_double(); + } else if (name == plugin_name_ + ".initial_rotation_tolerance") { + params_.initial_rotation_tolerance = parameter.as_double(); } else if (name == plugin_name_ + ".rotation_scaling_factor") { params_.rotation_scaling_factor = parameter.as_double(); + } else if (name == plugin_name_ + ".in_place_collision_resolution") { + params_.in_place_collision_resolution = parameter.as_double(); } } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".initial_rotation") { @@ -160,8 +177,8 @@ ParameterHandler::dynamicParametersCallback(std::vector param continue; } params_.initial_rotation = parameter.as_bool(); - } else if (name == plugin_name_ + ".final_rotation") { - params_.final_rotation = parameter.as_bool(); + } else if (name == plugin_name_ + ".prefer_final_rotation") { + params_.prefer_final_rotation = parameter.as_bool(); } else if (name == plugin_name_ + ".allow_backward") { if (params_.initial_rotation && parameter.as_bool()) { RCLCPP_WARN( diff --git a/nav2_graceful_controller/src/utils.cpp b/nav2_graceful_controller/src/utils.cpp index e6cdbadd569..0e79879efcd 100644 --- a/nav2_graceful_controller/src/utils.cpp +++ b/nav2_graceful_controller/src/utils.cpp @@ -17,16 +17,6 @@ namespace nav2_graceful_controller { -geometry_msgs::msg::PointStamped createMotionTargetMsg( - const geometry_msgs::msg::PoseStamped & motion_target) -{ - geometry_msgs::msg::PointStamped motion_target_point; - motion_target_point.header = motion_target.header; - motion_target_point.point = motion_target.pose.position; - motion_target_point.point.z = 0.01; - return motion_target_point; -} - visualization_msgs::msg::Marker createSlowdownMarker( const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius) { diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index fd5f986c76a..de95598bb0f 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -59,19 +59,6 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} - geometry_msgs::msg::PoseStamped getMotionTarget( - const double & motion_target_distance, const nav_msgs::msg::Path & plan) - { - return nav2_graceful_controller::GracefulController::getMotionTarget( - motion_target_distance, plan); - } - - geometry_msgs::msg::PointStamped createMotionTargetMsg( - const geometry_msgs::msg::PoseStamped & motion_target) - { - return nav2_graceful_controller::createMotionTargetMsg(motion_target); - } - visualization_msgs::msg::Marker createSlowdownMarker( const geometry_msgs::msg::PoseStamped & motion_target) { @@ -86,16 +73,6 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController angle_to_target); } - bool simulateTrajectory( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::PoseStamped & motion_target, - const geometry_msgs::msg::TransformStamped & costmap_transform, - nav_msgs::msg::Path & trajectory, const bool & backward) - { - return nav2_graceful_controller::GracefulController::simulateTrajectory( - robot_pose, motion_target, costmap_transform, trajectory, backward); - } - double getSpeedLinearMax() {return params_->v_linear_max;} nav_msgs::msg::Path transformGlobalPlan( @@ -289,7 +266,8 @@ TEST(GracefulControllerTest, dynamicParameters) { // Set parameters auto results = params->set_parameters_atomically( {rclcpp::Parameter("test.transform_tolerance", 1.0), - rclcpp::Parameter("test.motion_target_dist", 2.0), + rclcpp::Parameter("test.min_lookahead", 1.0), + rclcpp::Parameter("test.max_lookahead", 2.0), rclcpp::Parameter("test.k_phi", 4.0), rclcpp::Parameter("test.k_delta", 5.0), rclcpp::Parameter("test.beta", 6.0), @@ -297,19 +275,22 @@ TEST(GracefulControllerTest, dynamicParameters) { rclcpp::Parameter("test.v_linear_min", 8.0), rclcpp::Parameter("test.v_linear_max", 9.0), rclcpp::Parameter("test.v_angular_max", 10.0), + rclcpp::Parameter("test.v_angular_min_in_place", 14.0), rclcpp::Parameter("test.slowdown_radius", 11.0), rclcpp::Parameter("test.initial_rotation", false), - rclcpp::Parameter("test.initial_rotation_min_angle", 12.0), - rclcpp::Parameter("test.final_rotation", false), + rclcpp::Parameter("test.initial_rotation_tolerance", 12.0), + rclcpp::Parameter("test.prefer_final_rotation", false), rclcpp::Parameter("test.rotation_scaling_factor", 13.0), - rclcpp::Parameter("test.allow_backward", true)}); + rclcpp::Parameter("test.allow_backward", true), + rclcpp::Parameter("test.in_place_collision_resolution", 15.0)}); // Spin rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); // Check parameters EXPECT_EQ(node->get_parameter("test.transform_tolerance").as_double(), 1.0); - EXPECT_EQ(node->get_parameter("test.motion_target_dist").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.min_lookahead").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("test.max_lookahead").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.k_phi").as_double(), 4.0); EXPECT_EQ(node->get_parameter("test.k_delta").as_double(), 5.0); EXPECT_EQ(node->get_parameter("test.beta").as_double(), 6.0); @@ -317,12 +298,14 @@ TEST(GracefulControllerTest, dynamicParameters) { EXPECT_EQ(node->get_parameter("test.v_linear_min").as_double(), 8.0); EXPECT_EQ(node->get_parameter("test.v_linear_max").as_double(), 9.0); EXPECT_EQ(node->get_parameter("test.v_angular_max").as_double(), 10.0); + EXPECT_EQ(node->get_parameter("test.v_angular_min_in_place").as_double(), 14.0); EXPECT_EQ(node->get_parameter("test.slowdown_radius").as_double(), 11.0); EXPECT_EQ(node->get_parameter("test.initial_rotation").as_bool(), false); - EXPECT_EQ(node->get_parameter("test.initial_rotation_min_angle").as_double(), 12.0); - EXPECT_EQ(node->get_parameter("test.final_rotation").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.initial_rotation_tolerance").as_double(), 12.0); + EXPECT_EQ(node->get_parameter("test.prefer_final_rotation").as_bool(), false); EXPECT_EQ(node->get_parameter("test.rotation_scaling_factor").as_double(), 13.0); EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.in_place_collision_resolution").as_double(), 15.0); // Set initial rotation to true results = params->set_parameters_atomically( @@ -356,84 +339,6 @@ TEST(GracefulControllerTest, dynamicParameters) { EXPECT_EQ(controller->getAllowBackward(), false); } -TEST(GracefulControllerTest, getDifferentMotionTargets) { - auto node = std::make_shared("testGraceful"); - auto tf = std::make_shared(node->get_clock()); - auto costmap_ros = std::make_shared("global_costmap"); - - // Create controller - auto controller = std::make_shared(); - costmap_ros->on_configure(rclcpp_lifecycle::State()); - controller->configure(node, "test", tf, costmap_ros); - controller->activate(); - - // Set the plan - nav_msgs::msg::Path plan; - plan.header.frame_id = "map"; - plan.poses.resize(3); - plan.poses[0].header.frame_id = "map"; - plan.poses[0].pose.position.x = 1.0; - plan.poses[0].pose.position.y = 2.0; - plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - plan.poses[1].header.frame_id = "map"; - plan.poses[1].pose.position.x = 3.0; - plan.poses[1].pose.position.y = 4.0; - plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - plan.poses[2].header.frame_id = "map"; - plan.poses[2].pose.position.x = 5.0; - plan.poses[2].pose.position.y = 6.0; - plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - controller->setPlan(plan); - - // Set distance and get motion target - double motion_target_distance = 3.5; - auto motion_target = controller->getMotionTarget(motion_target_distance, plan); - - // Check results, should be the second one - EXPECT_EQ(motion_target.header.frame_id, "map"); - EXPECT_EQ(motion_target.pose.position.x, 3.0); - EXPECT_EQ(motion_target.pose.position.y, 4.0); - EXPECT_EQ(motion_target.pose.orientation, tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0))); - - // Set a new distance greater than the path length and get motion target - motion_target_distance = 10.0; - motion_target = controller->getMotionTarget(motion_target_distance, plan); - - // Check results: should be the last one - EXPECT_EQ(motion_target.header.frame_id, "map"); - EXPECT_EQ(motion_target.pose.position.x, 5.0); - EXPECT_EQ(motion_target.pose.position.y, 6.0); - EXPECT_EQ(motion_target.pose.orientation, tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0))); -} - -TEST(GracefulControllerTest, createMotionTargetMsg) { - auto node = std::make_shared("testGraceful"); - auto tf = std::make_shared(node->get_clock()); - auto costmap_ros = std::make_shared("global_costmap"); - - // Create controller - auto controller = std::make_shared(); - costmap_ros->on_configure(rclcpp_lifecycle::State()); - controller->configure(node, "test", tf, costmap_ros); - controller->activate(); - - // Create motion target - geometry_msgs::msg::PoseStamped motion_target; - motion_target.header.frame_id = "map"; - motion_target.pose.position.x = 1.0; - motion_target.pose.position.y = 2.0; - motion_target.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); - - // Create motion target message - auto motion_target_msg = controller->createMotionTargetMsg(motion_target); - - // Check results - EXPECT_EQ(motion_target_msg.header.frame_id, "map"); - EXPECT_EQ(motion_target_msg.point.x, 1.0); - EXPECT_EQ(motion_target_msg.point.y, 2.0); - EXPECT_EQ(motion_target_msg.point.z, 0.01); -} - TEST(GracefulControllerTest, createSlowdownMsg) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -522,6 +427,19 @@ TEST(GracefulControllerTest, rotateToTarget) { // Check results: it must be a negative rotation EXPECT_EQ(cmd_vel.linear.x, 0.0); EXPECT_EQ(cmd_vel.angular.z, -0.25); + + // Set very high v_angular_min_in_place velocity + results = params->set_parameters_atomically( + {rclcpp::Parameter("test.v_angular_min_in_place", 1.0)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Set a new angle to target + angle_to_target = 0.5; + cmd_vel = controller->rotateToTarget(angle_to_target); + + // Check results: positive velocity, at least as high as min_in_place + EXPECT_EQ(cmd_vel.linear.x, 0.0); + EXPECT_EQ(cmd_vel.angular.z, 1.0); } TEST(GracefulControllerTest, setSpeedLimit) { @@ -937,12 +855,12 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { plan.poses[0].pose.position.y = 0.0; plan.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); plan.poses[1].header.frame_id = "test_global_frame"; - plan.poses[1].pose.position.x = 1.0; - plan.poses[1].pose.position.y = 1.0; + plan.poses[1].pose.position.x = 0.5; + plan.poses[1].pose.position.y = 0.5; plan.poses[1].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); plan.poses[2].header.frame_id = "test_global_frame"; - plan.poses[2].pose.position.x = 2.0; - plan.poses[2].pose.position.y = 2.0; + plan.poses[2].pose.position.x = 1.0; + plan.poses[2].pose.position.y = 1.0; plan.poses[2].pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0)); controller->setPlan(plan); @@ -960,8 +878,8 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { // Check results: the robot should rotate in place. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. EXPECT_EQ(cmd_vel.twist.linear.x, 0.0); - EXPECT_GE(cmd_vel.twist.angular.x, 0.0); - EXPECT_LE(cmd_vel.twist.angular.x, 0.5); + EXPECT_GT(cmd_vel.twist.angular.z, 0.0); + EXPECT_LE(cmd_vel.twist.angular.z, 0.5); } TEST(GracefulControllerTest, computeVelocityCommandRegular) { @@ -1117,7 +1035,9 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { // Check results: the robot should go straight to the target. // So, both linear velocity should be some negative values. EXPECT_LT(cmd_vel.twist.linear.x, 0.0); - EXPECT_LT(cmd_vel.twist.angular.z, 0.0); + // There might be a small bit of noise on angular velocity + EXPECT_LT(cmd_vel.twist.angular.z, 0.01); + EXPECT_GT(cmd_vel.twist.angular.z, -0.01); } TEST(GracefulControllerTest, computeVelocityCommandFinal) { @@ -1200,8 +1120,8 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { // Check results: the robot should do a final rotation near the target. // So, linear velocity should be zero and angular velocity should be a positive value below 0.5. EXPECT_EQ(cmd_vel.twist.linear.x, 0.0); - EXPECT_GE(cmd_vel.twist.angular.x, 0.0); - EXPECT_LE(cmd_vel.twist.angular.x, 0.5); + EXPECT_GE(cmd_vel.twist.angular.z, 0.0); + EXPECT_LE(cmd_vel.twist.angular.z, 0.5); } int main(int argc, char ** argv) diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 6a2df2f39a0..d13d19dae10 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.3.4 + 1.3.5 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index df450febd99..eb34bc37573 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,7 +2,7 @@ nav2_loopback_sim - 1.3.4 + 1.3.5 A loopback simulator to replace physics simulation steve macenski Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 2b27d23e5e1..ce7462c12c0 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.3.4 + 1.3.5 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index fa105867d4d..f12058fa479 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.3.4 + 1.3.5 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index c30ea21c1ff..eae04b4c640 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.3.4 + 1.3.5 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index d8745e96dd7..ac33cb68e3b 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.3.4 + 1.3.5 Nav2 NavFn planner Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index ba9a85b78e0..91bae80cc58 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.3.4 + 1.3.5 Nav2 planner server package Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 06ca9ccfba2..adead1f74d1 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.3.4 + 1.3.5 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 03c1c9881c3..78307d7ebf6 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" @@ -106,6 +107,11 @@ class RotationShimController : public nav2_core::Controller */ void setSpeedLimit(const double & speed_limit, const bool & percentage) override; + /** + * @brief Reset the state of the controller + */ + void reset() override; + protected: /** * @brief Finds the point on the path that is roughly the sampling @@ -183,6 +189,8 @@ class RotationShimController : public nav2_core::Controller double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_; + bool closed_loop_; + double last_angular_vel_ = std::numeric_limits::max(); // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 87b6f4776de..86274088e08 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.3.4 + 1.3.5 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 6b3d24a4ab1..10c6a7211c9 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -68,6 +68,8 @@ void RotationShimController::configure( node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".closed_loop", rclcpp::ParameterValue(true)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_); @@ -84,6 +86,7 @@ void RotationShimController::configure( node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_); + node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_); try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); @@ -114,6 +117,7 @@ void RotationShimController::activate() primary_controller_->activate(); in_rotation_ = false; + last_angular_vel_ = std::numeric_limits::max(); auto node = node_.lock(); dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -179,7 +183,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw); - return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); + auto cmd_vel = computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); + last_angular_vel_ = cmd_vel.twist.angular.z; + return cmd_vel; } } catch (const std::runtime_error & e) { RCLCPP_INFO( @@ -208,7 +214,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands logger_, "Robot is not within the new path's rough heading, rotating to heading..."); in_rotation_ = true; - return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); + auto cmd_vel = computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); + last_angular_vel_ = cmd_vel.twist.angular.z; + return cmd_vel; } else { RCLCPP_DEBUG( logger_, @@ -227,7 +235,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands // If at this point, use the primary controller to path track in_rotation_ = false; - return primary_controller_->computeVelocityCommands(pose, velocity, goal_checker); + auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker); + last_angular_vel_ = cmd_vel.twist.angular.z; + return cmd_vel; } geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() @@ -285,16 +295,27 @@ RotationShimController::computeRotateToHeadingCommand( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) { + auto current = closed_loop_ ? velocity.angular.z : last_angular_vel_; + if (current == std::numeric_limits::max()) { + current = 0.0; + } + geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.header = pose.header; const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0; const double angular_vel = sign * rotate_to_heading_angular_vel_; const double & dt = control_duration_; - const double min_feasible_angular_speed = velocity.angular.z - max_angular_accel_ * dt; - const double max_feasible_angular_speed = velocity.angular.z + max_angular_accel_ * dt; + const double min_feasible_angular_speed = current - max_angular_accel_ * dt; + const double max_feasible_angular_speed = current + max_angular_accel_ * dt; cmd_vel.twist.angular.z = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + // Check if we need to slow down to avoid overshooting + double max_vel_to_stop = std::sqrt(2 * max_angular_accel_ * fabs(angular_distance_to_heading)); + if (fabs(cmd_vel.twist.angular.z) > max_vel_to_stop) { + cmd_vel.twist.angular.z = sign * max_vel_to_stop; + } + isCollisionFree(cmd_vel, angular_distance_to_heading, pose); return cmd_vel; } @@ -362,6 +383,12 @@ void RotationShimController::setSpeedLimit(const double & speed_limit, const boo primary_controller_->setSpeedLimit(speed_limit, percentage); } +void RotationShimController::reset() +{ + last_angular_vel_ = std::numeric_limits::max(); + primary_controller_->reset(); +} + rcl_interfaces::msg::SetParametersResult RotationShimController::dynamicParametersCallback(std::vector parameters) { @@ -389,6 +416,8 @@ RotationShimController::dynamicParametersCallback(std::vector rotate_to_goal_heading_ = parameter.as_bool(); } else if (name == plugin_name_ + ".rotate_to_heading_once") { rotate_to_heading_once_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".closed_loop") { + closed_loop_ = parameter.as_bool(); } } } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1d63a77b47e..b202d96f44f 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -314,6 +314,84 @@ TEST(RotationShimControllerTest, computeVelocityTests) EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); } +TEST(RotationShimControllerTest, openLoopRotationTests) { + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "base_link"; + transform.child_frame_id = "odom"; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + tf_broadcaster->sendTransform(transform); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter( + "controller_frequency", + 20.0); + node->declare_parameter( + "PathFollower.rotate_to_goal_heading", + true); + node->declare_parameter( + "PathFollower.closed_loop", + false); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + // Test state update and path setting + nav_msgs::msg::Path path; + path.header.frame_id = "base_link"; + path.poses.resize(4); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "base_link"; + geometry_msgs::msg::Twist velocity; + nav2_controller::SimpleGoalChecker checker; + node->declare_parameter( + "checker.xy_goal_tolerance", + 1.0); + checker.initialize(node, "checker", costmap); + + path.header.frame_id = "base_link"; + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = 0.0; + path.poses[1].pose.position.x = 0.05; + path.poses[1].pose.position.y = 0.05; + path.poses[2].pose.position.x = 0.10; + path.poses[2].pose.position.y = 0.10; + // goal position within checker xy_goal_tolerance + path.poses[3].pose.position.x = 0.20; + path.poses[3].pose.position.y = 0.20; + // goal heading 45 degrees to the left + path.poses[3].pose.orientation.z = -0.3826834; + path.poses[3].pose.orientation.w = 0.9238795; + path.poses[3].header.frame_id = "base_link"; + + // Calculate first velocity command + controller->setPlan(path); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4); + + // Test second velocity command with wrong odometry + velocity.angular.z = 1.8; + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4); +} + TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); @@ -387,6 +465,84 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } +TEST(RotationShimControllerTest, accelerationTests) { + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "base_link"; + transform.child_frame_id = "odom"; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + tf_broadcaster->sendTransform(transform); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter( + "controller_frequency", + 20.0); + node->declare_parameter( + "PathFollower.rotate_to_goal_heading", + true); + node->declare_parameter( + "PathFollower.max_angular_accel", + 0.5); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + // Test state update and path setting + nav_msgs::msg::Path path; + path.header.frame_id = "base_link"; + path.poses.resize(4); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "base_link"; + geometry_msgs::msg::Twist velocity; + nav2_controller::SimpleGoalChecker checker; + node->declare_parameter( + "checker.xy_goal_tolerance", + 1.0); + checker.initialize(node, "checker", costmap); + + path.header.frame_id = "base_link"; + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = 0.0; + path.poses[1].pose.position.x = 0.05; + path.poses[1].pose.position.y = 0.05; + path.poses[2].pose.position.x = 0.10; + path.poses[2].pose.position.y = 0.10; + // goal position within checker xy_goal_tolerance + path.poses[3].pose.position.x = 0.20; + path.poses[3].pose.position.y = 0.20; + // goal heading 45 degrees to the left + path.poses[3].pose.orientation.z = -0.3826834; + path.poses[3].pose.orientation.w = 0.9238795; + path.poses[3].header.frame_id = "base_link"; + + // Test acceleration limits + controller->setPlan(path); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_EQ(cmd_vel.twist.angular.z, -0.025); + + // Test slowing down to avoid overshooting + velocity.angular.z = -1.8; + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_NEAR(cmd_vel.twist.angular.z, -std::sqrt(2 * 0.5 * M_PI / 4), 1e-4); +} + TEST(RotationShimControllerTest, isGoalChangedTest) { auto ctrl = std::make_shared(); @@ -469,7 +625,8 @@ TEST(RotationShimControllerTest, testDynamicParameter) rclcpp::Parameter("test.simulate_ahead_time", 7.0), rclcpp::Parameter("test.primary_controller", std::string("HI")), rclcpp::Parameter("test.rotate_to_goal_heading", true), - rclcpp::Parameter("test.rotate_to_heading_once", true)}); + rclcpp::Parameter("test.rotate_to_heading_once", true), + rclcpp::Parameter("test.closed_loop", false)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -482,4 +639,5 @@ TEST(RotationShimControllerTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true); EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.closed_loop").as_bool(), false); } diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 5cc51009da1..20621ffb9ca 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.3.4 + 1.3.5 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index e1dd961ac40..8c8205d79b9 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.3.4 + 1.3.5 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 9fc728ad34d..ff4974fcaab 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.3.4 + 1.3.5 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 728d9910fd9..afbbce075e3 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -258,7 +258,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; - // Corner case of start and goal beeing on the same cell + // Corner case of start and goal being on the same cell if (std::floor(mx_start) == std::floor(mx_goal) && std::floor(my_start) == std::floor(my_goal)) { pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal @@ -268,6 +268,12 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( pose.pose.orientation = goal.pose.orientation; } plan.poses.push_back(pose); + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(plan); + } + return plan; } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 70806aae201..b5a785c9ce5 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -361,8 +361,13 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates - float mx, my; - if (!costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, my)) { + float mx_start, my_start, mx_goal, my_goal; + if (!costmap->worldToMapContinuous( + start.pose.position.x, + start.pose.position.y, + mx_start, + my_start)) + { throw nav2_core::StartOutsideMapBounds( "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + std::to_string(start.pose.position.y) + ") was outside bounds"); @@ -376,10 +381,15 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setStart(mx, my, static_cast(orientation_bin)); + _a_star->setStart(mx_start, my_start, static_cast(orientation_bin)); // Set goal point, in A* bin search coordinates - if (!costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) { + if (!costmap->worldToMapContinuous( + goal.pose.position.x, + goal.pose.position.y, + mx_goal, + my_goal)) + { throw nav2_core::GoalOutsideMapBounds( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); @@ -392,7 +402,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setGoal(mx, my, static_cast(orientation_bin)); + _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin)); // Setup message nav_msgs::msg::Path plan; @@ -406,6 +416,22 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; + // Corner case of start and goal being on the same cell + if (std::floor(mx_start) == std::floor(mx_goal) && + std::floor(my_start) == std::floor(my_goal)) + { + pose.pose = start.pose; + pose.pose.orientation = goal.pose.orientation; + plan.poses.push_back(pose); + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(plan); + } + + return plan; + } + // Compute plan NodeHybrid::CoordinateVector path; int num_iterations = 0; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index c5a9ecdff60..447a15ef623 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -287,24 +287,34 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates - float mx, my; - if (!_costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, my)) { + float mx_start, my_start, mx_goal, my_goal; + if (!_costmap->worldToMapContinuous( + start.pose.position.x, + start.pose.position.y, + mx_start, + my_start)) + { throw nav2_core::StartOutsideMapBounds( "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + std::to_string(start.pose.position.y) + ") was outside bounds"); } _a_star->setStart( - mx, my, + mx_start, my_start, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); // Set goal point, in A* bin search coordinates - if (!_costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) { + if (!_costmap->worldToMapContinuous( + goal.pose.position.x, + goal.pose.position.y, + mx_goal, + my_goal)) + { throw nav2_core::GoalOutsideMapBounds( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); } _a_star->setGoal( - mx, my, + mx_goal, my_goal, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); // Setup message @@ -319,6 +329,22 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; + // Corner case of start and goal being on the same cell + if (std::floor(mx_start) == std::floor(mx_goal) && + std::floor(my_start) == std::floor(my_goal)) + { + pose.pose = start.pose; + pose.pose.orientation = goal.pose.orientation; + plan.poses.push_back(pose); + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(plan); + } + + return plan; + } + // Compute plan NodeLattice::CoordinateVector path; int num_iterations = 0; diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index e2dff5a56f4..89ad666b789 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -77,6 +77,13 @@ TEST(SmacTest, test_smac_2d) { } catch (...) { } + // corner case where the start and goal are on the same cell + goal.pose.position.x = 0.01; + goal.pose.position.y = 0.01; + + nav_msgs::msg::Path plan = planner_2d->createPlan(start, goal, dummy_cancel_checker); + EXPECT_EQ(plan.poses.size(), 1); // single point path + planner_2d->deactivate(); planner_2d->cleanup(); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 9952912596b..a70ae86fc7d 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -76,6 +76,13 @@ TEST(SmacTest, test_smac_se2) } catch (...) { } + // corner case where the start and goal are on the same cell + goal.pose.position.x = 0.01; + goal.pose.position.y = 0.01; + + nav_msgs::msg::Path plan = planner->createPlan(start, goal, dummy_cancel_checker); + EXPECT_EQ(plan.poses.size(), 1); // single point path + planner->deactivate(); planner->cleanup(); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index dcc925e9e72..c187abd8591 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -84,6 +84,13 @@ TEST(SmacTest, test_smac_lattice) } catch (...) { } + // corner case where the start and goal are on the same cell + goal.pose.position.x = 0.01; + goal.pose.position.y = 0.01; + + nav_msgs::msg::Path plan = planner->createPlan(start, goal, dummy_cancel_checker); + EXPECT_EQ(plan.poses.size(), 1); // single point path + planner->deactivate(); planner->cleanup(); diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 2e8fea4a945..f9839d92eaf 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -92,9 +92,8 @@ class SimpleSmoother : public nav2_core::Smoother * @param reversing_segment Return if this is a reversing segment * @param costmap Pointer to minimal costmap * @param max_time Maximum time to compute, stop early if over limit - * @return If smoothing was successful */ - bool smoothImpl( + void smoothImpl( nav_msgs::msg::Path & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index cb7702903ce..00f82870b7b 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.3.4 + 1.3.5 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 0b494d96c6e..5b4c2fff6e0 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -65,8 +65,7 @@ bool SimpleSmoother::smooth( steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time.seconds(); - bool success = true, reversing_segment; - unsigned int segments_smoothed = 0; + bool reversing_segment; nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; @@ -88,15 +87,9 @@ bool SimpleSmoother::smooth( time_remaining = max_time.seconds() - duration_cast>(now - start).count(); refinement_ctr_ = 0; - bool segment_was_smoothed = smoothImpl( - curr_path_segment, reversing_segment, costmap.get(), time_remaining); - - if (segment_was_smoothed) { - segments_smoothed++; - } - - // Smooth path segment naively - success = success && segment_was_smoothed; + // Attempt to smooth the segment + // May throw SmootherTimedOut + smoothImpl(curr_path_segment, reversing_segment, costmap.get(), time_remaining); // Assemble the path changes to the main path std::copy( @@ -106,14 +99,10 @@ bool SimpleSmoother::smooth( } } - if (segments_smoothed == 0) { - throw nav2_core::FailedToSmoothPath("No segments were smoothed"); - } - - return success; + return true; } -bool SimpleSmoother::smoothImpl( +void SimpleSmoother::smoothImpl( nav_msgs::msg::Path & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, @@ -142,7 +131,7 @@ bool SimpleSmoother::smoothImpl( "Number of iterations has exceeded limit of %i.", max_its_); path = last_path; updateApproximatePathOrientations(path, reversing_segment); - return false; + return; } // Make sure still have time left to process @@ -188,7 +177,7 @@ bool SimpleSmoother::smoothImpl( "Returning the last path before the infeasibility was introduced."); path = last_path; updateApproximatePathOrientations(path, reversing_segment); - return false; + return; } } @@ -204,7 +193,6 @@ bool SimpleSmoother::smoothImpl( updateApproximatePathOrientations(new_path, reversing_segment); path = new_path; - return true; } double SimpleSmoother::getFieldByDim( diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index c8f7da8e871..6c845bfb6c6 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -180,7 +180,7 @@ TEST(SmootherTest, test_simple_smoother) EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.607, 0.01); EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.387, 0.01); - // Test that collisions are rejected + // Test that collisions are disregarded nav_msgs::msg::Path collision_path; collision_path.poses.resize(11); collision_path.poses[0].pose.position.x = 0.0; @@ -205,7 +205,7 @@ TEST(SmootherTest, test_simple_smoother) collision_path.poses[9].pose.position.y = 1.4; collision_path.poses[10].pose.position.x = 1.5; collision_path.poses[10].pose.position.y = 1.5; - EXPECT_THROW(smoother->smooth(collision_path, max_time), nav2_core::FailedToSmoothPath); + EXPECT_TRUE(smoother->smooth(collision_path, max_time)); // test cusp / reversing segments nav_msgs::msg::Path reversing_path; @@ -232,7 +232,7 @@ TEST(SmootherTest, test_simple_smoother) reversing_path.poses[9].pose.position.y = 0.1; reversing_path.poses[10].pose.position.x = 0.5; reversing_path.poses[10].pose.position.y = 0.0; - EXPECT_THROW(smoother->smooth(reversing_path, max_time), nav2_core::FailedToSmoothPath); + EXPECT_TRUE(smoother->smooth(reversing_path, max_time)); // test rotate in place tf2::Quaternion quat1, quat2; @@ -244,7 +244,18 @@ TEST(SmootherTest, test_simple_smoother) straight_irregular_path.poses[6].pose.position.x = 0.5; straight_irregular_path.poses[6].pose.position.y = 0.5; straight_irregular_path.poses[6].pose.orientation = tf2::toMsg(quat2); - EXPECT_THROW(smoother->smooth(straight_irregular_path, max_time), nav2_core::FailedToSmoothPath); + EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time)); + + // test approach + nav_msgs::msg::Path approach_path; + approach_path.poses.resize(3); + approach_path.poses[0].pose.position.x = 0.5; + approach_path.poses[0].pose.position.y = 0.0; + approach_path.poses[1].pose.position.x = 0.5; + approach_path.poses[1].pose.position.y = 0.1; + approach_path.poses[2].pose.position.x = 0.5; + approach_path.poses[2].pose.position.y = 0.2; + EXPECT_TRUE(smoother->smooth(approach_path, max_time)); // test max iterations smoother->setMaxItsToInvalid(); @@ -272,5 +283,5 @@ TEST(SmootherTest, test_simple_smoother) max_its_path.poses[9].pose.position.y = 0.9; max_its_path.poses[10].pose.position.x = 0.5; max_its_path.poses[10].pose.position.y = 1.0; - EXPECT_THROW(smoother->smooth(max_its_path, max_time), nav2_core::FailedToSmoothPath); + EXPECT_TRUE(smoother->smooth(max_its_path, max_time)); } diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 91b1b846705..1cb400360cc 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.3.4 + 1.3.5 A sets of system-level tests for Nav2 usually involving full robot simulation Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 420c5322f4a..fd6c269294f 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.3.4 + 1.3.5 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index d652982aef3..38b2ef11a0a 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -168,6 +168,11 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode return nav2_util::CallbackReturn::SUCCESS; } + /** + * @brief Automatically configure and active the node + */ + void autostart(); + /** * @brief Perform preshutdown activities before our Context is shutdown. * Note that this is related to our Context's shutdown sequence, not the @@ -207,6 +212,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; double bond_heartbeat_period; + rclcpp::TimerBase::SharedPtr autostart_timer_; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 342f930ab9f..b391f59ddce 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -150,6 +150,9 @@ class SimpleActionServer std::lock_guard lock(update_mutex_); if (!server_active_) { + RCLCPP_INFO( + node_logging_interface_->get_logger(), + "Action server is inactive. Rejecting the goal."); return rclcpp_action::GoalResponse::REJECT; } diff --git a/nav2_util/package.xml b/nav2_util/package.xml index d43a35fdd0d..1168f181625 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.3.4 + 1.3.5 Nav2 utilities Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index 5976d098a86..3bc9dba1574 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -21,6 +21,8 @@ #include "lifecycle_msgs/msg/state.hpp" #include "nav2_util/node_utils.hpp" +using namespace std::chrono_literals; + namespace nav2_util { @@ -40,6 +42,14 @@ LifecycleNode::LifecycleNode( this, "bond_heartbeat_period", rclcpp::ParameterValue(0.1)); this->get_parameter("bond_heartbeat_period", bond_heartbeat_period); + bool autostart_node = false; + nav2_util::declare_parameter_if_not_declared( + this, "autostart_node", rclcpp::ParameterValue(false)); + this->get_parameter("autostart_node", autostart_node); + if (autostart_node) { + autostart(); + } + printLifecycleNodeNotification(); register_rcl_preshutdown_callback(); @@ -74,6 +84,24 @@ void LifecycleNode::createBond() } } +void LifecycleNode::autostart() +{ + using lifecycle_msgs::msg::State; + autostart_timer_ = this->create_wall_timer( + 0s, + [this]() -> void { + autostart_timer_->cancel(); + RCLCPP_INFO(get_logger(), "Auto-starting node: %s", this->get_name()); + if (configure().id() != State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(get_logger(), "Auto-starting node %s failed to configure!", this->get_name()); + return; + } + if (activate().id() != State::PRIMARY_STATE_ACTIVE) { + RCLCPP_ERROR(get_logger(), "Auto-starting node %s failed to activate!", this->get_name()); + } + }); +} + void LifecycleNode::runCleanups() { /* diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 5ecd95d60b9..0ae2ffe0c0e 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -258,6 +258,7 @@ class ActionTest : public ::testing::Test TEST_F(ActionTest, test_simple_action) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); node_->activate_server(); // The goal for this invocation @@ -272,6 +273,7 @@ TEST_F(ActionTest, test_simple_action) future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result auto future_result = node_->action_client_->async_get_result(goal_handle); @@ -320,6 +322,7 @@ TEST_F(ActionTest, test_simple_action_with_feedback) future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result auto future_result = node_->action_client_->async_get_result(goal_handle); @@ -364,6 +367,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) node_->deactivate_server(); auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result auto future_result = node_->action_client_->async_get_result(goal_handle); @@ -430,6 +434,7 @@ TEST_F(ActionTest, test_simple_action_preemption) future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result auto future_result = node_->action_client_->async_get_result(goal_handle); @@ -478,6 +483,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) // Get the results auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result of initial goal auto future_result = node_->action_client_->async_get_result(goal_handle); diff --git a/nav2_util/test/test_lifecycle_node.cpp b/nav2_util/test/test_lifecycle_node.cpp index 07ef0177d72..d29da5e6df4 100644 --- a/nav2_util/test/test_lifecycle_node.cpp +++ b/nav2_util/test/test_lifecycle_node.cpp @@ -26,6 +26,27 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; +class LifecycleTransitionTestNode : public nav2_util::LifecycleNode +{ +public: + explicit LifecycleTransitionTestNode(rclcpp::NodeOptions options) + : nav2_util::LifecycleNode("test_node", "", options) {} + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override + { + configured = true; + return nav2_util::CallbackReturn::SUCCESS; + } + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override + { + activated = true; + return nav2_util::CallbackReturn::SUCCESS; + } + + bool configured{false}; + bool activated{false}; +}; + // For the following two tests, if the LifecycleNode doesn't shut down properly, // the overall test will hang since the rclcpp thread will still be running, // preventing the executable from exiting (the test will hang) @@ -48,6 +69,30 @@ TEST(LifecycleNode, MultipleRclcppNodesExitCleanly) SUCCEED(); } +TEST(LifecycleNode, AutostartTransitions) +{ + auto executor = std::make_shared(); + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + executor->add_node(node->get_node_base_interface()); + executor->spin_some(); + EXPECT_FALSE(node->configured); + EXPECT_FALSE(node->activated); + executor.reset(); + node.reset(); + + + executor = std::make_shared(); + options.parameter_overrides({{"autostart_node", true}}); + node = std::make_shared(options); + executor->add_node(node->get_node_base_interface()); + executor->spin_some(); + EXPECT_TRUE(node->configured); + EXPECT_TRUE(node->activated); + executor.reset(); + node.reset(); +} + TEST(LifecycleNode, OnPreshutdownCbFires) { // Ensure the on_rcl_preshutdown_cb fires diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 2f925f1d414..fc0557e96a9 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.3.4 + 1.3.5 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 78cd7d1d4b0..852feba8d62 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.3.4 + 1.3.5 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 38d2f9c72c1..046ba692e29 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.3.4 + 1.3.5 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index dcabfa10b18..de6a4411e52 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.3.4 + 1.3.5 ROS2 Navigation Stack diff --git a/tools/bt2img.py b/tools/bt2img.py index 2b12d777d82..ab8307b2138 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -186,15 +186,13 @@ def add_nodes(dot, parent_dot_name, parent_node): # type, the name if provided, and the parameters. def make_label(node): label = "< " - label += "" + label += f"" name = node.get('name') if name: - label += "" + label += f"" - for (param_name, value) in node.items(): - label += ( - "" - ) + for param_name, value in node.items(): + label += f"" label += '
' + node.tag + '
{node.tag}
' + name + '
{name}
' + param_name + '=' + value + '
{param_name}={value}
>' return label