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 : std::vector<Matrix<double, 5, 1>> RobotDriverHandler::get_path() const noexcept
      28             : {
      29           0 :     return this->path;
      30             : }
      31             : 
      32           0 : std::uint64_t RobotDriverHandler::get_offset_current_move() const
      33             : {
      34           0 :     for (std::size_t i = 0; i < this->path.size(); ++i)
      35             :     {
      36           0 :         if (this->last_id_complete == this->path_ids[i])
      37             :         {
      38           0 :             return i;
      39             :         }
      40             :     }
      41             :     const std::string err(
      42           0 :         "Couldn't find the index of the last move that was completed.");
      43           0 :     ROS_ERROR_STREAM(err);
      44           0 :     throw std::runtime_error(err);
      45             : }
      46             : 
      47           0 : void RobotDriverHandler::move(const std::vector<Matrix<double, 5, 1>>& path,
      48             :                               double speed)
      49             : {
      50           0 :     if (!this->path.empty())
      51             :     {
      52             :         throw std::logic_error("The robot arm is currently moving. Can't move "
      53           0 :                                "allready moving arm.");
      54             :     }
      55             : 
      56           0 :     this->path = path;
      57             : 
      58           0 :     HLR::RV2AJDriverQueueAdd msg;
      59           0 :     msg.request.cmds.reserve(path.size());
      60           0 :     std::transform(path.begin(),
      61             :                    path.end(),
      62             :                    std::back_inserter(msg.request.cmds),
      63           0 :                    [speed](const auto& n) {
      64           0 :                        HLR::RV2AJDriverCommand cmd;
      65           0 :                        cmd.base = n[0][0];
      66           0 :                        cmd.shoulder = n[1][0];
      67           0 :                        cmd.elbow = n[2][0];
      68           0 :                        cmd.wrist = n[3][0];
      69           0 :                        cmd.wrist_rotate = n[4][0];
      70           0 :                        cmd.speed = speed;
      71           0 :                        return cmd;
      72           0 :                    });
      73             : 
      74           0 :     if (!this->driver_add.call(msg))
      75             :     {
      76           0 :         const std::string err("Unable to call robot driver to move the arm.");
      77           0 :         ROS_ERROR_STREAM(err);
      78           0 :         throw std::runtime_error(err);
      79             :     }
      80             : 
      81           0 :     if (msg.response.ids.size() != path.size())
      82             :     {
      83           0 :         this->stop();
      84             :         throw std::logic_error("Call to move robotarm didn't give the same "
      85             :                                "amount of ids as the amount of positions in "
      86           0 :                                "the path.");
      87             :     }
      88             : 
      89           0 :     this->path_ids = msg.response.ids;
      90           0 : }
      91             : 
      92           0 : void RobotDriverHandler::stop()
      93             : {
      94           0 :     HLR::RV2AJDriverClear msg;
      95           0 :     if (!this->driver_stop.call(msg))
      96             :     {
      97           0 :         const std::string err("Unable to call robot driver to stop the arm.");
      98           0 :         ROS_ERROR_STREAM(err);
      99           0 :         throw std::runtime_error(err);
     100             :     }
     101           0 :     this->path.clear();
     102           0 :     this->path_ids.clear();
     103           0 : }
     104             : 
     105           0 : void RobotDriverHandler::set_move_complete_handler(
     106             :     const std::function<void()>& handler)
     107             : {
     108           0 :     this->move_complete_handler = handler;
     109           0 : }
     110             : 
     111           0 : void RobotDriverHandler::sub_move_complete(const std_msgs::UInt64::ConstPtr& id)
     112             : {
     113           0 :     for (std::size_t i = 0; i < this->path.size(); ++i)
     114             :     {
     115           0 :         if (id->data == this->path_ids[i])
     116             :         {
     117           0 :             this->current_arm_state = this->path[i];
     118             :         }
     119             :     }
     120             : 
     121           0 :     if (id->data == this->path_ids.back() && this->move_complete_handler)
     122             :     {
     123           0 :         ROS_ERROR_STREAM("Calling move complete handler");
     124           0 :         this->path.clear();
     125           0 :         this->path_ids.clear();
     126           0 :         this->move_complete_handler();
     127             :     }
     128           0 : }
     129             : 
     130             : } // namespace Kinematics
     131           3 : } // namespace HLR