[BUG] Dynamic Reconfigure Server setConfigCallback service always returns true

[xaru8145]

Describe the bug
Branch: noetic-devel

The DynamicReconfigure Server class always return success upon SetConfigCallback service call, regardless of whether the new config was successfully derived from the service request.

If, by accident, you call the service with a wrong configuration, the __fromMesage__ function will fail since it won't be able to update the new_config, acording to server.h L219.

The __fromMesage__ function actually returns the result of the process, so I propose using the result to populate the service response correctly so the service caller can actually identify if the call failed.

Current approach

bool setConfigCallback(dynamic_reconfigure::Reconfigure::Request &req,
          dynamic_reconfigure::Reconfigure::Response &rsp)
  {
    boost::recursive_mutex::scoped_lock lock(mutex_);

    ConfigType new_config = config_;
    new_config.__fromMessage__(req.config);
    new_config.__clamp__();
    uint32_t level = config_.__level__(new_config);

    callCallback(new_config, level);

    updateConfigInternal(new_config);
    new_config.__toMessage__(rsp.config);
    return true;
  }

Proposed solution

bool setConfigCallback(dynamic_reconfigure::Reconfigure::Request &req,
          dynamic_reconfigure::Reconfigure::Response &rsp)
  {
    boost::recursive_mutex::scoped_lock lock(mutex_);

    ConfigType new_config = config_;
    bool success = new_config.__fromMessage__(req.config);
    if(success)
    {
     new_config.__clamp__();
     uint32_t level = config_.__level__(new_config);

     callCallback(new_config, level);

     updateConfigInternal(new_config);
     new_config.__toMessage__(rsp.config);
    }
    return success;
  }

I am happy to submit a PR with the proposed solution.