Port the world model lifecycle to lifecycle nodes (#654)

* Port the world model to lifecycle nodes
* Remove a couple stray printfs
* Address review feedback

nav2_world_model/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(rclcpp REQUIRED)
 find_package(nav2_util)
 find_package(nav2_msgs)
 find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_lifecycle REQUIRED)
 find_package(tf2_ros REQUIRED)
 
 nav2_package()
@@ -32,6 +33,7 @@ set(dependencies
   nav2_util
   nav2_msgs
   nav2_costmap_2d
+  nav2_lifecycle
   tf2_ros
 )
 

nav2_world_model/include/nav2_world_model/world_model.hpp

@@ -15,37 +15,50 @@
 #ifndef NAV2_WORLD_MODEL__WORLD_MODEL_HPP_
 #define NAV2_WORLD_MODEL__WORLD_MODEL_HPP_
 
-#include <string>
-#include <vector>
 #include <memory>
+#include <thread>
+
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "nav2_util/costmap.hpp"
+#include "nav2_lifecycle/lifecycle_node.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_msgs/srv/get_costmap.hpp"
-#include "tf2_ros/transform_listener.h"
 
 namespace nav2_world_model
 {
 
-class WorldModel : public rclcpp::Node
+class WorldModel : public nav2_lifecycle::LifecycleNode
 {
 public:
-  WorldModel(rclcpp::executor::Executor & executor, const std::string & name);
-  explicit WorldModel(rclcpp::executor::Executor & executor);
+  WorldModel();
+  ~WorldModel();
+
+protected:
+  // Implement the lifecycle interface
+  nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
+
+  // The WorldModel provides the GetCostmap service
+  rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
 
-private:
-  void costmap_callback(
+  // The callback for the GetCostmap service
+  void costmap_service_callback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request> request,
     const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response);
 
-  // Server for providing a costmap
-  rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmapServer_;
+  // The implementation of the WorldModel uses a Costmap2DROS node
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
-  nav2_costmap_2d::Costmap2D * costmap_;
-  tf2_ros::Buffer tfBuffer_;
-  tf2_ros::TransformListener tfListener_;
+
+  // The thread for the Costmap2DROS node
+  std::unique_ptr<std::thread> costmap_thread_;
+
+  // The frame_id and metadata layer values used in the service response message
+  static constexpr const char * frame_id_{"map"};
+  static constexpr const char * metadata_layer_{"Master"};
 };
 
 }  // namespace nav2_world_model

nav2_world_model/package.xml

@@ -8,18 +8,19 @@
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <build_depend>nav2_common</build_depend>
 
   <build_depend>rclcpp</build_depend>
   <build_depend>nav2_util</build_depend>
   <build_depend>nav2_msgs</build_depend>
   <build_depend>nav2_costmap_2d</build_depend>
+  <build_depend>nav2_lifecycle</build_depend>
   <build_depend>tf2_ros</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>nav2_util</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>nav2_costmap_2d</exec_depend>
+  <exec_depend>nav2_lifecycle</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>

nav2_world_model/src/main.cpp

@@ -10,19 +10,18 @@
 // distributed under the License is distributed on an "AS IS" BASIS,
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
-// limitations under the License. Reserved.
+// limitations under the License.
 
 #include <memory>
-#include "rclcpp/rclcpp.hpp"
+
 #include "nav2_world_model/world_model.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::executors::SingleThreadedExecutor exec;
-  auto world_model_node = std::make_shared<nav2_world_model::WorldModel>(exec);
-  exec.add_node(world_model_node);
-  exec.spin();
+  auto node = std::make_shared<nav2_world_model::WorldModel>();
+  rclcpp::spin(node->get_node_base_interface());
   rclcpp::shutdown();
 
   return 0;

nav2_world_model/src/world_model.cpp

@@ -12,67 +12,127 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <iostream>
-#include <memory>
-#include <vector>
-#include <string>
 #include "nav2_world_model/world_model.hpp"
 
-using std::vector;
-using std::string;
+#include <memory>
 
 namespace nav2_world_model
 {
 
-WorldModel::WorldModel(rclcpp::executor::Executor & executor, const string & name)
-: Node(name),
-  tfBuffer_(get_clock()),
-  tfListener_(tfBuffer_)
+WorldModel::WorldModel()
+: nav2_lifecycle::LifecycleNode("world_model")
+{
+  RCLCPP_INFO(get_logger(), "Creating");
+
+  // The costmap node is used in the implementation of the world model
+  costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("world_model_global_costmap");
+
+  // Launch a thread to run the costmap node
+  costmap_thread_ = std::make_unique<std::thread>(
+    [](rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+    {rclcpp::spin(node->get_node_base_interface());}, costmap_ros_);
+}
+
+WorldModel::~WorldModel()
+{
+  RCLCPP_INFO(get_logger(), "Destroying");
+  costmap_thread_->join();
+}
+
+nav2_lifecycle::CallbackReturn
+WorldModel::on_configure(const rclcpp_lifecycle::State & state)
 {
-  costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap", tfBuffer_);
-  costmap_ = costmap_ros_->getCostmap();
-  executor.add_node(costmap_ros_);
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  costmap_ros_->on_configure(state);
 
-  // Create a service that will use the callback function to handle requests.
-  costmapServer_ = create_service<nav2_msgs::srv::GetCostmap>("GetCostmap",
-      std::bind(&WorldModel::costmap_callback, this,
+  // Create a service that will use the callback function to handle requests
+  costmap_service_ = create_service<nav2_msgs::srv::GetCostmap>("GetCostmap",
+      std::bind(&WorldModel::costmap_service_callback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+WorldModel::on_activate(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  costmap_ros_->on_activate(state);
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+WorldModel::on_deactivate(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  costmap_ros_->on_deactivate(state);
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+WorldModel::on_cleanup(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  costmap_ros_->on_cleanup(state);
+  costmap_service_.reset();
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+WorldModel::on_error(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_ERROR(get_logger(), "Handling error state");
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
 }
 
-void WorldModel::costmap_callback(
+nav2_lifecycle::CallbackReturn
+WorldModel::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+void
+WorldModel::costmap_service_callback(
   const std::shared_ptr<rmw_request_id_t>/*request_header*/,
   const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request>/*request*/,
   const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response)
 {
-  RCLCPP_INFO(this->get_logger(), "Received costmap request");
+  RCLCPP_INFO(get_logger(), "Received costmap service request");
 
-  response->map.metadata.size_x = costmap_->getSizeInCellsX();
-  response->map.metadata.size_y = costmap_->getSizeInCellsY();
-  response->map.metadata.resolution = costmap_->getResolution();
-  response->map.metadata.layer = "Master";
-  response->map.metadata.map_load_time = now();
-  response->map.metadata.update_time = now();
-
-  tf2::Quaternion quaternion;
   // TODO(bpwilcox): Grab correct orientation information
-  quaternion.setRPY(0.0, 0.0, 0.0);  // set roll, pitch, yaw
+  tf2::Quaternion quaternion;
+  quaternion.setRPY(0.0, 0.0, 0.0);
+
+  nav2_costmap_2d::Costmap2D * costmap_ = costmap_ros_->getCostmap();
+
+  auto size_x = costmap_->getSizeInCellsX();
+  auto size_y = costmap_->getSizeInCellsY();
+  auto data_length = size_x * size_y;
+  unsigned char * data = costmap_->getCharMap();
+  auto current_time = now();
+
+  response->map.header.stamp = current_time;
+  response->map.header.frame_id = frame_id_;
+  response->map.metadata.size_x = size_x;
+  response->map.metadata.size_y = size_y;
+  response->map.metadata.resolution = costmap_->getResolution();
+  response->map.metadata.layer = metadata_layer_;
+  response->map.metadata.map_load_time = current_time;
+  response->map.metadata.update_time = current_time;
   response->map.metadata.origin.position.x = costmap_->getOriginX();
   response->map.metadata.origin.position.y = costmap_->getOriginY();
   response->map.metadata.origin.position.z = 0.0;
   response->map.metadata.origin.orientation = tf2::toMsg(quaternion);
-
-  response->map.header.stamp = now();
-  response->map.header.frame_id = "map";
-
-  unsigned char * data = costmap_->getCharMap();
-  auto data_length = response->map.metadata.size_x * response->map.metadata.size_y;
   response->map.data.resize(data_length);
   response->map.data.assign(data, data + data_length);
 }
 
-WorldModel::WorldModel(rclcpp::executor::Executor & executor)
-: WorldModel(executor, "world_model")
-{
-}
-
 }  // namespace nav2_world_model