HLR  0.0.1
RobotController.hpp
1 #ifndef KINEMATICS_ROBOTCONTROLLER_HPP
2 #define KINEMATICS_ROBOTCONTROLLER_HPP
3 
4 #include <ros/ros.h>
5 #include "Planners/Planner.hpp"
6 #include <CGAL/Simple_cartesian.h>
7 #include <CGAL/Surface_mesh.h>
8 #include <Kinematics/ConcurrentQueue.hpp>
9 #include <Kinematics/Matrix.hpp>
10 
11 #include <mutex>
12 #include <stdexcept>
13 #include <thread>
14 #include <unordered_set>
15 
16 #include "RobotDriverHandler.hpp"
17 
18 namespace HLR
19 {
20 namespace Kinematics
21 {
26 {
30  std::uint32_t id;
31 
36 
41 
45  bool is_cup;
46 
50  bool has_speed;
51 
55  CGAL::Surface_mesh<CGAL::Simple_cartesian<double>::Point_3> polyhedron;
56 };
57 
62 {
66  idle,
67 
72 
77 
81  giving_cup,
82 
86  cancelling,
87 
91  stopped
92 };
93 
98 enum class ArmError
99 {
104 
110 
116 };
117 
125 
132 std::string arm_error_to_string(ArmError error);
133 
140 {
141 public:
146  enum class Event
147  {
151  move_to_ready,
152 
156  pick_up_cup,
157 
161  stop,
162 
166  resume,
167 
171  cancel,
172 
176  terminate
177  };
178 
185 
189  virtual ~RobotController();
190 
195  RobotController(const RobotController& rc) = delete;
196 
201  RobotController(RobotController&& rc) = delete;
202 
208  RobotController& operator=(const RobotController& rc) = delete;
209 
215  RobotController& operator=(RobotController&& rc) = delete;
216 
222  RobotControllerState get_state() const;
223 
234  void pick_up_cup(std::uint32_t id);
235 
241  void move_stop();
242 
248  void move_continue();
249 
257  void move_cancel();
258 
265  void remove_object(const EnvironmentObject& obj);
266 
272  void add_object(EnvironmentObject&& obj);
273 
283  void set_error_callback(const std::function<void(ArmError)>& cb);
284 
290  void set_state_change_callback(
291  const std::function<void(RobotControllerState)>& cb);
292 
293 private:
299  void set_state(RobotControllerState state);
300 
305  void position_reached();
306 
311  void event_handler();
312 
316  void move_to_ready_handler();
317 
321  void pick_up_cup_handler();
322 
326  void stop_handler();
327 
331  void resume_handler();
332 
336  void cancel_handler();
337 
341  const Matrix<double, 3, 1> target_position = {{-0.2}, {0.0175}, {0.53}};
342 
346  const Matrix<double, 3, 1> ready_pos = {{0.357}, {0.216506}, {0}};
347 
351  const double speed = 100;
352 
357  const double distance_from_cup = 0.12;
358 
362  const double lift_distance = 0.10;
363 
367  const double angle_step_size = 360;
368 
372  const double euclidean_step_size = 0.05;
373 
377  std::function<void(ArmError)> error_callback;
378 
382  std::function<void(RobotControllerState)> state_callback;
383 
389 
394 
398  mutable std::mutex state_m;
399 
403  Matrix<double, 3, 1> cup_position;
404 
408  std::unordered_map<std::uint32_t, EnvironmentObject> objects;
409 
413  mutable std::recursive_mutex objects_m;
414 
419  std::thread worker;
420 
424  ConcurrentQueue<Event> events;
425 
429  RobotDriverHandler& driver;
430 };
431 
432 } // namespace Kinematics
433 } // namespace HLR
434 
435 #endif
Matrix< double, 3, 1 > velocity
The velocity vector of the object.
Definition: RobotController.hpp:40
std::string arm_error_to_string(ArmError error)
Convert ArmError enum to string representation.
Robot arm is moving to ready position,.
CGAL::Surface_mesh< CGAL::Simple_cartesian< double >::Point_3 > polyhedron
The CGAL polyhedron data. This can be used for collision checking.
Definition: RobotController.hpp:55
RobotControllerState
The current state of the robot controller.
Definition: RobotController.hpp:61
The arm has stopped due to a safety concern.
std::uint32_t id
The id of the object.
Definition: RobotController.hpp:30
Describes an object that is detected in the environment.
Definition: RobotController.hpp:25
This class contains the main logic of the robotic arm. It handles the calculation of robotic path and...
Definition: RobotController.hpp:139
The arm couldn&#39;t be moved because there is an underlying driver error.
Robot arm is currently stopped.
bool has_speed
Whether this object has a valid speed.
Definition: RobotController.hpp:50
The arm has stopped because no valid path was found to execute the given action.
Concrete handle on the robot using the ROS interface.
Definition: RobotDriverHandler.hpp:19
Event
Internal event class. These events are send to the worker thread and handled accordingly.
Definition: RobotController.hpp:146
Robot arm is moving gripper to surround the cup.
bool is_cup
Whether this object is a cup.
Definition: RobotController.hpp:45
std::string robot_controller_state_to_string(RobotControllerState state)
Convert RobotControllerState enum to string representation.
The robot arm is currently idle.
ArmError
Enum containing possible reasons for the robot arm not being able to succesfully execute the specifie...
Definition: RobotController.hpp:98
Matrix< double, 3, 1 > position
The position vector of the object.
Definition: RobotController.hpp:35