ros_control: make motor_start_time_ an std::optional (#515)

Author: Flova

bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.hpp

@@ -33,7 +33,7 @@ class WolfgangHardwareInterface {
   std::vector<std::vector<bitbots_ros_control::HardwareInterface *>> interfaces_;
   DynamixelServoHardwareInterface servo_interface_;
   rclcpp::Publisher<bitbots_msgs::msg::Audio>::SharedPtr speak_pub_;
-  rclcpp::Time motor_start_time_;
+  std::optional<rclcpp::Time> motor_start_time_;
   bool motor_first_write_{false};
 
   // prevent unnecessary error when power is turned on

bitbots_lowlevel/bitbots_ros_control/src/wolfgang_hardware_interface.cpp

@@ -304,7 +304,7 @@ void WolfgangHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Durat
     motor_start_time_ = t + rclcpp::Duration::from_seconds(nh_->get_parameter("servos.start_delay").as_double());
     motor_first_write_ = true;
   }
-  if (t > motor_start_time_) {
+  if (motor_start_time_ && t > motor_start_time_) {
     if (motor_first_write_) {
       servo_interface_.writeROMRAM(false);
       motor_first_write_ = false;