diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6e42c131b79..c095eb03ad0 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -66,10 +66,7 @@ PlannerServer::PlannerServer() PlannerServer::~PlannerServer() { RCLCPP_INFO(get_logger(), "Destroying"); - PlannerMap::iterator it; - for (it = planners_.begin(); it != planners_.end(); ++it) { - it->second.reset(); - } + planners_.clear(); } nav2_util::CallbackReturn diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 534b8237301..4c65a5a7b2a 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -67,6 +67,10 @@ void PlannerTester::activate() // The navfn wrapper auto state = rclcpp_lifecycle::State(); planner_tester_ = std::make_shared(); + planner_tester_->declare_parameter( + "GridBased.use_astar", rclcpp::ParameterValue(true)); + planner_tester_->set_parameter( + rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true))); planner_tester_->onConfigure(state); publishRobotTransform(); map_pub_ = this->create_publisher("map", 1); diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 09017bfc6ca..a7ae2aa43fa 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -1,3 +1,6 @@ +# NOTE: The ASTAR=True does not work currently due to remapping not functioning +# All set to false, A* testing of NavFn happens in the planning test portion + ament_add_test(test_bt_navigator GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" @@ -9,7 +12,7 @@ ament_add_test(test_bt_navigator TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=True + ASTAR=False ) ament_add_test(test_bt_navigator_with_dijkstra @@ -37,7 +40,7 @@ ament_add_test(test_dynamic_obstacle TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=True + ASTAR=False ) # ament_add_test(test_multi_robot diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index dde6618cdc7..92ea252eba0 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -41,7 +41,8 @@ def generate_launch_description(): params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') # Replace the `use_astar` setting on the params file - param_substitutions = {'GridBased.use_astar': os.getenv('ASTAR')} + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR')} configured_params = RewrittenYaml( source_file=params_file, root_key='', diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py index a66e2f7b89d..83e9df9dc9f 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/tester_node.py @@ -228,12 +228,8 @@ def run_all_tests(robot_tester): robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() - # TODO(orduno) Test sending the navigation request through the topic interface. - # Need to update tester to receive multiple goal poses. - # Need to fix bug with shutting down while bt_navigator - # is still running. - # if (result): - # result = test_RobotMovesToGoal(robot_tester) + if (result): + result = test_RobotMovesToGoal(robot_tester) # Add more tests here if desired diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 31bd9effbf2..97ad511675f 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -37,6 +37,7 @@ def __init__(self): self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', 10) self.initial_pose_received = False + self.goal_handle = None pose_qos = QoSProfile( durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, @@ -71,7 +72,7 @@ def setWaypoints(self, waypoints): msg.pose.orientation.w = 1.0 self.waypoints.append(msg) - def run(self): + def run(self, block): if not self.waypoints: rclpy.error_msg('Did not set valid waypoints before running test!') return False @@ -86,16 +87,19 @@ def run(self): send_goal_future = self.action_client.send_goal_async(action_request) try: rclpy.spin_until_future_complete(self, send_goal_future) - goal_handle = send_goal_future.result() + self.goal_handle = send_goal_future.result() except Exception as e: self.error_msg('Service call failed %r' % (e,)) - if not goal_handle.accepted: + if not self.goal_handle.accepted: self.error_msg('Goal rejected') return False self.info_msg('Goal accepted') - get_result_future = goal_handle.get_result_async() + if not block: + return True + + get_result_future = self.goal_handle.get_result_async() self.info_msg("Waiting for 'FollowWaypoints' action to complete") try: @@ -148,14 +152,18 @@ def shutdown(self): except Exception as e: self.error_msg('Service call failed %r' % (e,)) + def cancel_goal(self): + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + def info_msg(self, msg: str): - self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + self.get_logger().info(msg) def warn_msg(self, msg: str): - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warn(msg) def error_msg(self, msg: str): - self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + self.get_logger().error(msg) def main(argv=sys.argv[1:]): @@ -179,7 +187,19 @@ def main(argv=sys.argv[1:]): test.info_msg('Waiting for amcl_pose to be received') rclpy.spin_once(test, timeout_sec=1.0) # wait for poseCallback - result = test.run() + result = test.run(True) + + # preempt with new point + test.setWaypoints([starting_pose]) + result = test.run(False) + time.sleep(2) + test.setWaypoints([wps[1]]) + result = test.run(False) + + # cancel + time.sleep(2) + test.cancel_goal() + test.shutdown() test.info_msg('Done Shutting Down.') diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 98d85f932b2..777c74ff8d5 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -120,6 +120,7 @@ class WaypointFollower : public nav2_util::LifecycleNode std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; rclcpp::Node::SharedPtr client_node_; + std::shared_future::SharedPtr> future_goal_handle_; bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 69407589253..17419ccac00 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -139,7 +139,8 @@ WaypointFollower::followWaypoints() while (rclcpp::ok()) { // Check if asked to stop processing action if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(get_logger(), "Cancelling action."); + auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); + rclcpp::spin_until_future_complete(client_node_, cancel_future); action_server_->terminate_all(); return; } @@ -163,7 +164,7 @@ WaypointFollower::followWaypoints() std::bind(&WaypointFollower::resultCallback, this, std::placeholders::_1); send_goal_options.goal_response_callback = std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); - auto future_goal_handle = + future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); current_goal_status_ = ActionStatus::PROCESSING; }