Bunches of random new tests (#1909)

* trying to get A* to work again

* make flake 8 happy

* adding cancel and preempt test

* planner tests use A*

* adding A* note

* test with topic

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

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<NavFnPlannerTester>();
+  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<nav_msgs::msg::OccupancyGrid>("map", 1);

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

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='',

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
 

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.')
 

nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp

@@ -115,6 +115,7 @@ class WaypointFollower : public nav2_util::LifecycleNode
   std::unique_ptr<ActionServer> action_server_;
   ActionClient::SharedPtr nav_to_pose_client_;
   rclcpp::Node::SharedPtr client_node_;
+  std::shared_future<rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr> future_goal_handle_;
   bool stop_on_failure_;
   ActionStatus current_goal_status_;
   int loop_rate_;

nav2_waypoint_follower/src/waypoint_follower.cpp

@@ -132,7 +132,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;
     }
@@ -156,7 +157,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;
     }