following_server.hpp

// Copyright (c) 2024 Open Navigation LLC
// Copyright (c) 2024 Alberto J. Tudela Roldán
//
// 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 OPENNAV_FOLLOWING__FOLLOWING_SERVER_HPP_
#define OPENNAV_FOLLOWING__FOLLOWING_SERVER_HPP_
 
#include <vector>
#include <memory>
#include <string>
#include <mutex>
#include <functional>
 
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "nav2_msgs/action/follow_object.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "nav2_util/twist_publisher.hpp"
#include "nav2_util/odometry_utils.hpp"
#include "opennav_docking/controller.hpp"
#include "opennav_docking/pose_filter.hpp"
#include "opennav_following/parameter_handler.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
 
namespace opennav_following
{
class FollowingServer : public nav2_util::LifecycleNode
{
public:
  using FollowObject = nav2_msgs::action::FollowObject;
  using FollowingActionServer = nav2_util::SimpleActionServer<FollowObject>;
 
  explicit FollowingServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
  ~FollowingServer() = default;
 
  void publishFollowingFeedback(uint16_t state);
 
  virtual bool approachObject(
    geometry_msgs::msg::PoseStamped & object_pose,
    const std::string & target_frame = std::string(""));
 
  virtual bool rotateToObject(
    geometry_msgs::msg::PoseStamped & object_pose,
    const std::string & target_frame = std::string(""));
 
  template<typename ActionT>
  void getPreemptedGoalIfRequested(
    typename std::shared_ptr<const typename ActionT::Goal> goal,
    const std::unique_ptr<nav2_util::SimpleActionServer<ActionT>> & action_server);
 
  template<typename ActionT>
  bool checkAndWarnIfCancelled(
    std::unique_ptr<nav2_util::SimpleActionServer<ActionT>> & action_server,
    const std::string & name);
 
  template<typename ActionT>
  bool checkAndWarnIfPreempted(
    std::unique_ptr<nav2_util::SimpleActionServer<ActionT>> & action_server,
    const std::string & name);
 
  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
 
  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
 
  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
 
  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
 
  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
  void publishZeroVelocity();
 
protected:
  void followObject();
 
  virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose);
 
  virtual bool getFramePose(geometry_msgs::msg::PoseStamped & pose, const std::string & frame_id);
 
  virtual bool getTrackingPose(
    geometry_msgs::msg::PoseStamped & pose, const std::string & frame_id);
 
  geometry_msgs::msg::PoseStamped getPoseAtDistance(
    const geometry_msgs::msg::PoseStamped & pose, double distance);
 
  bool isGoalReached(const geometry_msgs::msg::PoseStamped & goal_pose);
 
  // Parameter handler
  std::unique_ptr<opennav_following::ParameterHandler> param_handler_;
  Parameters * params_;
 
  // Time when object became static
  rclcpp::Time static_object_start_time_;
  // Flag to track if we've initialized the static timer
  bool static_timer_initialized_;
  // Maximum number of times the robot will retry to approach the object
  int num_retries_;
 
  // Timestamp of the last time a iteration was started
  rclcpp::Time iteration_start_time_;
 
  // This is a class member so it can be accessed in publish feedback
  rclcpp::Time action_start_time_;
 
  // Subscribe to the dynamic pose
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr dynamic_pose_sub_;
 
  // Publish the filtered dynamic pose
  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseStamped>::SharedPtr
    filtered_dynamic_pose_pub_;
 
  // Latest message
  geometry_msgs::msg::PoseStamped detected_dynamic_pose_;
 
  // Filtering of detected poses
  std::unique_ptr<opennav_docking::PoseFilter> filter_;
 
  std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
  std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
  std::unique_ptr<FollowingActionServer> following_action_server_;
 
  std::unique_ptr<opennav_docking::Controller> controller_;
 
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
  std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
};
 
}  // namespace opennav_following
 
#endif  // OPENNAV_FOLLOWING__FOLLOWING_SERVER_HPP_