toid/toid_behaviors/include/toid_behaviors/simple_move.hpp

#pragma once
#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_core/behavior.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/twist_publisher.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "toid_msgs/action/simple_rotate.hpp"
#include "toid_msgs/msg/rival.hpp"

namespace toid
{

template <typename ActionT>
class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
{
public:
  using Rival = toid_msgs::msg::Rival;

  virtual void configureCB() {}
  virtual void cleanupCB() {}

  virtual void activateCB() {}
  virtual void deactivateCB() {}

  virtual nav2_behaviors::ResultStatus onStart(
    const std::shared_ptr<const typename ActionT::Goal> command,
    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) = 0;

  virtual nav2_behaviors::ResultStatus updateVel(
    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
    geometry_msgs::msg::Twist & out_vel) = 0;

  void onConfigure() override
  {
    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
    nav2_util::declare_parameter_if_not_declared(
      node, "odom_topic", rclcpp::ParameterValue("/odom"));
    std::string odom_topic_name = node->get_parameter("odom_topic").as_string();

    nav2_util::declare_parameter_if_not_declared(node, "robot_width", rclcpp::ParameterValue(0.30));
    node->get_parameter("robot_width", robot_width_);

    nav2_util::declare_parameter_if_not_declared(
      node, "robot_length", rclcpp::ParameterValue(0.30));
    node->get_parameter("robot_length", robot_length_);

    nav2_util::declare_parameter_if_not_declared(
      node, "rival_radius", rclcpp::ParameterValue(0.30));
    node->get_parameter("rival_radius", rival_radius_);

    odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
      odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
        std::lock_guard lock(mut_);
        current_vel_ = msg.twist.twist;
      });
    control_duration_ = 1.0 / this->cycle_frequency_;
    configureCB();
  }

  void onCleanup() override
  {
    odom_sub_.reset();
    cleanupCB();
  }

  void activate() override
  {
    nav2_behaviors::TimedBehavior<ActionT>::activate();
    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
    using namespace std::placeholders;
    rivals_sub_ = node->create_subscription<Rival>(
      "/dynamicObstacle", 1, std::bind(&SimpleMove<ActionT>::rival_cb, this, _1));
    activateCB();
  }

  void deactivate() override
  {
    nav2_behaviors::TimedBehavior<ActionT>::deactivate();
    rivals_sub_.reset();
    deactivateCB();
  }

  nav2_behaviors::ResultStatus onRun(
    const std::shared_ptr<const typename ActionT::Goal> command) override
  {
    geometry_msgs::msg::PoseStamped pose;
    geometry_msgs::msg::Twist vel;
    {
      std::lock_guard lock(mut_);
      vel = current_vel_;
    }

    if (!nav2_util::getCurrentPose(
          pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
          this->transform_tolerance_)) {
      return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED};
    }

    return onStart(command, pose.pose, vel);
  }

  nav2_behaviors::ResultStatus onCycleUpdate() override
  {
    geometry_msgs::msg::PoseStamped pose;
    geometry_msgs::msg::Twist vel;
    geometry_msgs::msg::Twist out_vel;
    auto vel_p = std::make_unique<geometry_msgs::msg::TwistStamped>();
    {
      std::lock_guard lock(mut_);
      vel = current_vel_;
    }

    if (!nav2_util::getCurrentPose(
          pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
          this->transform_tolerance_)) {
      return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED};
    }

    nav2_behaviors::ResultStatus r = updateVel(pose.pose, vel, out_vel);
    vel_p->twist = out_vel;
    vel_p->header.stamp = this->clock_->now();
    vel_p->header.frame_id = this->robot_base_frame_;
    this->vel_pub_->publish(std::move(vel_p));
    return r;
  }

  bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
  {
    if (!rivals_ || (this->clock_->now() - rivals_->header.stamp ) > rclcpp::Duration::from_seconds(1.0)) {
      return false;
    }
    const double cosp = std::cos(pose.theta);
    const double sinp = std::sin(pose.theta);
    int i = 0;
    for (const auto & rival : rivals_->point) {
      geometry_msgs::msg::Point local_rival;
      const double dx = rival.x - pose.x;
      const double dy = rival.y - pose.y;
      local_rival.x = dx * cosp - dy * sinp;
      local_rival.y = dx * sinp + dy * cosp;

      const double qx = std::abs(local_rival.x) - robot_length_ / 2.0;
      const double qy = std::abs(local_rival.y) - robot_width_ / 2.0;

      const double mqx = std::max(qx, 0.0);
      const double mqy = std::max(qy, 0.0);

      double length = std::sqrt(mqx * mqx + mqy * mqy);

      double sdf = length + std::min(std::max(qx, qy), 0.0);
      RCLCPP_INFO(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_);
      if (sdf < rival_radius_) {
        return true;
      }
    }
    return false;
  }

  void rival_cb(Rival::SharedPtr msg) { rivals_ = msg; }

protected:
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
  geometry_msgs::msg::Pose current_pose_;
  geometry_msgs::msg::Twist current_vel_;
  std::recursive_mutex mut_;
  double control_duration_;

  double robot_width_ = 0.30;
  double robot_length_ = 0.30;
  double rival_radius_ = 0.30;

  Rival::SharedPtr rivals_;
  rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;
};

}  // namespace toid