Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Communicate double / double array parameters with type info, explicitly cast when set from integer #256

Merged
merged 2 commits into from Aug 21, 2023
Merged

Communicate double / double array parameters with type info, explicitly cast when set from integer #256

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
32471cc
Support handling of `float64` and `float64_array` parameter types
achim-k Aug 8, 2023
9c3b50b
Update foxglove_bridge_base/src/serialization.cpp
achim-k Aug 21, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
27 changes: 25 additions & 2 deletions foxglove_bridge_base/src/serialization.cpp
View file
Open in desktop
Expand Up @@ -77,6 +77,13 @@ void to_json(nlohmann::json& j, const Parameter& p) {
j["name"] = p.getName();
if (p.getType() == ParameterType::PARAMETER_BYTE_ARRAY) {
j["type"] = "byte_array";
} else if (p.getType() == ParameterType::PARAMETER_DOUBLE) {
j["type"] = "float64";
} else if (p.getType() == ParameterType::PARAMETER_ARRAY) {
const auto& vec = p.getValue().getValue<std::vector<ParameterValue>>();
if (!vec.empty() && vec.front().getType() == ParameterType::PARAMETER_DOUBLE) {
j["type"] = "float64_array";
}
}
}

Expand All @@ -90,10 +97,26 @@ void from_json(const nlohmann::json& j, Parameter& p) {

ParameterValue pValue;
from_json(j["value"], pValue);
const auto typeIt = j.find("type");
const std::string type = typeIt != j.end() ? typeIt->get<std::string>() : "";

if (j.find("type") != j.end() && j["type"] == "byte_array" &&
pValue.getType() == ParameterType::PARAMETER_STRING) {
if (pValue.getType() == ParameterType::PARAMETER_STRING && type == "byte_array") {
p = Parameter(name, base64Decode(pValue.getValue<std::string>()));
} else if (pValue.getType() == ParameterType::PARAMETER_INTEGER && type == "float64") {
// Explicitly cast integer value to double.
p = Parameter(name, static_cast<double>(pValue.getValue<int64_t>()));
} else if (pValue.getType() == ParameterType::PARAMETER_ARRAY && type == "float64_array") {
// Explicitly cast elements to double, if possible.
auto values = pValue.getValue<std::vector<ParameterValue>>();
for (ParameterValue& value : values) {
if (value.getType() == ParameterType::PARAMETER_INTEGER) {
value = ParameterValue(static_cast<double>(value.getValue<int64_t>()));
} else if (value.getType() != ParameterType::PARAMETER_DOUBLE) {
throw std::runtime_error("Parameter '" + name +
"' (float64_array) contains non-numeric elements.");
}
}
p = Parameter(name, values);
} else {
p = Parameter(name, pValue);
}
Expand Down
51 changes: 51 additions & 0 deletions ros2_foxglove_bridge/tests/smoke_test.cpp
View file
Open in desktop
Expand Up @@ -34,6 +34,14 @@ class ParameterTest : public ::testing::Test {
inline static const std::string PARAM_2_NAME = "int_array_param";
inline static const PARAM_2_TYPE PARAM_2_DEFAULT_VALUE = {1, 2, 3};

using PARAM_3_TYPE = double;
inline static const std::string PARAM_3_NAME = "float_param";
inline static const PARAM_3_TYPE PARAM_3_DEFAULT_VALUE = 1.123;

using PARAM_4_TYPE = std::vector<double>;
inline static const std::string PARAM_4_NAME = "float_array_param";
inline static const PARAM_4_TYPE PARAM_4_DEFAULT_VALUE = {1.1, 2.2, 3.3};

protected:
void SetUp() override {
auto nodeOptions = rclcpp::NodeOptions();
Expand All @@ -52,6 +60,8 @@ class ParameterTest : public ::testing::Test {
p2Param.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
p2Param.read_only = false;
_paramNode2->declare_parameter(p2Param.name, PARAM_2_DEFAULT_VALUE, p2Param);
_paramNode2->declare_parameter(PARAM_3_NAME, PARAM_3_DEFAULT_VALUE);
_paramNode2->declare_parameter(PARAM_4_NAME, PARAM_4_DEFAULT_VALUE);

_executor.add_node(_paramNode1);
_executor.add_node(_paramNode2);
Expand Down Expand Up @@ -419,6 +429,47 @@ TEST_F(ParameterTest, testSetParametersWithReqId) {
EXPECT_EQ(1UL, params.size());
}

TEST_F(ParameterTest, testSetFloatParametersWithIntegers) {
const auto floatParamName = NODE_2_NAME + "." + PARAM_3_NAME;
const auto floatArrayParamName = NODE_2_NAME + "." + PARAM_4_NAME;
const int64_t floatParamVal = 10;
const std::vector<int64_t> floatArrayParamVal = {3, 2, 1};
const std::string requestId = "req-testSetFloatParametersWithIntegers";
auto future = foxglove::waitForParameters(_wsClient, requestId);
const nlohmann::json::array_t parameters = {
{{"name", floatParamName}, {"value", floatParamVal}, {"type", "float64"}},
{{"name", floatArrayParamName}, {"value", floatArrayParamVal}, {"type", "float64_array"}},
};
_wsClient->sendText(
nlohmann::json{{"op", "setParameters"}, {"id", requestId}, {"parameters", parameters}}.dump());
ASSERT_EQ(std::future_status::ready, future.wait_for(ONE_SECOND));
std::vector<foxglove::Parameter> params = future.get();

{
const auto param =
std::find_if(params.begin(), params.end(), [floatParamName](const foxglove::Parameter& p) {
return p.getName() == floatParamName;
});
ASSERT_NE(param, params.end());
EXPECT_EQ(param->getType(), foxglove::ParameterType::PARAMETER_DOUBLE);
EXPECT_NEAR(param->getValue().getValue<double>(), static_cast<double>(floatParamVal), 1e-9);
}
{
const auto param = std::find_if(params.begin(), params.end(),
[floatArrayParamName](const foxglove::Parameter& p) {
return p.getName() == floatArrayParamName;
});
ASSERT_NE(param, params.end());
EXPECT_EQ(param->getType(), foxglove::ParameterType::PARAMETER_ARRAY);
const auto paramValue = param->getValue().getValue<std::vector<foxglove::ParameterValue>>();
ASSERT_EQ(paramValue.size(), floatArrayParamVal.size());
for (size_t i = 0; i < paramValue.size(); ++i) {
EXPECT_NEAR(paramValue[i].getValue<double>(), static_cast<double>(floatArrayParamVal[i]),
1e-9);
}
}
}

TEST_F(ParameterTest, testUnsetParameter) {
const auto p1 = NODE_1_NAME + "." + DELETABLE_PARAM_NAME;
const std::vector<foxglove::Parameter> parameters = {
Expand Down