diff --git a/nav2_world_model/CMakeLists.txt b/nav2_world_model/CMakeLists.txt index 50e69049a24e56e8aa55d9980a8bb2cd52853bc2..eb0bf04f332142ee9bc9e619e99b50b8fe76bd5d 100644 --- a/nav2_world_model/CMakeLists.txt +++ b/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 ) diff --git a/nav2_world_model/include/nav2_world_model/world_model.hpp b/nav2_world_model/include/nav2_world_model/world_model.hpp index eef48c952e7a6f9de31020892fb1692e1a652fc2..a07c67655f7ca05600352cbd0fab7f983b1995c0 100644 --- a/nav2_world_model/include/nav2_world_model/world_model.hpp +++ b/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 diff --git a/nav2_world_model/package.xml b/nav2_world_model/package.xml index 8e54c3c8f1cfffd6533e9edcf27345c22b77844d..8da97d60ce321871a6d8be6a1667fe3fb0e1fb09 100644 --- a/nav2_world_model/package.xml +++ b/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> diff --git a/nav2_world_model/src/main.cpp b/nav2_world_model/src/main.cpp index d5bf7b4284229d9aee5be9fb1c473632308d051f..4fef67a9a835e90bf44cbe4488b0bb937f65cf82 100644 --- a/nav2_world_model/src/main.cpp +++ b/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; diff --git a/nav2_world_model/src/world_model.cpp b/nav2_world_model/src/world_model.cpp index 3af03d260e0df9cf4b73fd3c724b3bd6bc00ae48..44dfa3f1039549139e300f786f8b6574f6d5eea1 100644 --- a/nav2_world_model/src/world_model.cpp +++ b/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