LCOV - code coverage report
Current view: top level - src/Kinematics - RobotController.cpp (source / functions) Hit Total Coverage
Test: HLR Lines: 1 246 0.4 %
Date: 2018-01-24 10:01:51 Functions: 2 23 8.7 %

          Line data    Source code
       1             : #include "RobotController.hpp"
       2             : 
       3             : #include "HLR/ObjectMoveStart.h"
       4             : #include "Planners/InterpolatingAnglePlanner.hpp"
       5             : #include "Planners/InterpolatingEuclideanPlanner.hpp"
       6             : 
       7             : namespace HLR
       8             : {
       9             : namespace Kinematics
      10             : {
      11           0 : RobotController::RobotController(RobotDriverHandler& driver) : driver(driver)
      12             : {
      13           0 :     this->driver.set_move_complete_handler(
      14           0 :         [this]() { this->position_reached(); });
      15             : 
      16           0 :     this->events.push(Event::move_to_ready);
      17           0 :     this->worker = std::thread(&RobotController::event_handler, this);
      18           0 : }
      19             : 
      20           0 : RobotController::~RobotController()
      21             : {
      22           0 :     this->events.push(Event::terminate);
      23           0 :     this->worker.join();
      24           0 : }
      25             : 
      26           0 : RobotControllerState RobotController::get_state() const
      27             : {
      28           0 :     std::lock_guard<std::mutex> l(this->state_m);
      29           0 :     return this->state;
      30             : }
      31             : 
      32           0 : void RobotController::pick_up_cup(std::uint32_t id)
      33             : {
      34           0 :     std::lock_guard<std::mutex> l(this->state_m);
      35           0 :     if (this->state != RobotControllerState::idle)
      36             :     {
      37           0 :         throw std::logic_error("Robot is not currently idle.");
      38             :     }
      39             : 
      40             :     {
      41           0 :         std::lock_guard<std::recursive_mutex> l(this->objects_m);
      42           0 :         const auto it = this->objects.find(id);
      43           0 :         if (it == this->objects.end())
      44             :         {
      45             :             throw std::invalid_argument(
      46           0 :                 "No object found with the specified id.");
      47             :         }
      48             : 
      49           0 :         if (!it->second.is_cup)
      50             :         {
      51             :             throw std::invalid_argument(
      52           0 :                 "Specifed object exists but is not a cup.");
      53             :         }
      54           0 :         this->cup_position = it->second.position;
      55             :     }
      56           0 :     this->set_state(RobotControllerState::calculating_path);
      57           0 :     this->events.push(Event::pick_up_cup);
      58           0 : }
      59             : 
      60           0 : void RobotController::move_stop()
      61             : {
      62           0 :     std::lock_guard<std::mutex> l(this->state_m);
      63           0 :     switch (this->state)
      64             :     {
      65           0 :         case RobotControllerState::moving_to_ready:
      66             :         case RobotControllerState::calculating_path:
      67             :         case RobotControllerState::giving_cup:
      68           0 :             this->set_state(RobotControllerState::stopped);
      69           0 :             this->events.push(Event::stop);
      70           0 :             break;
      71           0 :         default:
      72             :             throw std::logic_error(
      73           0 :                 "Robot is not currently moving or movement is cancelling.");
      74             :     }
      75           0 : }
      76             : 
      77           0 : void RobotController::move_continue()
      78             : {
      79           0 :     std::lock_guard<std::mutex> l(this->state_m);
      80           0 :     switch (this->state)
      81             :     {
      82           0 :         case RobotControllerState::stopped:
      83           0 :             this->set_state(this->prev_state);
      84           0 :             this->events.push(Event::resume);
      85           0 :             break;
      86           0 :         default:
      87           0 :             throw std::logic_error("Robot is not currently stopped.");
      88             :     }
      89           0 : }
      90             : 
      91           0 : void RobotController::move_cancel()
      92             : {
      93           0 :     std::lock_guard<std::mutex> l(this->state_m);
      94             :     // Can't cancel a move to ready.
      95           0 :     if ((this->state == RobotControllerState::stopped
      96           0 :          && this->prev_state == RobotControllerState::moving_to_ready)
      97           0 :         || this->state == RobotControllerState::moving_to_ready)
      98             :     {
      99           0 :         throw std::logic_error("Can't cancel a move to ready.");
     100             :     }
     101             : 
     102           0 :     switch (this->state)
     103             :     {
     104           0 :         case RobotControllerState::stopped:
     105             :         case RobotControllerState::giving_cup:
     106             :         case RobotControllerState::calculating_path:
     107           0 :             this->set_state(RobotControllerState::cancelling);
     108           0 :             this->events.push(Event::cancel);
     109           0 :             break;
     110           0 :         default:
     111             :             throw std::logic_error(
     112           0 :                 "Robot is not currently stopped or moving to a position.");
     113             :     }
     114           0 : }
     115             : 
     116           0 : void RobotController::remove_object(const EnvironmentObject& obj)
     117             : {
     118           0 :     std::lock_guard<std::recursive_mutex> l(this->objects_m);
     119           0 :     const auto it = this->objects.find(obj.id);
     120           0 :     if (it != this->objects.end())
     121             :     {
     122           0 :         this->objects.erase(it);
     123             :     }
     124           0 : }
     125             : 
     126           0 : void RobotController::add_object(EnvironmentObject&& obj)
     127             : {
     128           0 :     std::lock_guard<std::recursive_mutex> l(this->objects_m);
     129           0 :     this->remove_object(obj);
     130           0 :     const auto id = obj.id;
     131           0 :     this->objects.insert(std::make_pair(id, std::move(obj)));
     132           0 : }
     133             : 
     134           0 : void RobotController::set_state(RobotControllerState state)
     135             : {
     136           0 :     this->prev_state = this->state;
     137           0 :     this->state = state;
     138           0 :     if (this->state_callback)
     139             :     {
     140           0 :         this->state_callback(state);
     141             :     }
     142           0 : }
     143             : 
     144           0 : void RobotController::set_error_callback(
     145             :     const std::function<void(ArmError)>& cb)
     146             : {
     147           0 :     this->error_callback = cb;
     148           0 : }
     149             : 
     150           0 : void RobotController::set_state_change_callback(
     151             :     const std::function<void(RobotControllerState)>& cb)
     152             : {
     153           0 :     this->state_callback = cb;
     154           0 : }
     155             : 
     156           0 : void RobotController::position_reached()
     157             : {
     158           0 :     std::lock_guard<std::mutex> l(this->state_m);
     159           0 :     switch (this->state)
     160             :     {
     161           0 :         case RobotControllerState::moving_to_ready:
     162           0 :             this->set_state(RobotControllerState::idle);
     163           0 :             break;
     164           0 :         case RobotControllerState::cancelling:
     165             :         case RobotControllerState::giving_cup:
     166           0 :             this->set_state(RobotControllerState::moving_to_ready);
     167           0 :             this->events.push(Event::move_to_ready);
     168           0 :             break;
     169           0 :         default:
     170           0 :             return;
     171             :     }
     172             : }
     173             : 
     174           0 : void RobotController::event_handler()
     175             : {
     176           0 :     bool term = false;
     177           0 :     while (!term)
     178             :     {
     179           0 :         auto ev = this->events.pop();
     180           0 :         switch (ev)
     181             :         {
     182           0 :             case Event::move_to_ready:
     183           0 :                 this->move_to_ready_handler();
     184           0 :                 break;
     185           0 :             case Event::pick_up_cup:
     186           0 :                 this->pick_up_cup_handler();
     187           0 :                 break;
     188           0 :             case Event::stop:
     189           0 :                 this->stop_handler();
     190           0 :                 break;
     191           0 :             case Event::resume:
     192           0 :                 this->resume_handler();
     193           0 :                 break;
     194           0 :             case Event::cancel:
     195           0 :                 this->cancel_handler();
     196           0 :                 break;
     197           0 :             case Event::terminate:
     198           0 :                 term = true;
     199           0 :                 break;
     200             :         }
     201             :     }
     202           0 : }
     203             : 
     204           0 : void RobotController::move_to_ready_handler()
     205             : {
     206           0 :     const Planners::InterpolatingAnglePlanner planner(this->angle_step_size);
     207             :     auto path_opt =
     208           0 :         planner.get_path(this->driver.get_arm_state(), this->ready_pos);
     209           0 :     if (!path_opt.has_value())
     210             :     {
     211           0 :         if (this->error_callback)
     212             :         {
     213           0 :             this->error_callback(ArmError::no_path_found);
     214             :         }
     215           0 :         this->set_state(RobotControllerState::idle);
     216           0 :         return;
     217             :     }
     218             : 
     219           0 :     this->path = path_opt.value();
     220             : 
     221             :     try
     222             :     {
     223           0 :         this->driver.move(this->path, this->speed);
     224             :     }
     225           0 :     catch (const std::runtime_error& e)
     226             :     {
     227           0 :         if (this->error_callback)
     228             :         {
     229           0 :             this->error_callback(ArmError::driver_error);
     230             :         }
     231             :     }
     232             : }
     233             : 
     234           0 : void RobotController::pick_up_cup_handler()
     235             : {
     236             :     // The different planners that are used.
     237             :     const Planners::InterpolatingAnglePlanner angle_planner(
     238           0 :         this->angle_step_size);
     239             :     const Planners::InterpolatingEuclideanPlanner euclidean_planner(
     240           0 :         this->euclidean_step_size);
     241             : 
     242             :     /**
     243             :      * The intermediate positions. The following positons are computed:
     244             :      *      1. Just prior to pickup up the cup.
     245             :      *      2. Move gripper around cup.
     246             :      *      3. Move gripper upwards to lock in cup.
     247             :      *      4. Move gripper with cup above the target position.
     248             :      *      5. Move gripper down so cup will stand on the target position.
     249             :      *      6. Move gripper back a little bit so the gripper no longer surrounds
     250             :      * the cup.
     251             :      */
     252           0 :     Matrix<double, 3, 1> ignore_y(this->cup_position);
     253           0 :     ignore_y[1][0] = 0;
     254           0 :     auto magnitude = ignore_y.get_magnitude();
     255           0 :     auto scalefactor = (magnitude - this->distance_from_cup) / magnitude;
     256             : 
     257           0 :     Matrix<double, 3, 1> pre_cup_position(this->cup_position);
     258           0 :     pre_cup_position[0][0] *= scalefactor;
     259           0 :     pre_cup_position[2][0] *= scalefactor;
     260             : 
     261           0 :     Matrix<double, 3, 1> lift_cup_postion(this->cup_position);
     262           0 :     lift_cup_postion[1][0] += lift_distance;
     263             : 
     264           0 :     Matrix<double, 3, 1> target_lift_position(this->target_position);
     265           0 :     target_lift_position[1][0] += lift_distance;
     266             : 
     267           0 :     ignore_y = this->target_position;
     268           0 :     ignore_y[1][0] = 0;
     269           0 :     magnitude = ignore_y.get_magnitude();
     270           0 :     scalefactor = (magnitude - this->distance_from_cup) / magnitude;
     271           0 :     Matrix<double, 3, 1> post_target_position(this->target_position);
     272           0 :     post_target_position[0][0] *= scalefactor;
     273           0 :     post_target_position[2][0] *= scalefactor;
     274             : 
     275             :     // All path parts.
     276             :     const auto to_pre_cup_opt =
     277           0 :         angle_planner.get_path(this->driver.get_arm_state(), pre_cup_position);
     278           0 :     if (!to_pre_cup_opt.has_value())
     279             :     {
     280           0 :         if (this->error_callback)
     281             :         {
     282           0 :             this->error_callback(ArmError::no_path_found);
     283             :         }
     284           0 :         this->set_state(RobotControllerState::idle);
     285           0 :         return;
     286             :     }
     287             : 
     288             :     const auto to_cup_opt = euclidean_planner.get_path(
     289           0 :         to_pre_cup_opt.value().back(), this->cup_position);
     290           0 :     if (!to_cup_opt.has_value())
     291             :     {
     292           0 :         if (this->error_callback)
     293             :         {
     294           0 :             this->error_callback(ArmError::no_path_found);
     295             :         }
     296           0 :         this->set_state(RobotControllerState::idle);
     297           0 :         return;
     298             :     }
     299             : 
     300             :     const auto lift_cup_opt =
     301           0 :         euclidean_planner.get_path(to_cup_opt.value().back(), lift_cup_postion);
     302           0 :     if (!lift_cup_opt.has_value())
     303             :     {
     304           0 :         if (this->error_callback)
     305             :         {
     306           0 :             this->error_callback(ArmError::no_path_found);
     307             :         }
     308           0 :         this->set_state(RobotControllerState::idle);
     309           0 :         return;
     310             :     }
     311             : 
     312             :     const auto target_lift_opt = angle_planner.get_path(
     313           0 :         lift_cup_opt.value().back(), target_lift_position);
     314           0 :     if (!target_lift_opt.has_value())
     315             :     {
     316           0 :         if (this->error_callback)
     317             :         {
     318           0 :             this->error_callback(ArmError::no_path_found);
     319             :         }
     320           0 :         this->set_state(RobotControllerState::idle);
     321           0 :         return;
     322             :     }
     323             : 
     324             :     const auto target_opt = euclidean_planner.get_path(
     325           0 :         target_lift_opt.value().back(), this->target_position);
     326           0 :     if (!target_opt.has_value())
     327             :     {
     328           0 :         if (this->error_callback)
     329             :         {
     330           0 :             this->error_callback(ArmError::no_path_found);
     331             :         }
     332           0 :         this->set_state(RobotControllerState::idle);
     333           0 :         return;
     334             :     }
     335             : 
     336             :     const auto post_target_opt = euclidean_planner.get_path(
     337           0 :         target_opt.value().back(), post_target_position);
     338           0 :     if (!post_target_opt.has_value())
     339             :     {
     340           0 :         if (this->error_callback)
     341             :         {
     342           0 :             this->error_callback(ArmError::no_path_found);
     343             :         }
     344           0 :         this->set_state(RobotControllerState::idle);
     345           0 :         return;
     346             :     }
     347             : 
     348             :     // Move all parts into single vector.
     349           0 :     this->path_index = 0;
     350           0 :     this->path.clear();
     351           0 :     this->path.reserve(
     352           0 :         to_pre_cup_opt.value().size() + to_cup_opt.value().size()
     353           0 :         + lift_cup_opt.value().size() + target_lift_opt.value().size()
     354           0 :         + target_opt.value().size() + post_target_opt.value().size());
     355           0 :     std::move(to_pre_cup_opt.value().begin(),
     356           0 :               to_pre_cup_opt.value().end(),
     357           0 :               std::back_inserter(this->path));
     358           0 :     std::move(to_cup_opt.value().begin(),
     359           0 :               to_cup_opt.value().end(),
     360           0 :               std::back_inserter(this->path));
     361           0 :     std::move(lift_cup_opt.value().begin(),
     362           0 :               lift_cup_opt.value().end(),
     363           0 :               std::back_inserter(this->path));
     364           0 :     std::move(target_lift_opt.value().begin(),
     365           0 :               target_lift_opt.value().end(),
     366           0 :               std::back_inserter(this->path));
     367           0 :     std::move(target_opt.value().begin(),
     368           0 :               target_opt.value().end(),
     369           0 :               std::back_inserter(this->path));
     370           0 :     std::move(post_target_opt.value().begin(),
     371           0 :               post_target_opt.value().end(),
     372           0 :               std::back_inserter(this->path));
     373             : 
     374             :     try
     375             :     {
     376           0 :         std::lock_guard<std::mutex> l(this->state_m);
     377           0 :         this->set_state(RobotControllerState::giving_cup);
     378           0 :         this->driver.move(this->path, this->speed);
     379             :     }
     380           0 :     catch (const std::runtime_error& e)
     381             :     {
     382           0 :         if (this->error_callback)
     383             :         {
     384           0 :             this->error_callback(ArmError::driver_error);
     385             :         }
     386             :     }
     387             : }
     388             : 
     389           0 : void RobotController::stop_handler()
     390             : {
     391             :     try
     392             :     {
     393           0 :         this->driver.stop();
     394             :     }
     395           0 :     catch (const std::runtime_error& e)
     396             :     {
     397           0 :         if (this->error_callback)
     398             :         {
     399           0 :             this->error_callback(ArmError::driver_error);
     400             :         }
     401             :     }
     402           0 : }
     403             : 
     404           0 : void RobotController::resume_handler()
     405             : {
     406           0 :     std::vector<Matrix<double, 5, 1>> remaining_path(this->path);
     407           0 :     remaining_path.erase(this->path.begin() + this->path_index,
     408           0 :                          remaining_path.end());
     409             :     try
     410             :     {
     411           0 :         this->driver.move(remaining_path, this->speed);
     412             :     }
     413           0 :     catch (const std::runtime_error& e)
     414             :     {
     415           0 :         if (this->error_callback)
     416             :         {
     417           0 :             this->error_callback(ArmError::driver_error);
     418             :         }
     419             :     }
     420           0 : }
     421             : 
     422           0 : void RobotController::cancel_handler()
     423             : {
     424           0 :     std::vector<Matrix<double, 5, 1>> remaining_path(this->path);
     425           0 :     remaining_path.erase(this->path.end() - this->path_index, this->path.end());
     426           0 :     std::reverse(remaining_path.begin(), remaining_path.end());
     427             :     try
     428             :     {
     429           0 :         this->driver.move(remaining_path, this->speed);
     430             :     }
     431           0 :     catch (const std::runtime_error& e)
     432             :     {
     433           0 :         if (this->error_callback)
     434             :         {
     435           0 :             this->error_callback(ArmError::driver_error);
     436             :         }
     437             :     }
     438           0 : }
     439             : 
     440             : } // namespace Kinematics
     441           3 : } // namespace HLR

Generated by: LCOV version 1.12