costmap_filter_info_server.cpp

// Copyright (c) 2020 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// 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.
 
// TODO(AlexeyMerzlyakov): This dummy info publisher should be removed
// after Semantic Map Server having the same functionality will be developed.
 
#include "nav2_map_server/costmap_filter_info_server.hpp"
 
#include <string>
#include <memory>
#include <utility>
 
namespace nav2_map_server
{
 
CostmapFilterInfoServer::CostmapFilterInfoServer(const rclcpp::NodeOptions & options)
: nav2::LifecycleNode("costmap_filter_info_server", "", options)
{
}
 
CostmapFilterInfoServer::~CostmapFilterInfoServer()
{
}
 
nav2::CallbackReturn
CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Configuring");
  auto node = shared_from_this();
 
  std::string filter_info_topic = node->declare_or_get_parameter(
    "filter_info_topic", std::string("costmap_filter_info"));
 
  publisher_ = this->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(
    filter_info_topic, nav2::qos::LatchedPublisherQoS());
 
  msg_ = nav2_msgs::msg::CostmapFilterInfo();
  msg_.header.frame_id = "";
  msg_.header.stamp = now();
  msg_.type = node->declare_or_get_parameter("type", 0);
  msg_.filter_mask_topic = node->declare_or_get_parameter(
    "mask_topic", std::string("filter_mask"));
  msg_.base = static_cast<float>(
    node->declare_or_get_parameter("base", 0.0));
  msg_.multiplier = static_cast<float>(
    node->declare_or_get_parameter("multiplier", 1.0));
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Activating");
 
  publisher_->on_activate();
  auto costmap_filter_info = std::make_unique<nav2_msgs::msg::CostmapFilterInfo>(msg_);
  publisher_->publish(std::move(costmap_filter_info));
 
  // create bond connection
  createBond();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
CostmapFilterInfoServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Deactivating");
 
  publisher_->on_deactivate();
 
  // destroy bond connection
  destroyBond();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
CostmapFilterInfoServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Cleaning up");
 
  publisher_.reset();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
CostmapFilterInfoServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Shutting down");
 
  return nav2::CallbackReturn::SUCCESS;
}
 
}  // namespace nav2_map_server
 
#include "rclcpp_components/register_node_macro.hpp"
 
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::CostmapFilterInfoServer)