ur_configuration_controller: use try_set on RTBox (#1470)

* ur_configuration_controller: use try_set on RTBox

Using set could potentially block the RT thread.

* Remove temp variable

Author: urfeex

ur_controllers/include/ur_controllers/ur_configuration_controller.hpp

@@ -88,6 +88,7 @@ class URConfigurationController : public controller_interface::ControllerInterfa
   realtime_tools::RealtimeBox<std::shared_ptr<VersionInformation>> robot_software_version_{
     std::make_shared<VersionInformation>()
   };
+  std::atomic<bool> robot_software_version_set_{ false };
 
   rclcpp::Service<ur_msgs::srv::GetRobotSoftwareVersion>::SharedPtr get_robot_software_version_srv_;
 

ur_controllers/src/ur_configuration_controller.cpp

@@ -88,18 +88,21 @@ controller_interface::InterfaceConfiguration URConfigurationController::state_in
 controller_interface::return_type URConfigurationController::update(const rclcpp::Time& /* time */,
                                                                     const rclcpp::Duration& /* period */)
 {
+  if (!robot_software_version_set_) {
+    robot_software_version_set_ =
+        robot_software_version_.try_set([this](const std::shared_ptr<VersionInformation>& ptr) {
+          ptr->major = state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_optional().value_or(0.0);
+          ptr->minor = state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_optional().value_or(0.0);
+          ptr->build = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_optional().value_or(0.0);
+          ptr->bugfix = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_optional().value_or(0.0);
+        });
+  }
   return controller_interface::return_type::OK;
 }
 
 controller_interface::CallbackReturn
 URConfigurationController::on_activate(const rclcpp_lifecycle::State& /* previous_state */)
 {
-  robot_software_version_.set([this](const std::shared_ptr<VersionInformation> ptr) {
-    ptr->major = state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_optional().value_or(0.0);
-    ptr->minor = state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_optional().value_or(0.0);
-    ptr->build = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_optional().value_or(0.0);
-    ptr->bugfix = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_optional().value_or(0.0);
-  });
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -113,7 +116,10 @@ bool URConfigurationController::getRobotSoftwareVersion(
     ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr /*req*/,
     ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp)
 {
-  std::shared_ptr<VersionInformation> temp;
+  if (!robot_software_version_set_) {
+    RCLCPP_WARN(get_node()->get_logger(), "Robot software version not set yet.");
+    return false;
+  }
   return robot_software_version_.try_get([resp](const std::shared_ptr<VersionInformation> ptr) {
     resp->major = ptr->major;
     resp->minor = ptr->minor;