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->state_pub = this->nh.advertise<HLR::State>("robot_state", 100);
      16           0 :     this->error_pub = this->nh.advertise<HLR::Error>("robot_error", 100);
      17             : 
      18           0 :     this->service_move_start = this->nh.advertiseService(
      19           0 :         "move_start", &ObjectMoveHandler::object_move_start, this);
      20           0 :     this->service_move_cancel = this->nh.advertiseService(
      21           0 :         "move_cancel", &ObjectMoveHandler::object_move_cancel, this);
      22           0 :     this->service_move_continue = this->nh.advertiseService(
      23           0 :         "move_continue", &ObjectMoveHandler::object_move_continue, this);
      24           0 :     this->service_move_stop = this->nh.advertiseService(
      25           0 :         "move_stop", &ObjectMoveHandler::object_move_stop, this);
      26             : 
      27           0 :     this->controller.set_error_callback([this](const auto& e) {
      28           0 :         HLR::Error msg;
      29           0 :         msg.value = this->convert_error_to_ros_msg(e);
      30           0 :         ROS_ERROR_STREAM("Controller error occured: " << msg.value);
      31           0 :         this->error_pub.publish(msg);
      32           0 :     });
      33             : 
      34           0 :     this->controller.set_state_change_callback([this](const auto& s) {
      35           0 :         HLR::State msg;
      36           0 :         msg.value = this->convert_controller_state_to_ros_msg(s);
      37           0 :         this->state_pub.publish(msg);
      38           0 :     });
      39           0 : }
      40             : 
      41           0 : bool ObjectMoveHandler::object_move_start(HLR::ObjectMoveStart::Request& req,
      42             :                                           HLR::ObjectMoveStart::Response& res)
      43             : {
      44           0 :     res.state.value =
      45           0 :         this->convert_controller_state_to_ros_msg(this->controller.get_state());
      46             :     try
      47             :     {
      48           0 :         this->controller.pick_up_cup(req.cup_id);
      49             :     }
      50           0 :     catch (const std::exception& e)
      51             :     {
      52           0 :         ROS_ERROR_STREAM("Couldn't start to move cup: " << e.what());
      53           0 :         return false;
      54             :     }
      55           0 :     return true;
      56             : }
      57             : 
      58           0 : bool ObjectMoveHandler::object_move_cancel(
      59             :     HLR::ObjectMoveAction::Request& req[[maybe_unused]],
      60             :     HLR::ObjectMoveAction::Response& res)
      61             : {
      62           0 :     res.state.value =
      63           0 :         this->convert_controller_state_to_ros_msg(this->controller.get_state());
      64             : 
      65             :     try
      66             :     {
      67           0 :         this->controller.move_cancel();
      68             :     }
      69           0 :     catch (const std::exception& e)
      70             :     {
      71           0 :         ROS_ERROR_STREAM("Couldn't cancel movement: " << e.what());
      72           0 :         return false;
      73             :     }
      74             : 
      75           0 :     return true;
      76             : }
      77             : 
      78           0 : bool ObjectMoveHandler::object_move_continue(
      79             :     HLR::ObjectMoveAction::Request& req[[maybe_unused]],
      80             :     HLR::ObjectMoveAction::Response& res)
      81             : {
      82           0 :     res.state.value =
      83           0 :         this->convert_controller_state_to_ros_msg(this->controller.get_state());
      84             : 
      85             :     try
      86             :     {
      87           0 :         this->controller.move_continue();
      88             :     }
      89           0 :     catch (const std::exception& e)
      90             :     {
      91           0 :         ROS_ERROR_STREAM("Couldn't continue movement: " << e.what());
      92           0 :         return false;
      93             :     }
      94             : 
      95           0 :     return true;
      96             : }
      97             : 
      98           0 : bool ObjectMoveHandler::object_move_stop(
      99             :     HLR::ObjectMoveAction::Request& req[[maybe_unused]],
     100             :     HLR::ObjectMoveAction::Response& res)
     101             : {
     102           0 :     res.state.value =
     103           0 :         this->convert_controller_state_to_ros_msg(this->controller.get_state());
     104             : 
     105             :     try
     106             :     {
     107           0 :         this->controller.move_stop();
     108             :     }
     109           0 :     catch (const std::exception& e)
     110             :     {
     111           0 :         ROS_ERROR_STREAM("Couldn't stop arm: " << e.what());
     112           0 :         return false;
     113             :     }
     114             : 
     115           0 :     return true;
     116             : }
     117             : 
     118           0 : std::uint8_t ObjectMoveHandler::convert_controller_state_to_ros_msg(
     119             :     RobotControllerState state) const noexcept
     120             : {
     121           0 :     switch (state)
     122             :     {
     123           0 :         case RobotControllerState::idle:
     124           0 :             return HLR::State::IDLE;
     125           0 :         case RobotControllerState::moving_to_ready:
     126           0 :             return HLR::State::MOVING_TO_READY;
     127           0 :         case RobotControllerState::calculating_path:
     128           0 :             return HLR::State::CALCULATING_PATH;
     129           0 :         case RobotControllerState::giving_cup:
     130           0 :             return HLR::State::GIVING_CUP;
     131           0 :         case RobotControllerState::cancelling:
     132           0 :             return HLR::State::CANCELLING;
     133           0 :         case RobotControllerState::stopped:
     134           0 :             return HLR::State::STOPPED;
     135           0 :         default:
     136           0 :             std::terminate();
     137             :     }
     138             : }
     139             : 
     140           0 : std::uint8_t ObjectMoveHandler::convert_error_to_ros_msg(ArmError err) const
     141             :     noexcept
     142             : {
     143           0 :     switch (err)
     144             :     {
     145           0 :         case ArmError::safety_concern:
     146           0 :             return HLR::Error::SAFETY_CONCERN;
     147           0 :         case ArmError::no_path_found:
     148           0 :             return HLR::Error::NO_PATH_FOUND;
     149           0 :         case ArmError::driver_error:
     150           0 :             return HLR::Error::DRIVER_ERROR;
     151           0 :         default:
     152           0 :             std::terminate();
     153             :     }
     154             : }
     155             : 
     156             : } // namespace Kinematics
     157           3 : } // namespace HLR