Merge

doc/parameters/param_list.md

@@ -78,6 +78,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | `<static layer>`.subscribe_to_updates | false | Subscribe to static map updates after receiving first |
 | `<static layer>`.map_subscribe_transient_local | true | QoS settings for map topic |
 | `<static layer>`.transform_tolerance | 0.0 | TF tolerance |
+| `<static layer>`.map_topic | "" | Name of the map topic to subscribe to (empty means use the map_topic defined by `costmap_2d_ros`) |
 
 ## inflation_layer plugin
 
@@ -561,6 +562,7 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 | z_short | 0.05 | Mixture weight for z_short part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. |
 | always_reset_initial_pose | false | Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize |
 | scan_topic | scan | Topic to subscribe to in order to receive the laser scan for localization |
+| map_topic | map | Topic to subscribe to in order to receive the map for localization |
 
 ---
 

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -249,6 +249,7 @@ class AmclNode : public nav2_util::LifecycleNode
   double z_short_;
   double z_rand_;
   std::string scan_topic_{"scan"};
+  std::string map_topic_{"map"};
 };
 
 }  // namespace nav2_amcl

nav2_amcl/src/amcl_node.cpp

@@ -217,6 +217,10 @@ AmclNode::AmclNode()
   add_parameter(
     "scan_topic", rclcpp::ParameterValue("scan"),
     "Topic to subscribe to in order to receive the laser scan for localization");
+
+  add_parameter(
+    "map_topic", rclcpp::ParameterValue("map"),
+    "Topic to subscribe to in order to receive the map to localize on");
 }
 
 AmclNode::~AmclNode()
@@ -1102,6 +1106,7 @@ AmclNode::initParameters()
   get_parameter("first_map_only_", first_map_only_);
   get_parameter("always_reset_initial_pose", always_reset_initial_pose_);
   get_parameter("scan_topic", scan_topic_);
+  get_parameter("map_topic", map_topic_);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
   transform_tolerance_ = tf2::durationFromSec(tmp_tol);
@@ -1277,7 +1282,7 @@ AmclNode::initPubSub()
     std::bind(&AmclNode::initialPoseReceived, this, std::placeholders::_1));
 
   map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
-    "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    map_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&AmclNode::mapReceived, this, std::placeholders::_1));
 
   RCLCPP_INFO(get_logger(), "Subscribed to map topic.");

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -37,7 +37,7 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
   {
     return providedBasicPorts(
       {
-        BT::InputPort<double>("backup_dist", -0.15, "Distance to backup"),
+        BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup")
       });
   }

nav2_behavior_tree/plugins/action/back_up_action.cpp

@@ -31,11 +31,6 @@ BackUpAction::BackUpAction(
   double speed;
   getInput("backup_speed", speed);
 
-  // silently fix, vector direction determined by distance sign
-  if (speed < 0.0) {
-    speed *= -1.0;
-  }
-
   // Populate the input message
   goal_.target.x = dist;
   goal_.target.y = 0.0;

nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp

@@ -114,7 +114,7 @@ TEST_F(BackUpActionTestFixture, test_ports)
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
-  EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), -0.15);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 0.15);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.025);
 
   xml_txt =
@@ -136,7 +136,7 @@ TEST_F(BackUpActionTestFixture, test_tick)
     R"(
       <root main_tree_to_execute = "MainTree" >
         <BehaviorTree ID="MainTree">
-            <BackUp backup_dist="2" backup_speed="-0.26" />
+            <BackUp backup_dist="2" backup_speed="0.26" />
         </BehaviorTree>
       </root>)";
 

nav2_bt_navigator/src/bt_navigator.cpp

@@ -312,9 +312,6 @@ BtNavigator::navigateToPose()
       RCLCPP_INFO(get_logger(), "Navigation canceled");
       action_server_->terminate_all();
       break;
-
-    default:
-      throw std::logic_error("Invalid status return from BT");
   }
 }
 

nav2_controller/src/nav2_controller.cpp

@@ -278,13 +278,8 @@ void ControllerServer::computeControl()
 
     rclcpp::Rate loop_rate(controller_frequency_);
     while (rclcpp::ok()) {
-      if (action_server_ == nullptr) {
-        RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping.");
-        return;
-      }
-
-      if (!action_server_->is_server_active()) {
-        RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping.");
+      if (action_server_ == nullptr || !action_server_->is_server_active()) {
+        RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
         return;
       }
 

nav2_costmap_2d/plugins/static_layer.cpp

@@ -124,10 +124,18 @@ StaticLayer::getParameters()
   declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false));
   declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true));
   declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0));
+  declareParameter("map_topic", rclcpp::ParameterValue(""));
 
   node_->get_parameter(name_ + "." + "enabled", enabled_);
   node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
-  node_->get_parameter("map_topic", map_topic_);
+  std::string private_map_topic, global_map_topic;
+  node_->get_parameter(name_ + "." + "map_topic", private_map_topic);
+  node_->get_parameter("map_topic", global_map_topic);
+  if (!private_map_topic.empty()) {
+    map_topic_ = private_map_topic;
+  } else {
+    map_topic_ = global_map_topic;
+  }
   node_->get_parameter(
     name_ + "." + "map_subscribe_transient_local",
     map_subscribe_transient_local_);

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -325,6 +325,9 @@ DWBLocalPlanner::computeVelocityCommands(
 
   prepareGlobalPlan(pose, transformed_plan, goal_pose);
 
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
   for (TrajectoryCritic::Ptr critic : critics_) {
     if (critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan) == false) {
       RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare");
@@ -344,6 +347,8 @@ DWBLocalPlanner::computeVelocityCommands(
       critic->debrief(cmd_vel.velocity);
     }
 
+    lock.unlock();
+
     pub_->publishLocalPlan(pose.header, best.traj);
     pub_->publishCostGrid(costmap_ros_, critics_);
 
@@ -355,6 +360,9 @@ DWBLocalPlanner::computeVelocityCommands(
     for (TrajectoryCritic::Ptr critic : critics_) {
       critic->debrief(empty_cmd);
     }
+
+    lock.unlock();
+
     pub_->publishLocalPlan(pose.header, empty_traj);
     pub_->publishCostGrid(costmap_ros_, critics_);
 

nav2_navfn_planner/src/navfn_planner.cpp

@@ -174,13 +174,17 @@ NavfnPlanner::makePlan(
   // clear the starting cell within the costmap because we know it can't be an obstacle
   clearRobotCell(mx, my);
 
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+
   // make sure to resize the underlying array that Navfn uses
   planner_->setNavArr(
     costmap_->getSizeInCellsX(),
     costmap_->getSizeInCellsY());
 
   planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
 
+  lock.unlock();
+
   int map_start[2];
   map_start[0] = mx;
   map_start[1] = my;

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
@@ -214,13 +211,8 @@ PlannerServer::computePlan()
   auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
 
   try {
-    if (action_server_ == nullptr) {
-      RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping.");
-      return;
-    }
-
-    if (!action_server_->is_server_active()) {
-      RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping.");
+    if (action_server_ == nullptr || !action_server_->is_server_active()) {
+      RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
       return;
     }
 

nav2_recoveries/plugins/back_up.cpp

@@ -51,8 +51,9 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
       "will only move in X.");
   }
 
-  command_x_ = command->target.x;
-  command_speed_ = command->speed;
+  // Silently ensure that both the speed and direction are positive.
+  command_x_ = std::fabs(command->target.x);
+  command_speed_ = std::fabs(command->speed);
 
   if (!nav2_util::getCurrentPose(
       initial_pose_, *tf_, global_frame_, robot_base_frame_,
@@ -83,7 +84,7 @@ Status BackUp::onCycleUpdate()
   feedback_->distance_traveled = distance;
   action_server_->publish_feedback(feedback_);
 
-  if (distance >= abs(command_x_)) {
+  if (distance >= command_x_) {
     stopRobot();
     return Status::SUCCEEDED;
   }
@@ -92,7 +93,7 @@ Status BackUp::onCycleUpdate()
   auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
   cmd_vel->linear.y = 0.0;
   cmd_vel->angular.z = 0.0;
-  command_x_ < 0 ? cmd_vel->linear.x = -command_speed_ : cmd_vel->linear.x = command_speed_;
+  cmd_vel->linear.x = -command_speed_;
 
   geometry_msgs::msg::Pose2D pose2d;
   pose2d.x = current_pose.pose.position.x;

nav2_system_tests/CMakeLists.txt

@@ -55,6 +55,7 @@ if(BUILD_TESTING)
   add_subdirectory(src/planning)
   add_subdirectory(src/localization)
   add_subdirectory(src/system)
+  add_subdirectory(src/system_failure)
   add_subdirectory(src/updown)
   add_subdirectory(src/waypoint_follower)
   add_subdirectory(src/recoveries/spin)

nav2_system_tests/src/planning/planner_tester.cpp

@@ -67,6 +67,12 @@ 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_->set_parameter(
+    rclcpp::Parameter(std::string("expected_planner_frequency"), rclcpp::ParameterValue(-1.0)));
   planner_tester_->onConfigure(state);
   publishRobotTransform();
   map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("map", 1);

nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp

@@ -62,11 +62,16 @@ WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr;
 TEST_P(WaitRecoveryTestFixture, testSWaitRecovery)
 {
   float wait_time = std::get<0>(GetParam());
+  float cancel = std::get<1>(GetParam());
 
   bool success = false;
   int num_tries = 3;
   for (int i = 0; i != num_tries; i++) {
-    success = success || wait_recovery_tester->recoveryTest(wait_time);
+    if (cancel == 1.0) {
+      success = success || wait_recovery_tester->recoveryTestCancel(wait_time);
+    } else {
+      success = success || wait_recovery_tester->recoveryTest(wait_time);
+    }
     if (success) {
       break;
     }
@@ -81,7 +86,8 @@ INSTANTIATE_TEST_CASE_P(
   ::testing::Values(
     std::make_tuple(1.0, 0.0),
     std::make_tuple(2.0, 0.0),
-    std::make_tuple(5.0, 0.0)),
+    std::make_tuple(5.0, 0.0),
+    std::make_tuple(10.0, 1.0)),
   testNameGenerator);
 
 int main(int argc, char ** argv)

nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp

@@ -172,6 +172,83 @@ bool WaitRecoveryTester::recoveryTest(
   return true;
 }
 
+bool WaitRecoveryTester::recoveryTestCancel(
+  const float wait_time)
+{
+  if (!is_active_) {
+    RCLCPP_ERROR(node_->get_logger(), "Not activated");
+    return false;
+  }
+
+  // Sleep to let recovery server be ready for serving in multiple runs
+  std::this_thread::sleep_for(5s);
+
+  auto start_time = node_->now();
+  auto goal_msg = Wait::Goal();
+  goal_msg.time = rclcpp::Duration(wait_time, 0.0);
+
+  RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
+
+  auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
+
+  if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
+    return false;
+  }
+
+  // Wait for the server to be done with the goal
+  auto result_future = client_ptr_->async_cancel_all_goals();
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for cancellation");
+  if (rclcpp::spin_until_future_complete(node_, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "get cancel result call failed :(");
+    return false;
+  }
+
+  auto status = goal_handle_future.get()->get_status();
+
+  switch (status) {
+    case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal succeeded");
+      return false;
+    case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was aborted");
+      return false;
+    case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO(
+        node_->get_logger(),
+        "Goal was canceled");
+      return true;
+    case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO(
+        node_->get_logger(),
+        "Goal is cancelling");
+      return true;
+    case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal is executing");
+      return false;
+    case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal is processing");
+      return false;
+    default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
+      return false;
+  }
+
+  return false;
+}
+
 void WaitRecoveryTester::sendInitialPose()
 {
   geometry_msgs::msg::PoseWithCovarianceStamped pose;

nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp

@@ -52,6 +52,8 @@ class WaitRecoveryTester
   bool recoveryTest(
     float time);
 
+  bool recoveryTestCancel(float time);
+
   void activate();
 
   void deactivate();