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->driver_stop =
      13           0 :         this->nh.serviceClient<HLR::RV2AJDriverClear>("rv2aj_driver_clear");
      14           0 :     this->driver_add = this->nh.serviceClient<HLR::RV2AJDriverQueueAdd>(
      15           0 :         "rv2aj_driver_queue_add");
      16           0 : }
      17             : 
      18           0 : Matrix<double, 5, 1> RosRobotDriverHandler::get_arm_state() const noexcept
      19             : {
      20           0 :     return this->current_arm_state;
      21             : }
      22             : 
      23           0 : void RosRobotDriverHandler::move(
      24             :     const std::vector<Matrix<double, 5, 1>>& path[[maybe_unused]],
      25             :     double speed[[maybe_unused]])
      26             : {
      27           0 :     HLR::RV2AJDriverQueueAdd msg;
      28           0 :     for (const auto& n : path)
      29             :     {
      30           0 :         msg.request.base = n[0][0];
      31           0 :         msg.request.shoulder = n[1][0];
      32           0 :         msg.request.elbow = n[2][0];
      33           0 :         msg.request.wrist = n[3][0];
      34           0 :         msg.request.wrist_rotate = n[4][0];
      35           0 :         msg.request.speed = speed;
      36             : 
      37           0 :         if (!this->driver_add.call(msg))
      38             :         {
      39             :             const std::string err(
      40           0 :                 "Unable to call robot driver to move the arm.");
      41           0 :             ROS_ERROR_STREAM(err);
      42           0 :             throw std::runtime_error(err);
      43             :         }
      44             :     }
      45           0 : }
      46             : 
      47           0 : void RosRobotDriverHandler::stop()
      48             : {
      49           0 :     HLR::RV2AJDriverClear msg;
      50           0 :     if (!this->driver_stop.call(msg))
      51             :     {
      52           0 :         const std::string err("Unable to call robot driver to stop the arm.");
      53           0 :         ROS_ERROR_STREAM(err);
      54           0 :         throw std::runtime_error(err);
      55             :     }
      56           0 : }
      57             : 
      58             : } // namespace Kinematics
      59           3 : } // namespace HLR