Line data    Source code

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