Line data    Source code

       1             : #include "RobotDriverHandler.hpp"
       2             : 
       3             : #include "HLR/RV2AJDriverClear.h"
       4             : #include "HLR/RV2AJDriverQueueAdd.h"
       5             : 
       6             : namespace HLR
       7             : {
       8             : namespace Kinematics
       9             : {
      10           0 : RobotDriverHandler::RobotDriverHandler()
      11             : {
      12           0 :     this->id_subscriber = this->nh.subscribe(
      13           0 :         "rv2aj_driver_id", 100, &RobotDriverHandler::sub_move_complete, this);
      14             : 
      15           0 :     this->driver_stop =
      16           0 :         this->nh.serviceClient<HLR::RV2AJDriverClear>("rv2aj_driver_clear");
      17             : 
      18           0 :     this->driver_add = this->nh.serviceClient<HLR::RV2AJDriverQueueAdd>(
      19           0 :         "rv2aj_driver_queue_add");
      20           0 : }
      21             : 
      22           0 : Matrix<double, 5, 1> RobotDriverHandler::get_arm_state() const noexcept
      23             : {
      24           0 :     return this->current_arm_state;
      25             : }
      26             : 
      27           0 : void RobotDriverHandler::move(const std::vector<Matrix<double, 5, 1>>& path,
      28             :                               double speed)
      29             : {
      30           0 :     if (!this->moves_being_handled.empty())
      31             :     {
      32             :         throw std::logic_error("The robot arm is currently moving. Can't move "
      33           0 :                                "allready moving arm.");
      34             :     }
      35             : 
      36           0 :     HLR::RV2AJDriverQueueAdd msg;
      37           0 :     for (const auto& n : path)
      38             :     {
      39           0 :         msg.request.base = n[0][0];
      40           0 :         msg.request.shoulder = n[1][0];
      41           0 :         msg.request.elbow = n[2][0];
      42           0 :         msg.request.wrist = n[3][0];
      43           0 :         msg.request.wrist_rotate = n[4][0];
      44           0 :         msg.request.speed = speed;
      45             : 
      46           0 :         if (!this->driver_add.call(msg))
      47             :         {
      48             :             const std::string err(
      49           0 :                 "Unable to call robot driver to move the arm.");
      50           0 :             ROS_ERROR_STREAM(err);
      51           0 :             throw std::runtime_error(err);
      52             :         }
      53           0 :         this->moves_being_handled.insert(std::pair(msg.response.id, n));
      54           0 :         this->id_to_wait = msg.response.id;
      55             :     }
      56           0 : }
      57             : 
      58           0 : void RobotDriverHandler::stop()
      59             : {
      60           0 :     HLR::RV2AJDriverClear msg;
      61           0 :     if (!this->driver_stop.call(msg))
      62             :     {
      63           0 :         const std::string err("Unable to call robot driver to stop the arm.");
      64           0 :         ROS_ERROR_STREAM(err);
      65           0 :         throw std::runtime_error(err);
      66             :     }
      67           0 :     this->moves_being_handled.clear();
      68           0 : }
      69             : 
      70           0 : void RobotDriverHandler::set_move_complete_handler(
      71             :     const std::function<void()>& handler)
      72             : {
      73           0 :     this->move_complete_handler = handler;
      74           0 : }
      75             : 
      76           0 : void RobotDriverHandler::sub_move_complete(const std_msgs::UInt64::ConstPtr& id)
      77             : {
      78           0 :     if (this->moves_being_handled.empty())
      79             :     {
      80           0 :         return;
      81             :     }
      82             : 
      83           0 :     const auto it = this->moves_being_handled.find(id->data);
      84           0 :     if (it != this->moves_being_handled.end())
      85             :     {
      86           0 :         this->current_arm_state = it->second;
      87           0 :         this->moves_being_handled.erase(it);
      88             :     }
      89             : 
      90           0 :     if (id->data == this->id_to_wait && this->move_complete_handler)
      91             :     {
      92           0 :         ROS_ERROR_STREAM("Calling move complete handler");
      93           0 :         this->moves_being_handled.clear();
      94           0 :         this->move_complete_handler();
      95             :     }
      96             : }
      97             : 
      98             : } // namespace Kinematics
      99           3 : } // namespace HLR