[unfinished] attempt to improve handling of rejected parameter updates

include/requests.hpp

@@ -34,9 +34,12 @@ struct ParameterValueRequest : Request {
 };
 
 struct ParameterModificationRequest : Request {
-    ParameterModificationRequest(ROSParameter updatedParameter_) : Request(Type::MODIFY_PARAMETER_VALUE), parameter(std::move(updatedParameter_)) {};
+    ParameterModificationRequest(ROSParameter updatedParameter_, ROSParameterVariant previousValue_) : Request(
+            Type::MODIFY_PARAMETER_VALUE), parameter(std::move(updatedParameter_)),
+            previousValue(std::move(previousValue_)) {};
 
     ROSParameter parameter;
+    ROSParameterVariant previousValue;
 };
 
 #endif // RIG_RECONFIGURE_REQUESTS_HPP

include/responses.hpp

@@ -39,11 +39,13 @@ struct ParameterValueResponse : public Response {
 };
 
 struct ParameterModificationResponse : public Response {
-    ParameterModificationResponse(std::string parameterName_, bool success_, std::string reason_) :
-        Response(Type::MODIFICATION_RESULT), parameterName(std::move(parameterName_)), success(success_),
-        reason(std::move(reason_)) {};
+    ParameterModificationResponse(std::string parameterName_, ROSParameterVariant value_, bool success_,
+                                  std::string reason_) :
+        Response(Type::MODIFICATION_RESULT), parameterName(std::move(parameterName_)), value(std::move(value_)),
+        success(success_), reason(std::move(reason_)) {};
 
     std::string parameterName;
+    ROSParameterVariant value;
     bool success;
     std::string reason;
 };

include/service_wrapper.hpp

@@ -31,6 +31,11 @@ struct FutureTimeoutContainer {
     bool timeoutReached = false;
 };
 
+struct OutstandingParameterModification {
+    unsigned int numRequests;
+    ROSParameterVariant previousValue;
+};
+
 /**
  * Utility class wrapping all the ROS related calls.
  */
@@ -81,6 +86,8 @@ class ServiceWrapper {
     rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr getParametersClient;
     rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr setParametersClient;
 
+    std::map<std::string, OutstandingParameterModification> openModificationRequests;
+
     // callbacks for the results of the futures
     void nodeParametersReceived(const rclcpp::Client<rcl_interfaces::srv::ListParameters>::SharedFuture &future,
                                 const std::shared_ptr<FutureTimeoutContainer> &timeoutContainer);
@@ -89,6 +96,7 @@ class ServiceWrapper {
                                  const std::shared_ptr<FutureTimeoutContainer> &timeoutContainer);
     void parameterModificationResponseReceived(const rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture &future,
                                                const std::string &parameterName,
+                                               const ROSParameterVariant &value,
                                                const std::shared_ptr<FutureTimeoutContainer> &timeoutContainer);
 };
 

src/parameter_window.cpp

@@ -156,23 +156,29 @@ std::set<ImGuiID> visualizeParameters(ServiceWrapper &serviceWrapper,
         ImGui::SameLine();
         ImGui::PushItemWidth(static_cast<float>(textfieldWidth));
 
+        // Note: keeping track of the previous value is important for situations in which the modification is rejected
+        //       in order to overwrite the requested value displayed in the GUI state with the actual parameter value
+        // TODO: editing the value happens over multiple iterations, hence, the previous value is overwritten multiple
+        //       times and doesn't hold the initial value we intend to store
+        const ROSParameterVariant previousValue = value;
+
         if (std::holds_alternative<double>(value)) {
             ImGui::DragScalar(identifier.c_str(), ImGuiDataType_Double, &std::get<double>(value), 1.0F, nullptr,
                               nullptr, "%.6g");
             if (ImGui::IsItemDeactivatedAfterEdit()) {
                 serviceWrapper.pushRequest(
-                        std::make_shared<ParameterModificationRequest>(ROSParameter(fullPath, value)));
+                        std::make_shared<ParameterModificationRequest>(ROSParameter(fullPath, value), previousValue));
             }
         } else if (std::holds_alternative<bool>(value)) {
             if (ImGui::Checkbox(identifier.c_str(), &std::get<bool>(value))) {
                 serviceWrapper.pushRequest(
-                        std::make_shared<ParameterModificationRequest>(ROSParameter(fullPath, value)));
+                        std::make_shared<ParameterModificationRequest>(ROSParameter(fullPath, value), previousValue));
             }
         } else if (std::holds_alternative<int>(value)) {
             ImGui::DragInt(identifier.c_str(), &std::get<int>(value));
             if (ImGui::IsItemDeactivatedAfterEdit()) {
                 serviceWrapper.pushRequest(
-                        std::make_shared<ParameterModificationRequest>(ROSParameter(fullPath, value)));
+                        std::make_shared<ParameterModificationRequest>(ROSParameter(fullPath, value), previousValue));
             }
         } else if (std::holds_alternative<std::string>(value)) {
             // Set to true when enter is pressed
@@ -197,7 +203,7 @@ std::set<ImGuiID> visualizeParameters(ServiceWrapper &serviceWrapper,
             if (flush || (!ImGui::IsItemActive() && dirtyTextInput == fullPath)) {
                 dirtyTextInput.clear();
                 serviceWrapper.pushRequest(
-                        std::make_shared<ParameterModificationRequest>(ROSParameter(fullPath, value)));
+                        std::make_shared<ParameterModificationRequest>(ROSParameter(fullPath, value), previousValue));
             }
         }
         ImGui::PopItemWidth();
@@ -218,7 +224,7 @@ std::set<ImGuiID> visualizeParameters(ServiceWrapper &serviceWrapper,
             bool open = ImGui::TreeNode(label.c_str());
 
             ImGui::SameLine();
-            bool textClicked = false;
+            bool textClicked;
             if (subgroup->prefixSearchPatternStart.has_value() && subgroup->prefixSearchPatternEnd.has_value()) {
                 textClicked = highlightedSelectableText(subgroup->prefix, subgroup->prefixSearchPatternStart.value(),
                                                         subgroup->prefixSearchPatternEnd.value(),

src/rig_reconfigure.cpp

@@ -168,11 +168,15 @@ int main(int argc, char *argv[]) {
                 case Response::Type::MODIFICATION_RESULT: {
                     auto result = std::dynamic_pointer_cast<ParameterModificationResponse>(response);
 
-                    if (result->success) {
-                        status.text = "Parameter '" + result->parameterName + "' modified successfully!";
-                    } else {
+                    // in case of rejected updates the GUI value (at this point holding the requested value)
+                    // has be reset to the previous value
+                    if (!result->success) {
+                        parameterTree.set(ROSParameter(result->parameterName, result->value));
                         status.text = "Parameter '" + result->parameterName + "' couldn't be modified!";
+                    } else {
+                        status.text = "Parameter '" + result->parameterName + "' modified successfully!";
                     }
+
                     status.type = Status::Type::PARAMETER_CHANGED;
 
                     break;

src/service_wrapper.cpp

@@ -188,12 +188,22 @@ void ServiceWrapper::handleRequest(const RequestPtr &request) {
         }
 
         case Request::Type::MODIFY_PARAMETER_VALUE: {
-            auto updateRequest = std::dynamic_pointer_cast<ParameterModificationRequest>(request);
+            const auto updateRequest = std::dynamic_pointer_cast<ParameterModificationRequest>(request);
 
-            if (updateRequest->parameter.name.empty()) {
+            const auto parameterName = updateRequest->parameter.name;
+
+            if (parameterName.empty()) {
                 break;
             }
 
+            // keep track of previous value to set the GUI back in case the update is rejected
+            auto openRequest = openModificationRequests.find(parameterName);
+            if (openRequest == openModificationRequests.end()) {
+                openModificationRequests[parameterName] = {.numRequests = 1, .previousValue = updateRequest->previousValue};
+            } else {
+                openRequest->second.numRequests += 1;
+            }
+
             auto update = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
             rcl_interfaces::msg::Parameter parameterMsg;
             if (updateRequest->parameter.name.at(0) == '/') {
@@ -220,14 +230,16 @@ void ServiceWrapper::handleRequest(const RequestPtr &request) {
 
             const auto timeoutPtr = std::make_shared<FutureTimeoutContainer>();
 
-            auto callbackLambda = [&, name=parameterMsg.name, timeoutPtr](rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture future) {
-                parameterModificationResponseReceived(std::forward<decltype(future)>(future), name, timeoutPtr);
+            auto callbackLambda = [&, name=parameterMsg.name, value=updateRequest->parameter.value, timeoutPtr](rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture future) {
+                parameterModificationResponseReceived(std::forward<decltype(future)>(future), name, value, timeoutPtr);
             };
 
             setParametersClient->async_send_request(update, callbackLambda);
 
-            std::lock_guard unfinishedROSRequestsLock(unfinishedROSRequestsMutex);
-            unfinishedROSRequests.push_back(timeoutPtr);
+            {
+                std::lock_guard unfinishedROSRequestsLock(unfinishedROSRequestsMutex);
+                unfinishedROSRequests.push_back(timeoutPtr);
+            }
         }
 
         case Request::Type::TERMINATE:
@@ -288,10 +300,33 @@ void ServiceWrapper::parameterValuesReceived(const rclcpp::Client<rcl_interfaces
 
 void ServiceWrapper::parameterModificationResponseReceived(const rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture &future,
                                                            const std::string &parameterName,
+                                                           const ROSParameterVariant &parameterValue,
                                                            const std::shared_ptr<FutureTimeoutContainer> &timeoutContainer) {
     const auto &resultMsg = future.get();
 
-    auto response = std::make_shared<ParameterModificationResponse>(parameterName, resultMsg->results.at(0).successful, resultMsg->results.at(0).reason);
+    const bool success = resultMsg->results.at(0).successful;
+    const auto reason = resultMsg->results.at(0).reason;
+
+    // handling of previous parameter values
+    auto openRequest = openModificationRequests.find(parameterName);
+    assert (openRequest != openModificationRequests.end() && "No open modification request found!");
+
+    ROSParameterVariant value;
+    if (success) {
+        openRequest->second.previousValue = parameterValue;
+        value = parameterValue;
+    } else {
+        value = openRequest->second.previousValue;
+    }
+
+    if (openRequest->second.numRequests == 1) {
+        openModificationRequests.erase(parameterName);
+    } else {
+        openRequest->second.numRequests -= 1;
+    }
+
+    auto response = std::make_shared<ParameterModificationResponse>(parameterName, value,
+                                                                    success, reason);
 
     timeoutContainer->handled = true;