timed_behavior.hpp

// Copyright (c) 2018 Intel Corporation
//
// 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.
 
#ifndef NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
#define NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
 
 
#include <cstdint>
#include <memory>
#include <string>
#include <cmath>
#include <chrono>
#include <ctime>
#include <thread>
#include <utility>
 
#include "rclcpp/rclcpp.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/twist_publisher.hpp"
#include "nav2_ros_common/simple_action_server.hpp"
#include "nav2_ros_common/rate.hpp"
#include "nav2_core/behavior.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.hpp"
#pragma GCC diagnostic pop
 
 
namespace nav2_behaviors
{
 
enum class Status : int8_t
{
  SUCCEEDED = 1,
  FAILED = 2,
  RUNNING = 3,
};
 
struct ResultStatus
{
  Status status;
  uint16_t error_code{0};
  std::string error_msg;
};
 
using namespace std::chrono_literals;  //NOLINT
 
template<typename ActionT>
class TimedBehavior : public nav2_core::Behavior
{
public:
  using ActionServer = nav2::SimpleActionServer<ActionT>;
 
  TimedBehavior()
  : action_server_(nullptr),
    cycle_frequency_(10.0),
    enabled_(false)
  {
  }
 
  virtual ~TimedBehavior() = default;
 
  // Derived classes can override this method to catch the command and perform some checks
  // before getting into the main loop. The method will only be called
  // once and should return SUCCEEDED otherwise behavior will return FAILED.
  virtual ResultStatus onRun(const std::shared_ptr<const typename ActionT::Goal> command) = 0;
 
 
  // This is the method derived classes should mainly implement
  // and will be called cyclically while it returns RUNNING.
  // Implement the behavior such that it runs some unit of work on each call
  // and provides a status. The Behavior will finish once SUCCEEDED is returned
  // It's up to the derived class to define the final commanded velocity.
  virtual ResultStatus onCycleUpdate() = 0;
 
  // an opportunity for derived classes to do something on configuration
  // if they chose
  virtual void onConfigure()
  {
  }
 
  // an opportunity for derived classes to do something on cleanup
  // if they chose
  virtual void onCleanup()
  {
  }
 
  // an opportunity for a derived class to do something on action completion
  virtual void onActionCompletion(std::shared_ptr<typename ActionT::Result>/*result*/)
  {
  }
 
  // configure the server on lifecycle setup
  void configure(
    const nav2::LifecycleNode::WeakPtr & parent,
    const std::string & name, nav2::TransformBuffer::SharedPtr tf,
    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> local_collision_checker,
    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> global_collision_checker)
  override
  {
    node_ = parent;
    auto node = node_.lock();
    logger_ = node->get_logger();
    clock_ = node->get_clock();
 
    RCLCPP_INFO(logger_, "Configuring %s", name.c_str());
 
    behavior_name_ = name;
    tf_ = tf;
 
    node->get_parameter("cycle_frequency", cycle_frequency_);
    node->get_parameter("local_frame", local_frame_);
    node->get_parameter("global_frame", global_frame_);
    node->get_parameter("robot_base_frame", robot_base_frame_);
    transform_staleness_threshold_ = node->declare_or_get_parameter(
      "transform_staleness_threshold", 0.0);
 
    action_server_ = node->create_action_server<ActionT>(
      behavior_name_,
      std::bind(&TimedBehavior::execute, this), nullptr, nullptr, std::chrono::milliseconds(
        500), false);
 
    local_collision_checker_ = local_collision_checker;
    global_collision_checker_ = global_collision_checker;
 
    vel_pub_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel");
 
    onConfigure();
  }
 
  // Cleanup server on lifecycle transition
  void cleanup() override
  {
    action_server_.reset();
    vel_pub_.reset();
    onCleanup();
  }
 
  // Activate server on lifecycle transition
  void activate() override
  {
    RCLCPP_INFO(logger_, "Activating %s", behavior_name_.c_str());
 
    vel_pub_->on_activate();
    action_server_->activate();
    enabled_ = true;
  }
 
  // Deactivate server on lifecycle transition
  void deactivate() override
  {
    vel_pub_->on_deactivate();
    action_server_->deactivate();
    enabled_ = false;
  }
 
protected:
  bool getCurrentPoseChecked(geometry_msgs::msg::PoseStamped & pose)
  {
    if (!nav2_util::getFreshPose(
        *tf_, local_frame_, robot_base_frame_, clock_->now(),
        transform_staleness_threshold_, pose))
    {
      return false;
    }
    return true;
  }
 
  nav2::LifecycleNode::WeakPtr node_;
 
  std::string behavior_name_;
  std::unique_ptr<nav2_util::TwistPublisher> vel_pub_;
  typename ActionServer::SharedPtr action_server_;
  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> local_collision_checker_;
  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> global_collision_checker_;
  nav2::TransformBuffer::SharedPtr tf_;
 
  double cycle_frequency_;
  double enabled_;
  std::string local_frame_;
  std::string global_frame_;
  std::string robot_base_frame_;
  double transform_staleness_threshold_{0.0};
  rclcpp::Duration elapsed_time_{0, 0};
 
  // Clock
  rclcpp::Clock::SharedPtr clock_;
 
  // Logger
  rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")};
 
  // Main execution callbacks for the action server implementation calling the Behavior's
  // onRun and cycle functions to execute a specific behavior
  void execute()
  {
    RCLCPP_INFO(logger_, "Running %s", behavior_name_.c_str());
 
    if (!enabled_) {
      RCLCPP_WARN(
        logger_,
        "Called while inactive, ignoring request.");
      return;
    }
 
    // Initialize the ActionT result
    auto result = std::make_shared<typename ActionT::Result>();
 
    ResultStatus on_run_result = onRun(action_server_->get_current_goal());
    if (on_run_result.status != Status::SUCCEEDED) {
      result->error_code = on_run_result.error_code;
      result->error_msg = on_run_result.error_msg;
      RCLCPP_INFO(
        logger_, "Initial checks failed for %s - %s", behavior_name_.c_str(),
        on_run_result.error_msg.c_str());
      action_server_->terminate_current(result);
      return;
    }
 
    auto start_time = clock_->now();
    auto node = node_.lock();
    if (!node) {
      RCLCPP_ERROR(
        logger_,
        "Failed to run %s because the parent node is no longer available.",
        behavior_name_.c_str());
      result->error_msg = behavior_name_ + " failed: parent node expired";
      result->total_elapsed_time = clock_->now() - start_time;
      onActionCompletion(result);
      action_server_->terminate_current(result);
      return;
    }
    nav2::Rate loop_rate(node, cycle_frequency_);
 
    while (rclcpp::ok()) {
      elapsed_time_ = clock_->now() - start_time;
      // TODO(orduno) #868 Enable preempting a Behavior on-the-fly without stopping
      if (action_server_->is_preempt_requested()) {
        RCLCPP_ERROR(
          logger_, "Received a preemption request for %s,"
          " however feature is currently not implemented. Aborting and stopping.",
          behavior_name_.c_str());
        stopRobot();
        result->total_elapsed_time = clock_->now() - start_time;
        onActionCompletion(result);
        action_server_->terminate_current(result);
        return;
      }
 
      if (action_server_->is_cancel_requested()) {
        RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str());
        stopRobot();
        result->total_elapsed_time = elapsed_time_;
        onActionCompletion(result);
        action_server_->terminate_all(result);
        return;
      }
 
      ResultStatus on_cycle_update_result = onCycleUpdate();
      switch (on_cycle_update_result.status) {
        case Status::SUCCEEDED:
          RCLCPP_INFO(
            logger_,
            "%s completed successfully", behavior_name_.c_str());
          result->total_elapsed_time = clock_->now() - start_time;
          onActionCompletion(result);
          action_server_->succeeded_current(result);
          return;
 
        case Status::FAILED:
          result->error_code = on_cycle_update_result.error_code;
          result->error_msg = behavior_name_ + " failed:" + on_cycle_update_result.error_msg;
          RCLCPP_WARN(logger_, "%s", result->error_msg.c_str());
          result->total_elapsed_time = clock_->now() - start_time;
          onActionCompletion(result);
          action_server_->terminate_current(result);
          return;
 
        case Status::RUNNING:
 
        default:
          loop_rate.sleep();
          break;
      }
    }
  }
 
  // Stop the robot with a commanded velocity
  void stopRobot()
  {
    auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
    cmd_vel->header.frame_id = robot_base_frame_;
    cmd_vel->header.stamp = clock_->now();
    cmd_vel->twist.linear.x = 0.0;
    cmd_vel->twist.linear.y = 0.0;
    cmd_vel->twist.angular.z = 0.0;
 
    vel_pub_->publish(std::move(cmd_vel));
  }
};
 
}  // namespace nav2_behaviors
 
#endif  // NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_