Merge branch 'ros-navigation:main' into eigen_mppi

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

@@ -52,17 +52,23 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({
+    return providedBasicPorts(
+      {
         BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
-        BT::InputPort<double>("cost_threshold", 254.0,
+        BT::InputPort<double>(
+          "cost_threshold", 254.0,
           "Cost threshold for considering a goal in collision"),
         BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
+        BT::InputPort<bool>(
+          "consider_unknown_as_obstacle", false,
+          "Whether to consider unknown cost as obstacle"),
         BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
-    });
+      });
   }
 
 private:
   bool use_footprint_;
+  bool consider_unknown_as_obstacle_;
   double cost_threshold_;
   Goals input_goals_;
 };

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -37,6 +37,7 @@ void RemoveInCollisionGoals::on_tick()
   getInput("use_footprint", use_footprint_);
   getInput("cost_threshold", cost_threshold_);
   getInput("input_goals", input_goals_);
+  getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
 
   if (input_goals_.empty()) {
     setOutput("output_goals", input_goals_);
@@ -47,11 +48,7 @@ void RemoveInCollisionGoals::on_tick()
   request_->use_footprint = use_footprint_;
 
   for (const auto & goal : input_goals_) {
-    geometry_msgs::msg::Pose2D pose;
-    pose.x = goal.pose.position.x;
-    pose.y = goal.pose.position.y;
-    pose.theta = tf2::getYaw(goal.pose.orientation);
-    request_->poses.push_back(pose);
+    request_->poses.push_back(goal);
   }
 }
 
@@ -60,10 +57,18 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
 {
   Goals valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
-    if (response->costs[i] < cost_threshold_) {
+    if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
+      response->costs[i] < cost_threshold_)
+    {
       valid_goal_poses.push_back(input_goals_[i]);
     }
   }
+  // Inform if all goals have been removed
+  if (valid_goal_poses.empty()) {
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "All goals are in collision and have been removed from the list");
+  }
   setOutput("output_goals", valid_goal_poses);
   return BT::NodeStatus::SUCCESS;
 }

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -176,6 +176,10 @@ class ControllerServer : public nav2_util::LifecycleNode
    * @brief Calls velocity publisher to publish zero velocity
    */
   void publishZeroVelocity();
+  /**
+   * @brief Called on goal exit
+   */
+  void onGoalExit();
   /**
    * @brief Checks if goal is reached
    * @return true or false
@@ -267,6 +271,7 @@ class ControllerServer : public nav2_util::LifecycleNode
 
   double failure_tolerance_;
   bool use_realtime_priority_;
+  bool publish_zero_velocity_;
   rclcpp::Duration costmap_update_timeout_;
 
   // Whether we've published the single controller warning yet

nav2_controller/src/controller_server.cpp

@@ -64,6 +64,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 
   declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0));
   declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false));
+  declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true));
   declare_parameter("costmap_update_timeout", 0.30);  // 300ms
 
   // The costmap node is used in the implementation of the controller
@@ -130,6 +131,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   get_parameter("speed_limit_topic", speed_limit_topic);
   get_parameter("failure_tolerance", failure_tolerance_);
   get_parameter("use_realtime_priority", use_realtime_priority_);
+  get_parameter("publish_zero_velocity", publish_zero_velocity_);
 
   costmap_ros_->configure();
   // Launch a thread to run the costmap node
@@ -476,7 +478,7 @@ void ControllerServer::computeControl()
         if (controllers_[current_controller_]->cancel()) {
           RCLCPP_INFO(get_logger(), "Cancellation was successful. Stopping the robot.");
           action_server_->terminate_all();
-          publishZeroVelocity();
+          onGoalExit();
           return;
         } else {
           RCLCPP_INFO_THROTTLE(
@@ -513,63 +515,63 @@ void ControllerServer::computeControl()
     }
   } catch (nav2_core::InvalidController & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::INVALID_CONTROLLER;
     action_server_->terminate_current(result);
     return;
   } catch (nav2_core::ControllerTFError & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::TF_ERROR;
     action_server_->terminate_current(result);
     return;
   } catch (nav2_core::NoValidControl & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::NO_VALID_CONTROL;
     action_server_->terminate_current(result);
     return;
   } catch (nav2_core::FailedToMakeProgress & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::FAILED_TO_MAKE_PROGRESS;
     action_server_->terminate_current(result);
     return;
   } catch (nav2_core::PatienceExceeded & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::PATIENCE_EXCEEDED;
     action_server_->terminate_current(result);
     return;
   } catch (nav2_core::InvalidPath & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::INVALID_PATH;
     action_server_->terminate_current(result);
     return;
   } catch (nav2_core::ControllerTimedOut & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::CONTROLLER_TIMED_OUT;
     action_server_->terminate_current(result);
     return;
   } catch (nav2_core::ControllerException & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::UNKNOWN;
     action_server_->terminate_current(result);
     return;
   } catch (std::exception & e) {
     RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-    publishZeroVelocity();
+    onGoalExit();
     std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
     result->error_code = Action::Result::UNKNOWN;
     action_server_->terminate_current(result);
@@ -578,7 +580,7 @@ void ControllerServer::computeControl()
 
   RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result");
 
-  publishZeroVelocity();
+  onGoalExit();
 
   // TODO(orduno) #861 Handle a pending preemption and set controller name
   action_server_->succeeded_current();
@@ -746,6 +748,13 @@ void ControllerServer::publishZeroVelocity()
   velocity.header.frame_id = costmap_ros_->getBaseFrameID();
   velocity.header.stamp = now();
   publishVelocity(velocity);
+}
+
+void ControllerServer::onGoalExit()
+{
+  if (publish_zero_velocity_) {
+    publishZeroVelocity();
+  }
 
   // Reset the state of the controllers after the task has ended
   ControllerMap::iterator it;

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -254,7 +254,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Service to get the cost at a point
   get_cost_service_ = create_service<nav2_msgs::srv::GetCosts>(
     "get_cost_" + getName(),
-    std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2,
+    std::bind(
+      &Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 
   // Add cleaning service
@@ -835,26 +836,35 @@ void Costmap2DROS::getCostsCallback(
   Costmap2D * costmap = layered_costmap_->getCostmap();
 
   for (const auto & pose : request->poses) {
-    bool in_bounds = costmap->worldToMap(pose.x, pose.y, mx, my);
+    geometry_msgs::msg::PoseStamped pose_transformed;
+    transformPoseToGlobalFrame(pose, pose_transformed);
+    bool in_bounds = costmap->worldToMap(
+      pose_transformed.pose.position.x,
+      pose_transformed.pose.position.y, mx, my);
 
     if (!in_bounds) {
-      response->costs.push_back(-1.0);
+      response->costs.push_back(NO_INFORMATION);
       continue;
     }
+    double yaw = tf2::getYaw(pose_transformed.pose.orientation);
 
     if (request->use_footprint) {
       Footprint footprint = layered_costmap_->getFootprint();
       FootprintCollisionChecker<Costmap2D *> collision_checker(costmap);
 
       RCLCPP_DEBUG(
         get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)",
-        pose.x, pose.y, pose.theta);
+        pose_transformed.pose.position.x, pose_transformed.pose.position.y, yaw);
 
       response->costs.push_back(
-        collision_checker.footprintCostAtPose(pose.x, pose.y, pose.theta, footprint));
+        collision_checker.footprintCostAtPose(
+          pose_transformed.pose.position.x,
+          pose_transformed.pose.position.y, yaw, footprint));
     } else {
       RCLCPP_DEBUG(
-        get_logger(), "Received request to get cost at point (%f, %f)", pose.x, pose.y);
+        get_logger(), "Received request to get cost at point (%f, %f)",
+          pose_transformed.pose.position.x,
+        pose_transformed.pose.position.y);
 
       // Get the cost at the map coordinates
       response->costs.push_back(static_cast<float>(costmap->getCost(mx, my)));

nav2_costmap_2d/src/layered_costmap.cpp

@@ -72,11 +72,11 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo
   footprint_(std::make_shared<std::vector<geometry_msgs::msg::Point>>())
 {
   if (track_unknown) {
-    primary_costmap_.setDefaultValue(255);
-    combined_costmap_.setDefaultValue(255);
+    primary_costmap_.setDefaultValue(NO_INFORMATION);
+    combined_costmap_.setDefaultValue(NO_INFORMATION);
   } else {
-    primary_costmap_.setDefaultValue(0);
-    combined_costmap_.setDefaultValue(0);
+    primary_costmap_.setDefaultValue(FREE_SPACE);
+    combined_costmap_.setDefaultValue(FREE_SPACE);
   }
 }
 

nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp

@@ -50,9 +50,9 @@ class GetCostServiceTest : public ::testing::Test
 TEST_F(GetCostServiceTest, TestWithoutFootprint)
 {
   auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
-  geometry_msgs::msg::Pose2D pose;
-  pose.x = 0.5;
-  pose.y = 1.0;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 0.5;
+  pose.pose.position.y = 1.0;
   request->poses.push_back(pose);
   request->use_footprint = false;
 
@@ -72,10 +72,12 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint)
 TEST_F(GetCostServiceTest, TestWithFootprint)
 {
   auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
-  geometry_msgs::msg::Pose2D pose;
-  pose.x = 0.5;
-  pose.y = 1.0;
-  pose.theta = 0.5;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 0.5;
+  pose.pose.position.y = 1.0;
+  tf2::Quaternion q;
+  q.setRPY(0, 0, 0.5);
+  pose.pose.orientation = tf2::toMsg(q);
   request->poses.push_back(pose);
   request->use_footprint = true;
 

nav2_graceful_controller/README.md

@@ -1,5 +1,5 @@
 # Graceful Motion Controller
-The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow.
+The graceful motion controller implements a controller based on the works of Jong Jin Park in "A Smooth Control Law for Graceful Motion of Differential Wheeled Mobile Robots in 2D Environment". In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow.
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions.
 

nav2_msgs/srv/GetCosts.srv

@@ -1,6 +1,6 @@
 # Get costmap costs at given poses
 
 bool use_footprint
-geometry_msgs/Pose2D[] poses
+geometry_msgs/PoseStamped[] poses
 ---
 float32[] costs

nav2_rviz_plugins/src/costmap_cost_tool.cpp

@@ -94,23 +94,28 @@ int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event)
 
 void CostmapCostTool::callCostService(float x, float y)
 {
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   // Create request for local costmap
   auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
-  geometry_msgs::msg::Pose2D pose;
-  pose.x = x;
-  pose.y = y;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = context_->getFixedFrame().toStdString();
+  pose.header.stamp = node->now();
+  pose.pose.position.x = x;
+  pose.pose.position.y = y;
   request->poses.push_back(pose);
   request->use_footprint = false;
 
   // Call local costmap service
   if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) {
-    local_cost_client_->async_send_request(request,
+    local_cost_client_->async_send_request(
+      request,
       std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1));
   }
 
   // Call global costmap service
   if (global_cost_client_->wait_for_service(std::chrono::seconds(1))) {
-    global_cost_client_->async_send_request(request,
+    global_cost_client_->async_send_request(
+      request,
       std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1));
   }
 }
@@ -120,23 +125,15 @@ void CostmapCostTool::handleLocalCostResponse(
 {
   rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
-  if (response->costs[0] != -1) {
-    RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]);
-  } else {
-    RCLCPP_ERROR(node->get_logger(), "Failed to get local costmap cost");
-  }
+  RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]);
 }
 
 void CostmapCostTool::handleGlobalCostResponse(
   rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedFuture future)
 {
   rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
-  if (response->costs[0] != -1) {
-    RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]);
-  } else {
-    RCLCPP_ERROR(node->get_logger(), "Failed to get global costmap cost");
-  }
+  RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]);
 }
 }  // namespace nav2_rviz_plugins
 

nav2_util/include/nav2_util/simple_action_server.hpp

@@ -582,7 +582,7 @@ class SimpleActionServer
 
     if (is_active(handle)) {
       if (handle->is_canceling()) {
-        warn_msg("Client requested to cancel the goal. Cancelling.");
+        info_msg("Client requested to cancel the goal. Cancelling.");
         handle->canceled(result);
       } else {
         warn_msg("Aborting handle.");