.. _program_listing_file_include_as2_behavior___impl_behavior_server__impl.hpp:

Program Listing for File behavior_server__impl.hpp
==================================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_as2_behavior___impl_behavior_server__impl.hpp>` (``include/as2_behavior/__impl/behavior_server__impl.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #ifndef __AS2_BEHAVIOR_SERVER__IMPL_HPP__
   #define __AS2_BEHAVIOR_SERVER__IMPL_HPP__
   #include "as2_behavior/__detail/behavior_server__class.hpp"
   
   namespace as2_behavior {
   
   template <typename actionT>
   BehaviorServer<actionT>::BehaviorServer(const std::string& name, const rclcpp::NodeOptions& options)
       : as2::Node(name, options), action_name_(name) {
     register_action();
     register_service_servers();
     register_publishers();
     register_timers();
   }
   
   template <typename actionT>
   void BehaviorServer<actionT>::register_action() {
     // create a group for the action server
     auto action_server_group =
         this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   
     this->action_server_ = rclcpp_action::create_server<actionT>(
         this, this->generate_global_name(action_name_),
         std::bind(&BehaviorServer::handleGoal, this, std::placeholders::_1, std::placeholders::_2),
         std::bind(&BehaviorServer::handleCancel, this, std::placeholders::_1),
         std::bind(&BehaviorServer::handleAccepted, this, std::placeholders::_1));
   
     // this->action_server_->
   };
   
   template <typename actionT>
   rclcpp_action::GoalResponse BehaviorServer<actionT>::handleGoal(
       const rclcpp_action::GoalUUID& uuid,
       std::shared_ptr<const typename actionT::Goal> goal) {
     RCLCPP_DEBUG(this->get_logger(), "Received goal request with UUID: %s", (char*)uuid.data());
     if (this->activate(goal)) {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     } else {
       return rclcpp_action::GoalResponse::REJECT;
     }
   }
   
   template <typename actionT>
   rclcpp_action::CancelResponse BehaviorServer<actionT>::handleCancel(
       const std::shared_ptr<GoalHandleAction> goal_handle) {
     RCLCPP_INFO(this->get_logger(), "Request to cancel goal received");
     std_srvs::srv::Trigger::Request::SharedPtr req =
         std::make_shared<std_srvs::srv::Trigger::Request>();
     std_srvs::srv::Trigger::Response::SharedPtr res =
         std::make_shared<std_srvs::srv::Trigger::Response>();
     deactivate(req, res);
     if (res->success) {
       return rclcpp_action::CancelResponse::ACCEPT;
     } else {
       return rclcpp_action::CancelResponse::REJECT;
     }
   };
   
   template <typename actionT>
   void BehaviorServer<actionT>::handleAccepted(const std::shared_ptr<GoalHandleAction> goal_handle) {
     goal_handle_ = goal_handle;
   };
   
   template <typename actionT>
   std::string BehaviorServer<actionT>::generate_name(const std::string& name) {
     return std::string(this->get_name()) + "/_behavior/" + name;
   }
   
   template <typename actionT>
   void BehaviorServer<actionT>::register_service_servers() {
     pause_srv_ = this->create_service<std_srvs::srv::Trigger>(
         generate_name("pause"),
         std::bind(&BehaviorServer::pause, this, std::placeholders::_1, std::placeholders::_2));
     resume_srv_ = this->create_service<std_srvs::srv::Trigger>(
         generate_name("resume"),
         std::bind(&BehaviorServer::resume, this, std::placeholders::_1, std::placeholders::_2));
     stop_srv_ = this->create_service<std_srvs::srv::Trigger>(
         generate_name("stop"),
         std::bind(&BehaviorServer::deactivate, this, std::placeholders::_1, std::placeholders::_2));
   }
   
   template <typename actionT>
   void BehaviorServer<actionT>::register_publishers() {
     // feedback_pub_    = this->create_publisher<feedback_msg>(generate_name("feedback"), 10);
     // goal_status_pub_ = this->create_publisher<goal_status_msg>(generate_name("goal_status"), 10);
     behavior_status_pub_ =
         this->create_publisher<BehaviorStatus>(generate_name("behavior_status"), 10);
   }
   
   template <typename actionT>
   void BehaviorServer<actionT>::register_timers() {
     behavior_status_timer_ = this->create_timer(
         std::chrono::milliseconds(100), std::bind(&BehaviorServer::publish_behavior_status, this));
   }
   
   template <typename actionT>
   void BehaviorServer<actionT>::register_run_timer() {
     run_timer_ = this->create_timer(std::chrono::milliseconds(100),
                                     std::bind(&BehaviorServer::timer_callback, this));
   }
   template <typename actionT>
   void BehaviorServer<actionT>::cleanup_run_timer(const ExecutionStatus& state) {
     on_execution_end(state);
     goal_handle_.reset();
     run_timer_.reset();
   }
   
   template <typename actionT>
   bool BehaviorServer<actionT>::on_activate(std::shared_ptr<const typename actionT::Goal> goal) {
     return true;
   };
   
   template <typename actionT>
   bool BehaviorServer<actionT>::on_modify(std::shared_ptr<const typename actionT::Goal> goal) {
     return true;
   }
   
   template <typename actionT>
   bool BehaviorServer<actionT>::on_deactivate(const std::shared_ptr<std::string>& message) {
     return true;
   }
   template <typename actionT>
   bool BehaviorServer<actionT>::on_pause(const std::shared_ptr<std::string>& message) {
     return true;
   }
   template <typename actionT>
   bool BehaviorServer<actionT>::on_resume(const std::shared_ptr<std::string>& message) {
     return true;
   }
   
   /* template <typename actionT>
   ExecutionStatus BehaviorServer<actionT>::on_run(typename feedback_msg::SharedPtr& fb) {
     return ExecutionStatus::SUCCESS;
   } */
   
   template <typename actionT>
   ExecutionStatus BehaviorServer<actionT>::on_run(
       const typename std::shared_ptr<const typename actionT::Goal>& goal,
       typename std::shared_ptr<typename actionT::Feedback>& feedback_msg,
       typename std::shared_ptr<typename actionT::Result>& result_msg) {
     return ExecutionStatus::SUCCESS;
   }  // namespace as2_behavior
   
   template <typename actionT>
   bool BehaviorServer<actionT>::activate(std::shared_ptr<const typename actionT::Goal> goal) {
     RCLCPP_INFO(this->get_logger(), "START");
     if (on_activate(goal)) {
       register_run_timer();
       behavior_status_.status = BehaviorStatus::RUNNING;
       return true;
     }
     return false;
   };
   template <typename actionT>
   void BehaviorServer<actionT>::deactivate(
       const typename std_srvs::srv::Trigger::Request::SharedPtr goal,
       typename std_srvs::srv::Trigger::Response::SharedPtr result) {
     RCLCPP_INFO(this->get_logger(), "STOP");
     auto msg        = std::make_shared<std::string>();
     result->success = on_deactivate(msg);
     result->message = *msg;
     if (result->success) {
       cleanup_run_timer(ExecutionStatus::ABORTED);
       behavior_status_.status = BehaviorStatus::IDLE;
     }
   };
   template <typename actionT>
   
   void BehaviorServer<actionT>::modify(std::shared_ptr<const typename actionT::Goal> goal) {
     RCLCPP_INFO(this->get_logger(), "MODIFY");
     on_modify(goal);
   };
   template <typename actionT>
   void BehaviorServer<actionT>::pause(const typename std_srvs::srv::Trigger::Request::SharedPtr goal,
                                       typename std_srvs::srv::Trigger::Response::SharedPtr result) {
     RCLCPP_INFO(this->get_logger(), "PAUSE");
     if (behavior_status_.status != BehaviorStatus::RUNNING) {
       result->success = false;
       result->message = "Behavior is not running";
       return;
     }
     auto msg        = std::make_shared<std::string>();
     result->success = on_pause(msg);
     result->message = *msg;
     if (result->success) {
       behavior_status_.status = BehaviorStatus::PAUSED;
     }
   };
   template <typename actionT>
   void BehaviorServer<actionT>::resume(const typename std_srvs::srv::Trigger::Request::SharedPtr goal,
                                        typename std_srvs::srv::Trigger::Response::SharedPtr result) {
     RCLCPP_INFO(this->get_logger(), "RESUME");
     if (behavior_status_.status != BehaviorStatus::PAUSED) {
       result->success = false;
       result->message = "Behavior is not paused";
       return;
     }
     auto msg        = std::make_shared<std::string>();
     result->success = on_resume(msg);
     result->message = *msg;
     if (result->success) {
       behavior_status_.status = BehaviorStatus::RUNNING;
     }
   };
   
   template <typename actionT>
   void BehaviorServer<actionT>::run(
       const typename std::shared_ptr<GoalHandleAction>& goal_handle_action) {
     if (behavior_status_.status != BehaviorStatus::RUNNING) {
       return;
     };
     auto goal              = goal_handle_action->get_goal();
     auto feedback          = std::make_shared<typename actionT::Feedback>();
     auto result            = std::make_shared<typename actionT::Result>();
     ExecutionStatus status = on_run(goal, feedback, result);
   
     switch (status) {
       case ExecutionStatus::SUCCESS: {
         RCLCPP_INFO(this->get_logger(), "SUCCESS");
         behavior_status_.status = BehaviorStatus::IDLE;
         goal_handle_->succeed(result);
       } break;
       case ExecutionStatus::RUNNING: {
         auto clk = this->get_clock();
         RCLCPP_INFO_THROTTLE(this->get_logger(), *clk, 5000, "RUNNING");
         goal_handle_action->publish_feedback(feedback);
         behavior_status_.status = BehaviorStatus::RUNNING;
       } break;
       case ExecutionStatus::FAILURE: {
         RCLCPP_INFO(this->get_logger(), "FAILURE");
         behavior_status_.status = BehaviorStatus::IDLE;
         goal_handle_->abort(result);
       } break;
       case ExecutionStatus::ABORTED: {
         RCLCPP_INFO(this->get_logger(), "ABORTED");
         behavior_status_.status = BehaviorStatus::IDLE;
         goal_handle_->abort(result);
       } break;
     }
   
     if (behavior_status_.status != BehaviorStatus::RUNNING) {
       cleanup_run_timer(status);
     }
   }
   
   template <typename actionT>
   void BehaviorServer<actionT>::publish_behavior_status() {
     BehaviorStatus msg;
     msg.status = behavior_status_.status;
     behavior_status_pub_->publish(msg);
   }
   };  // namespace as2_behavior
   
   #endif  // AS2_BEHAVIOR__BEHAVIOR_SERVER_HPP_