Line data    Source code

       1             : #include "ObjectMoveHandler.hpp"
       2             : 
       3             : #include <chrono>
       4             : #include <cstdint>
       5             : 
       6             : namespace HLR
       7             : {
       8             : namespace Kinematics
       9             : {
      10           0 : ObjectMoveHandler::ObjectMoveHandler(RobotController& controller)
      11           0 :     : controller(controller)
      12             : {
      13             :     // bind topics and services
      14           0 :     this->progress_pub =
      15           0 :         this->nh.advertise<HLR::Progress>("move_progress", 1000);
      16           0 :     this->service_move_start = this->nh.advertiseService(
      17           0 :         "move_start", &ObjectMoveHandler::object_move_start, this);
      18           0 :     this->service_move_cancel = this->nh.advertiseService(
      19           0 :         "move_cancel", &ObjectMoveHandler::object_move_cancel, this);
      20           0 :     this->service_move_continue = this->nh.advertiseService(
      21           0 :         "move_continue", &ObjectMoveHandler::object_move_continue, this);
      22           0 :     this->service_move_stop = this->nh.advertiseService(
      23           0 :         "move_stop", &ObjectMoveHandler::object_move_stop, this);
      24           0 : }
      25             : 
      26           0 : bool ObjectMoveHandler::object_move_start(HLR::ObjectMoveStart::Request& req,
      27             :                                           HLR::ObjectMoveStart::Response& res)
      28             : {
      29           0 :     res.state.value =
      30           0 :         this->convert_controller_state_to_ros_msg(this->controller.get_state());
      31             :     try
      32             :     {
      33           0 :         this->controller.pick_up_cup(req.cup_id);
      34             :     }
      35           0 :     catch (const std::exception& e)
      36             :     {
      37           0 :         ROS_ERROR_STREAM("Couldn't start to move cup: " << e.what());
      38           0 :         return false;
      39             :     }
      40           0 :     return true;
      41             : }
      42             : 
      43           0 : bool ObjectMoveHandler::object_move_cancel(
      44             :     HLR::ObjectMoveAction::Request& req[[maybe_unused]],
      45             :     HLR::ObjectMoveAction::Response& res)
      46             : {
      47           0 :     res.state.value =
      48           0 :         this->convert_controller_state_to_ros_msg(this->controller.get_state());
      49           0 :     return true;
      50             : }
      51             : 
      52           0 : bool ObjectMoveHandler::object_move_continue(
      53             :     HLR::ObjectMoveAction::Request& req[[maybe_unused]],
      54             :     HLR::ObjectMoveAction::Response& res)
      55             : {
      56           0 :     res.state.value =
      57           0 :         this->convert_controller_state_to_ros_msg(this->controller.get_state());
      58           0 :     return true;
      59             : }
      60             : 
      61           0 : bool ObjectMoveHandler::object_move_stop(
      62             :     HLR::ObjectMoveAction::Request& req[[maybe_unused]],
      63             :     HLR::ObjectMoveAction::Response& res)
      64             : {
      65           0 :     res.state.value =
      66           0 :         this->convert_controller_state_to_ros_msg(this->controller.get_state());
      67           0 :     return true;
      68             : }
      69             : 
      70           0 : std::uint8_t ObjectMoveHandler::convert_controller_state_to_ros_msg(
      71             :     RobotControllerState state) const noexcept
      72             : {
      73           0 :     switch (state)
      74             :     {
      75           0 :         case RobotControllerState::init:
      76           0 :             return HLR::State::INIT;
      77           0 :         case RobotControllerState::idle:
      78           0 :             return HLR::State::IDLE;
      79           0 :         case RobotControllerState::moving_to_ready:
      80           0 :             return HLR::State::MOVING_TO_READY;
      81           0 :         case RobotControllerState::moving_to_cup:
      82           0 :             return HLR::State::MOVING_TO_CUP;
      83           0 :         case RobotControllerState::stopped:
      84           0 :             return HLR::State::STOPPED;
      85             :     }
      86           0 : }
      87             : 
      88             : } // namespace Kinematics
      89           3 : } // namespace HLR