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