Add a pure turning state before driving off.

include/robotem_rovne/Node.h

@@ -81,11 +81,12 @@ class Node : public rclcpp::Node
   void init_ctrl_loop();
   void ctrl_loop();
 
-  enum class State { Stopped, Starting, Driving, Stopping };
+  enum class State { Stopped, Orienting, Starting, Driving, Stopping };
   State _robot_state;
   quantity<m/s> _motor_base_vel;
   void control_yaw();
   void handle_Stopped();
+  void handle_Orienting();
   void handle_Starting();
   void handle_Driving();
   void handle_Stopping();

src/Node.cpp

@@ -196,6 +196,7 @@ void Node::ctrl_loop()
   switch (_robot_state)
   {
     case State::Stopped:  handle_Stopped(); break;
+    case State::Orienting: handle_Orienting(); break;
     case State::Starting: handle_Starting(); break;
     case State::Driving:  handle_Driving(); break;
     case State::Stopping: handle_Stopping(); break;
@@ -243,6 +244,19 @@ void Node::handle_Stopped()
   pub_motor_right(0. * m/s);
 }
 
+void Node::handle_Orienting()
+{
+  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Orienting");
+
+  _motor_base_vel = 0. * m/s;
+
+  control_yaw();
+
+  auto const yaw_err = (_yaw_target - _yaw_actual);
+  if ( yaw_err < (-5. * deg).in(rad) && yaw_err < (5. * deg).in(rad))
+    _robot_state = State::Starting;
+}
+
 void Node::handle_Starting()
 {
   RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Starting");