GoalManagerClient.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// 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 EASYNAV_SYSTEM__GOALMANAGERCLIENT_HPP_
#define EASYNAV_SYSTEM__GOALMANAGERCLIENT_HPP_
 
#include <string>
 
#include "rclcpp/rclcpp.hpp"
#include "easynav_interfaces/msg/navigation_control.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/goals.hpp"
 
namespace easynav
{

class GoalManagerClient
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS(GoalManagerClient)
 
  
  enum class State
  {
    IDLE,
    SENT_GOAL,
    SENT_PREEMPT,
    ACCEPTED_AND_NAVIGATING,
    NAVIGATION_FINISHED,
    NAVIGATION_REJECTED,
    NAVIGATION_FAILED,
    NAVIGATION_CANCELLED,
    ERROR
  };

  explicit GoalManagerClient(rclcpp::Node::SharedPtr node);

  void send_goal(const geometry_msgs::msg::PoseStamped & goal);

  void send_goals(const nav_msgs::msg::Goals & goals);

  void cancel();

  void pause();

  void resume();

  [[nodiscard]] bool is_paused() const {return paused_;}

  void reset();

  [[nodiscard]] State get_state() const {return state_;}

  [[nodiscard]] bool is_connected() const
  {
    return control_pub_->get_subscription_count() > 1;
  }

  [[nodiscard]] const easynav_interfaces::msg::NavigationControl & get_last_control() const;

  [[nodiscard]] const easynav_interfaces::msg::NavigationControl & get_feedback() const;

  [[nodiscard]] const easynav_interfaces::msg::NavigationControl & get_result() const;
 
private:
  rclcpp::Node::SharedPtr node_;

  rclcpp::Publisher<easynav_interfaces::msg::NavigationControl>::SharedPtr control_pub_;

  rclcpp::Subscription<easynav_interfaces::msg::NavigationControl>::SharedPtr control_sub_;

  easynav_interfaces::msg::NavigationControl::UniquePtr last_control_;

  easynav_interfaces::msg::NavigationControl last_feedback_;

  easynav_interfaces::msg::NavigationControl last_result_;

  std::string id_;

  State state_;

  bool paused_ {false};

  void control_callback(easynav_interfaces::msg::NavigationControl::UniquePtr msg);
};
 
}  // namespace easynav
 
#endif  // EASYNAV_SYSTEM__GOALMANAGERCLIENT_HPP_