Line data    Source code

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