input_at_waypoint.cpp

// Copyright (c) 2020 Samsung Research America
// 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.
 
#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
 
#include <string>
#include <exception>
 
#include "pluginlib/class_list_macros.hpp"
 
#include "nav2_ros_common/node_utils.hpp"
#include "nav2_ros_common/rate.hpp"
 
namespace nav2_waypoint_follower
{
 
using std::placeholders::_1;
 
InputAtWaypoint::InputAtWaypoint()
: input_received_(false),
  is_enabled_(true),
  timeout_(10.0, 0.0)
{
}
 
InputAtWaypoint::~InputAtWaypoint()
{
}
 
void InputAtWaypoint::initialize(
  const nav2::LifecycleNode::WeakPtr & parent,
  const std::string & plugin_name)
{
  auto node = parent.lock();
 
  if (!node) {
    throw std::runtime_error{"Failed to lock node in input at waypoint plugin!"};
  }
 
  node_ = parent;
  logger_ = node->get_logger();
  clock_ = node->get_clock();
 
  double timeout;
  std::string input_topic;
  is_enabled_ = node->declare_or_get_parameter(
    plugin_name + ".enabled", true);
  input_topic = node->declare_or_get_parameter(
    plugin_name + ".input_topic", std::string("input_at_waypoint/input"));
  timeout = node->declare_or_get_parameter(
    plugin_name + ".timeout", 10.0);
  timeout_ = rclcpp::Duration(timeout, 0.0);
 
  RCLCPP_INFO(
    logger_, "InputAtWaypoint: Subscribing to input topic %s.", input_topic.c_str());
  subscription_ = node->create_subscription<std_msgs::msg::Empty>(
    input_topic, std::bind(&InputAtWaypoint::Cb, this, _1));
}
 
void InputAtWaypoint::Cb(const std_msgs::msg::Empty::ConstSharedPtr & /*msg*/)
{
  std::lock_guard<std::mutex> lock(mutex_);
  input_received_ = true;
}
 
bool InputAtWaypoint::processAtWaypoint(
  const geometry_msgs::msg::PoseStamped & /*curr_pose*/,
  const int & curr_waypoint_index)
{
  if (!is_enabled_) {
    return true;
  }
 
  input_received_ = false;
 
  auto node = node_.lock();
  if (!node) {
    RCLCPP_ERROR(logger_, "Failed to lock node in input at waypoint plugin.");
    return false;
  }
 
  rclcpp::Time start = clock_->now();
  nav2::Rate r(node, 50);
  bool input_received = false;
  while (clock_->now() - start < timeout_) {
    {
      std::lock_guard<std::mutex> lock(mutex_);
      input_received = input_received_;
    }
 
    if (input_received) {
      return true;
    }
 
    r.sleep();
  }
 
  RCLCPP_WARN(
    logger_, "Unable to get external input at wp %i. Moving on.", curr_waypoint_index);
  return false;
}
 
}  // namespace nav2_waypoint_follower
 
PLUGINLIB_EXPORT_CLASS(
  nav2_waypoint_follower::InputAtWaypoint,
  nav2_core::WaypointTaskExecutor)