diff --git a/foxglove_bridge_base/include/foxglove_bridge/parameter.hpp b/foxglove_bridge_base/include/foxglove_bridge/parameter.hpp index 0c07a64..7a33e44 100644 --- a/foxglove_bridge_base/include/foxglove_bridge/parameter.hpp +++ b/foxglove_bridge_base/include/foxglove_bridge/parameter.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include namespace foxglove { @@ -18,47 +19,59 @@ enum class ParameterType { PARAMETER_INTEGER, PARAMETER_DOUBLE, PARAMETER_STRING, - PARAMETER_BYTE_ARRAY, - PARAMETER_BOOL_ARRAY, - PARAMETER_INTEGER_ARRAY, - PARAMETER_DOUBLE_ARRAY, - PARAMETER_STRING_ARRAY, + PARAMETER_ARRAY, + PARAMETER_STRUCT, // ROS 1 only + PARAMETER_BYTE_ARRAY, // ROS 2 only +}; + +class ParameterValue { +public: + ParameterValue(); + ParameterValue(bool value); + ParameterValue(int value); + ParameterValue(int64_t value); + ParameterValue(double value); + ParameterValue(const std::string& value); + ParameterValue(const char* value); + ParameterValue(const std::vector& value); + ParameterValue(const std::vector& value); + ParameterValue(const std::unordered_map& value); + + inline ParameterType getType() const { + return _type; + } + + template + inline const T& getValue() const { + return std::any_cast(_value); + } + +private: + ParameterType _type; + std::any _value; }; class Parameter { public: Parameter(); Parameter(const std::string& name); - Parameter(const std::string& name, bool value); - Parameter(const std::string& name, int value); - Parameter(const std::string& name, int64_t value); - Parameter(const std::string& name, double value); - Parameter(const std::string& name, std::string value); - Parameter(const std::string& name, const char* value); - Parameter(const std::string& name, const std::vector& value); - Parameter(const std::string& name, const std::vector& value); - Parameter(const std::string& name, const std::vector& value); - Parameter(const std::string& name, const std::vector& value); - Parameter(const std::string& name, const std::vector& value); - Parameter(const std::string& name, const std::vector& value); + Parameter(const std::string& name, const ParameterValue& value); inline const std::string& getName() const { return _name; } inline ParameterType getType() const { - return _type; + return _value.getType(); } - template - inline const T& getValue() const { - return std::any_cast(_value); + inline const ParameterValue& getValue() const { + return _value; } private: std::string _name; - ParameterType _type; - std::any _value; + ParameterValue _value; }; } // namespace foxglove diff --git a/foxglove_bridge_base/include/foxglove_bridge/serialization.hpp b/foxglove_bridge_base/include/foxglove_bridge/serialization.hpp index 9a21c21..859c427 100644 --- a/foxglove_bridge_base/include/foxglove_bridge/serialization.hpp +++ b/foxglove_bridge_base/include/foxglove_bridge/serialization.hpp @@ -46,6 +46,8 @@ inline uint32_t ReadUint32LE(const uint8_t* buf) { void to_json(nlohmann::json& j, const Channel& c); void from_json(const nlohmann::json& j, Channel& c); +void to_json(nlohmann::json& j, const ParameterValue& p); +void from_json(const nlohmann::json& j, ParameterValue& p); void to_json(nlohmann::json& j, const Parameter& p); void from_json(const nlohmann::json& j, Parameter& p); void to_json(nlohmann::json& j, const Service& p); diff --git a/foxglove_bridge_base/src/parameter.cpp b/foxglove_bridge_base/src/parameter.cpp index 892d0fa..4006218 100644 --- a/foxglove_bridge_base/src/parameter.cpp +++ b/foxglove_bridge_base/src/parameter.cpp @@ -2,70 +2,42 @@ namespace foxglove { -Parameter::Parameter() - : _name("") - , _type(ParameterType::PARAMETER_NOT_SET) - , _value() {} - -Parameter::Parameter(const std::string& name) - : _name(name) - , _type(ParameterType::PARAMETER_NOT_SET) - , _value() {} - -Parameter::Parameter(const std::string& name, bool value) - : _name(name) - , _type(ParameterType::PARAMETER_BOOL) +ParameterValue::ParameterValue() + : _type(ParameterType::PARAMETER_NOT_SET) {} +ParameterValue::ParameterValue(bool value) + : _type(ParameterType::PARAMETER_BOOL) , _value(value) {} - -Parameter::Parameter(const std::string& name, int value) - : Parameter(name, static_cast(value)) {} - -Parameter::Parameter(const std::string& name, int64_t value) - : _name(name) - , _type(ParameterType::PARAMETER_INTEGER) +ParameterValue::ParameterValue(int value) + : _type(ParameterType::PARAMETER_INTEGER) + , _value(static_cast(value)) {} +ParameterValue::ParameterValue(int64_t value) + : _type(ParameterType::PARAMETER_INTEGER) , _value(value) {} - -Parameter::Parameter(const std::string& name, double value) - : _name(name) - , _type(ParameterType::PARAMETER_DOUBLE) +ParameterValue::ParameterValue(double value) + : _type(ParameterType::PARAMETER_DOUBLE) , _value(value) {} - -Parameter::Parameter(const std::string& name, const char* value) - : Parameter(name, std::string(value)) {} - -Parameter::Parameter(const std::string& name, std::string value) - : _name(name) - , _type(ParameterType::PARAMETER_STRING) +ParameterValue::ParameterValue(const std::string& value) + : _type(ParameterType::PARAMETER_STRING) , _value(value) {} - -Parameter::Parameter(const std::string& name, const std::vector& value) - : _name(name) - , _type(ParameterType::PARAMETER_BYTE_ARRAY) +ParameterValue::ParameterValue(const char* value) + : _type(ParameterType::PARAMETER_STRING) + , _value(std::string(value)) {} +ParameterValue::ParameterValue(const std::vector& value) + : _type(ParameterType::PARAMETER_BYTE_ARRAY) , _value(value) {} - -Parameter::Parameter(const std::string& name, const std::vector& value) - : _name(name) - , _type(ParameterType::PARAMETER_BOOL_ARRAY) +ParameterValue::ParameterValue(const std::vector& value) + : _type(ParameterType::PARAMETER_ARRAY) , _value(value) {} - -Parameter::Parameter(const std::string& name, const std::vector& value) - : _name(name) - , _type(ParameterType::PARAMETER_INTEGER_ARRAY) - , _value(std::vector(value.begin(), value.end())) {} - -Parameter::Parameter(const std::string& name, const std::vector& value) - : _name(name) - , _type(ParameterType::PARAMETER_INTEGER_ARRAY) +ParameterValue::ParameterValue(const std::unordered_map& value) + : _type(ParameterType::PARAMETER_STRUCT) , _value(value) {} -Parameter::Parameter(const std::string& name, const std::vector& value) +Parameter::Parameter() {} +Parameter::Parameter(const std::string& name) : _name(name) - , _type(ParameterType::PARAMETER_DOUBLE_ARRAY) - , _value(value) {} - -Parameter::Parameter(const std::string& name, const std::vector& value) + , _value(ParameterValue()) {} +Parameter::Parameter(const std::string& name, const ParameterValue& value) : _name(name) - , _type(ParameterType::PARAMETER_STRING_ARRAY) , _value(value) {} } // namespace foxglove diff --git a/foxglove_bridge_base/src/serialization.cpp b/foxglove_bridge_base/src/serialization.cpp index a434a7e..e46149f 100644 --- a/foxglove_bridge_base/src/serialization.cpp +++ b/foxglove_bridge_base/src/serialization.cpp @@ -28,35 +28,56 @@ void from_json(const nlohmann::json& j, Channel& c) { c = Channel(j["id"].get(), channelWithoutId); } -void to_json(nlohmann::json& j, const Parameter& p) { +void to_json(nlohmann::json& j, const ParameterValue& p) { const auto paramType = p.getType(); if (paramType == ParameterType::PARAMETER_BOOL) { - j["value"] = p.getValue(); + j = p.getValue(); } else if (paramType == ParameterType::PARAMETER_INTEGER) { - j["value"] = p.getValue(); + j = p.getValue(); } else if (paramType == ParameterType::PARAMETER_DOUBLE) { - j["value"] = p.getValue(); + j = p.getValue(); } else if (paramType == ParameterType::PARAMETER_STRING) { - j["value"] = p.getValue(); + j = p.getValue(); } else if (paramType == ParameterType::PARAMETER_BYTE_ARRAY) { const auto& paramValue = p.getValue>(); const std::string_view strValue(reinterpret_cast(paramValue.data()), paramValue.size()); - j["value"] = base64Encode(strValue); - j["type"] = "byte_array"; - } else if (paramType == ParameterType::PARAMETER_BOOL_ARRAY) { - j["value"] = p.getValue>(); - } else if (paramType == ParameterType::PARAMETER_INTEGER_ARRAY) { - j["value"] = p.getValue>(); - } else if (paramType == ParameterType::PARAMETER_DOUBLE_ARRAY) { - j["value"] = p.getValue>(); - } else if (paramType == ParameterType::PARAMETER_STRING_ARRAY) { - j["value"] = p.getValue>(); + j = base64Encode(strValue); + } else if (paramType == ParameterType::PARAMETER_STRUCT) { + j = p.getValue>(); + } else if (paramType == ParameterType::PARAMETER_ARRAY) { + j = p.getValue>(); } else if (paramType == ParameterType::PARAMETER_NOT_SET) { // empty value. } +} + +void from_json(const nlohmann::json& j, ParameterValue& p) { + const auto jsonType = j.type(); + + if (jsonType == nlohmann::detail::value_t::string) { + p = ParameterValue(j.get()); + } else if (jsonType == nlohmann::detail::value_t::boolean) { + p = ParameterValue(j.get()); + } else if (jsonType == nlohmann::detail::value_t::number_integer) { + p = ParameterValue(j.get()); + } else if (jsonType == nlohmann::detail::value_t::number_unsigned) { + p = ParameterValue(j.get()); + } else if (jsonType == nlohmann::detail::value_t::number_float) { + p = ParameterValue(j.get()); + } else if (jsonType == nlohmann::detail::value_t::object) { + p = ParameterValue(j.get>()); + } else if (jsonType == nlohmann::detail::value_t::array) { + p = ParameterValue(j.get>()); + } +} +void to_json(nlohmann::json& j, const Parameter& p) { + to_json(j["value"], p.getValue()); j["name"] = p.getName(); + if (p.getType() == ParameterType::PARAMETER_BYTE_ARRAY) { + j["type"] = "byte_array"; + } } void from_json(const nlohmann::json& j, Parameter& p) { @@ -67,46 +88,14 @@ void from_json(const nlohmann::json& j, Parameter& p) { return; } - const auto value = j["value"]; - const auto jsonType = j["value"].type(); + ParameterValue pValue; + from_json(j["value"], pValue); - if (jsonType == nlohmann::detail::value_t::string) { - if (j.find("type") == j.end()) { - p = Parameter(name, value.get()); - } else if (j["type"] == "byte_array") { - p = Parameter(name, base64Decode(value.get())); - } else { - throw std::runtime_error("Unsupported parameter 'type' value: " + j.dump()); - } - } else if (jsonType == nlohmann::detail::value_t::boolean) { - p = Parameter(name, value.get()); - } else if (jsonType == nlohmann::detail::value_t::number_integer) { - p = Parameter(name, value.get()); - } else if (jsonType == nlohmann::detail::value_t::number_unsigned) { - p = Parameter(name, value.get()); - } else if (jsonType == nlohmann::detail::value_t::number_float) { - p = Parameter(name, value.get()); - } else if (jsonType == nlohmann::detail::value_t::array) { - if (value.empty()) { - // We do not know the type when an empty array is received. - throw std::runtime_error("Setting empty arrays is currently unsupported."); - } - - if (value.front().is_string()) { - p = Parameter(name, value.get>()); - } else if (value.front().is_boolean()) { - p = Parameter(name, value.get>()); - } else if (value.front().is_number_integer()) { - p = Parameter(name, value.get>()); - } else if (value.front().is_number_unsigned()) { - p = Parameter(name, value.get>()); - } else if (value.front().is_number_float()) { - p = Parameter(name, value.get>()); - } else { - throw std::runtime_error("Unsupported array type"); - } + if (j.find("type") != j.end() && j["type"] == "byte_array" && + pValue.getType() == ParameterType::PARAMETER_STRING) { + p = Parameter(name, base64Decode(pValue.getValue())); } else { - throw std::runtime_error("Unsupported type"); + p = Parameter(name, pValue); } } diff --git a/ros1_foxglove_bridge/include/foxglove_bridge/param_utils.hpp b/ros1_foxglove_bridge/include/foxglove_bridge/param_utils.hpp index 0241cf3..aa78e46 100644 --- a/ros1_foxglove_bridge/include/foxglove_bridge/param_utils.hpp +++ b/ros1_foxglove_bridge/include/foxglove_bridge/param_utils.hpp @@ -11,6 +11,7 @@ namespace foxglove_bridge { foxglove::Parameter fromRosParam(const std::string& name, const XmlRpc::XmlRpcValue& value); +XmlRpc::XmlRpcValue toRosParam(const foxglove::ParameterValue& param); std::vector parseRegexPatterns(const std::vector& strings); diff --git a/ros1_foxglove_bridge/src/param_utils.cpp b/ros1_foxglove_bridge/src/param_utils.cpp index 0004e15..33d87bc 100644 --- a/ros1_foxglove_bridge/src/param_utils.cpp +++ b/ros1_foxglove_bridge/src/param_utils.cpp @@ -1,61 +1,75 @@ +#include #include #include -namespace { - -template -std::vector toVector(const XmlRpc::XmlRpcValue& value) { - std::vector arr; - arr.reserve(static_cast(value.size())); - for (int i = 0; i < value.size(); i++) { - arr.push_back(static_cast(value[i])); - } - return arr; -} - -} // namespace - namespace foxglove_bridge { -foxglove::Parameter fromRosParam(const std::string& name, const XmlRpc::XmlRpcValue& value) { +foxglove::ParameterValue fromRosParam(const XmlRpc::XmlRpcValue& value) { const auto type = value.getType(); if (type == XmlRpc::XmlRpcValue::Type::TypeBoolean) { - return foxglove::Parameter(name, static_cast(value)); + return foxglove::ParameterValue(static_cast(value)); } else if (type == XmlRpc::XmlRpcValue::Type::TypeInt) { - return foxglove::Parameter(name, static_cast(static_cast(value))); + return foxglove::ParameterValue(static_cast(static_cast(value))); } else if (type == XmlRpc::XmlRpcValue::Type::TypeDouble) { - return foxglove::Parameter(name, static_cast(value)); + return foxglove::ParameterValue(static_cast(value)); } else if (type == XmlRpc::XmlRpcValue::Type::TypeString) { - return foxglove::Parameter(name, static_cast(value)); + return foxglove::ParameterValue(static_cast(value)); + } else if (type == XmlRpc::XmlRpcValue::Type::TypeStruct) { + std::unordered_map paramMap; + for (const auto& [elementName, elementVal] : value) { + paramMap.insert({elementName, fromRosParam(elementVal)}); + } + return foxglove::ParameterValue(paramMap); } else if (type == XmlRpc::XmlRpcValue::Type::TypeArray) { - if (value.size() == 0) { - // We can't defer the type in this case so we simply treat it as a int list - return foxglove::Parameter(name, std::vector()); - } else { - const auto firstElement = value[0]; - const auto firstElementType = firstElement.getType(); - if (firstElementType == XmlRpc::XmlRpcValue::Type::TypeBoolean) { - return foxglove::Parameter(name, toVector(value)); - } else if (firstElementType == XmlRpc::XmlRpcValue::Type::TypeInt) { - return foxglove::Parameter(name, toVector(value)); - } else if (firstElementType == XmlRpc::XmlRpcValue::Type::TypeDouble) { - return foxglove::Parameter(name, toVector(value)); - } else if (firstElementType == XmlRpc::XmlRpcValue::Type::TypeString) { - return foxglove::Parameter(name, toVector(value)); - } else { - throw std::runtime_error("Parameter '" + name + "': Unsupported parameter array type"); - } + std::vector paramVec; + for (int i = 0; i < value.size(); ++i) { + paramVec.push_back(fromRosParam(value[i])); } + return foxglove::ParameterValue(paramVec); } else if (type == XmlRpc::XmlRpcValue::Type::TypeInvalid) { - throw std::runtime_error("Parameter '" + name + "': Not set"); + throw std::runtime_error("Parameter not set"); + } else { + throw std::runtime_error("Unsupported parameter type: " + std::to_string(type)); + } +} + +foxglove::Parameter fromRosParam(const std::string& name, const XmlRpc::XmlRpcValue& value) { + return foxglove::Parameter(name, fromRosParam(value)); +} + +XmlRpc::XmlRpcValue toRosParam(const foxglove::ParameterValue& param) { + const auto paramType = param.getType(); + if (paramType == foxglove::ParameterType::PARAMETER_BOOL) { + return param.getValue(); + } else if (paramType == foxglove::ParameterType::PARAMETER_INTEGER) { + return static_cast(param.getValue()); + } else if (paramType == foxglove::ParameterType::PARAMETER_DOUBLE) { + return param.getValue(); + } else if (paramType == foxglove::ParameterType::PARAMETER_STRING) { + return param.getValue(); + } else if (paramType == foxglove::ParameterType::PARAMETER_STRUCT) { + XmlRpc::XmlRpcValue valueStruct; + const auto& paramMap = + param.getValue>(); + for (const auto& [paramName, paramElement] : paramMap) { + valueStruct[paramName] = toRosParam(paramElement); + } + return valueStruct; + } else if (paramType == foxglove::ParameterType::PARAMETER_ARRAY) { + XmlRpc::XmlRpcValue arr; + const auto vec = param.getValue>(); + for (int i = 0; i < static_cast(vec.size()); ++i) { + arr[i] = toRosParam(vec[i]); + } + return arr; } else { - throw std::runtime_error("Parameter '" + name + "': Unsupported parameter type"); + throw std::runtime_error("Unsupported parameter type"); } - return foxglove::Parameter(); + return XmlRpc::XmlRpcValue(); } std::vector parseRegexPatterns(const std::vector& patterns) { diff --git a/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp b/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp index 75263e5..35c16ca 100644 --- a/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp +++ b/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp @@ -685,30 +685,21 @@ class FoxgloveBridge : public nodelet::Nodelet { try { const auto paramType = param.getType(); - if (paramType == ParameterType::PARAMETER_BOOL) { - nh.setParam(paramName, param.getValue()); - } else if (paramType == ParameterType::PARAMETER_INTEGER) { - nh.setParam(paramName, static_cast(param.getValue())); - } else if (paramType == ParameterType::PARAMETER_DOUBLE) { - nh.setParam(paramName, param.getValue()); - } else if (paramType == ParameterType::PARAMETER_STRING) { - nh.setParam(paramName, param.getValue()); - } else if (paramType == ParameterType::PARAMETER_BOOL_ARRAY) { - nh.setParam(paramName, param.getValue>()); - } else if (paramType == ParameterType::PARAMETER_INTEGER_ARRAY) { - const auto int64Vec = param.getValue>(); - std::vector intVec(int64Vec.begin(), int64Vec.end()); - nh.setParam(paramName, intVec); - } else if (paramType == ParameterType::PARAMETER_DOUBLE_ARRAY) { - nh.setParam(paramName, param.getValue>()); - } else if (paramType == ParameterType::PARAMETER_STRING_ARRAY) { - nh.setParam(paramName, param.getValue>()); - } else if (paramType == ParameterType::PARAMETER_NOT_SET) { + const auto paramValue = param.getValue(); + if (paramType == ParameterType::PARAMETER_NOT_SET) { nh.deleteParam(paramName); + } else { + nh.setParam(paramName, toRosParam(paramValue)); } } catch (const std::exception& ex) { ROS_ERROR("Failed to set parameter '%s': %s", paramName.c_str(), ex.what()); success = false; + } catch (const XmlRpc::XmlRpcException& ex) { + ROS_ERROR("Failed to set parameter '%s': %s", paramName.c_str(), ex.getMessage().c_str()); + success = false; + } catch (...) { + ROS_ERROR("Failed to set parameter '%s'", paramName.c_str()); + success = false; } } diff --git a/ros1_foxglove_bridge/tests/smoke_test.cpp b/ros1_foxglove_bridge/tests/smoke_test.cpp index 7f082bb..2d3942d 100644 --- a/ros1_foxglove_bridge/tests/smoke_test.cpp +++ b/ros1_foxglove_bridge/tests/smoke_test.cpp @@ -178,14 +178,20 @@ TEST_F(ParameterTest, testGetParameters) { return param.getName() == PARAM_2_NAME; }); ASSERT_NE(p1Iter, params.end()); - EXPECT_EQ(PARAM_1_DEFAULT_VALUE, p1Iter->getValue()); + EXPECT_EQ(PARAM_1_DEFAULT_VALUE, p1Iter->getValue().getValue()); ASSERT_NE(p2Iter, params.end()); - EXPECT_EQ(PARAM_2_DEFAULT_VALUE, p2Iter->getValue()); + + std::vector double_array_val; + const auto array_params = p2Iter->getValue().getValue>(); + for (const auto& paramValue : array_params) { + double_array_val.push_back(paramValue.getValue()); + } + EXPECT_EQ(double_array_val, PARAM_2_DEFAULT_VALUE); } TEST_F(ParameterTest, testSetParameters) { const PARAM_1_TYPE newP1value = "world"; - const PARAM_2_TYPE newP2value = {4.1, 5.5, 6.6}; + const std::vector newP2value = {4.1, 5.5, 6.6}; const std::vector parameters = { foxglove::Parameter(PARAM_1_NAME, newP1value), @@ -207,9 +213,16 @@ TEST_F(ParameterTest, testSetParameters) { return param.getName() == PARAM_2_NAME; }); ASSERT_NE(p1Iter, params.end()); - EXPECT_EQ(newP1value, p1Iter->getValue()); + EXPECT_EQ(newP1value, p1Iter->getValue().getValue()); ASSERT_NE(p2Iter, params.end()); - EXPECT_EQ(newP2value, p2Iter->getValue()); + + std::vector double_array_val; + const auto array_params = p2Iter->getValue().getValue>(); + for (const auto& paramValue : array_params) { + double_array_val.push_back(paramValue.getValue()); + } + const std::vector expected_value = {4.1, 5.5, 6.6}; + EXPECT_EQ(double_array_val, expected_value); } TEST_F(ParameterTest, testSetParametersWithReqId) { diff --git a/ros2_foxglove_bridge/src/parameter_interface.cpp b/ros2_foxglove_bridge/src/parameter_interface.cpp index 1a510c9..5c23b7e 100644 --- a/ros2_foxglove_bridge/src/parameter_interface.cpp +++ b/ros2_foxglove_bridge/src/parameter_interface.cpp @@ -25,26 +25,52 @@ static rclcpp::Parameter toRosParam(const foxglove::Parameter& p) { using foxglove::ParameterType; const auto paramType = p.getType(); + const auto value = p.getValue(); + if (paramType == ParameterType::PARAMETER_BOOL) { - return rclcpp::Parameter(p.getName(), p.getValue()); + return rclcpp::Parameter(p.getName(), value.getValue()); } else if (paramType == ParameterType::PARAMETER_INTEGER) { - return rclcpp::Parameter(p.getName(), p.getValue()); + return rclcpp::Parameter(p.getName(), value.getValue()); } else if (paramType == ParameterType::PARAMETER_DOUBLE) { - return rclcpp::Parameter(p.getName(), p.getValue()); + return rclcpp::Parameter(p.getName(), value.getValue()); } else if (paramType == ParameterType::PARAMETER_STRING) { - return rclcpp::Parameter(p.getName(), p.getValue()); + return rclcpp::Parameter(p.getName(), value.getValue()); } else if (paramType == ParameterType::PARAMETER_BYTE_ARRAY) { - return rclcpp::Parameter(p.getName(), p.getValue>()); - } else if (paramType == ParameterType::PARAMETER_BOOL_ARRAY) { - return rclcpp::Parameter(p.getName(), p.getValue>()); - } else if (paramType == ParameterType::PARAMETER_INTEGER_ARRAY) { - return rclcpp::Parameter(p.getName(), p.getValue>()); - } else if (paramType == ParameterType::PARAMETER_DOUBLE_ARRAY) { - return rclcpp::Parameter(p.getName(), p.getValue>()); - } else if (paramType == ParameterType::PARAMETER_STRING_ARRAY) { - return rclcpp::Parameter(p.getName(), p.getValue>()); + return rclcpp::Parameter(p.getName(), value.getValue>()); + } else if (paramType == ParameterType::PARAMETER_ARRAY) { + const auto paramVec = value.getValue>(); + + const auto elementType = paramVec.front().getType(); + if (elementType == ParameterType::PARAMETER_BOOL) { + std::vector vec; + for (const auto& paramValue : paramVec) { + vec.push_back(paramValue.getValue()); + } + return rclcpp::Parameter(p.getName(), vec); + } else if (elementType == ParameterType::PARAMETER_INTEGER) { + std::vector vec; + for (const auto& paramValue : paramVec) { + vec.push_back(paramValue.getValue()); + } + return rclcpp::Parameter(p.getName(), vec); + } else if (elementType == ParameterType::PARAMETER_DOUBLE) { + std::vector vec; + for (const auto& paramValue : paramVec) { + vec.push_back(paramValue.getValue()); + } + return rclcpp::Parameter(p.getName(), vec); + } else if (elementType == ParameterType::PARAMETER_STRING) { + std::vector vec; + for (const auto& paramValue : paramVec) { + vec.push_back(paramValue.getValue()); + } + return rclcpp::Parameter(p.getName(), vec); + } + throw std::runtime_error("Unsupported parameter type"); } else if (paramType == ParameterType::PARAMETER_NOT_SET) { return rclcpp::Parameter(p.getName()); + } else { + throw std::runtime_error("Unsupported parameter type"); } return rclcpp::Parameter(); @@ -54,7 +80,7 @@ static foxglove::Parameter fromRosParam(const rclcpp::Parameter& p) { const auto type = p.get_type(); if (type == rclcpp::ParameterType::PARAMETER_NOT_SET) { - return foxglove::Parameter(p.get_name()); + return foxglove::Parameter(p.get_name(), foxglove::ParameterValue()); } else if (type == rclcpp::ParameterType::PARAMETER_BOOL) { return foxglove::Parameter(p.get_name(), p.as_bool()); } else if (type == rclcpp::ParameterType::PARAMETER_INTEGER) { @@ -66,13 +92,29 @@ static foxglove::Parameter fromRosParam(const rclcpp::Parameter& p) { } else if (type == rclcpp::ParameterType::PARAMETER_BYTE_ARRAY) { return foxglove::Parameter(p.get_name(), p.as_byte_array()); } else if (type == rclcpp::ParameterType::PARAMETER_BOOL_ARRAY) { - return foxglove::Parameter(p.get_name(), p.as_bool_array()); + std::vector paramVec; + for (const auto value : p.as_bool_array()) { + paramVec.push_back(value); + } + return foxglove::Parameter(p.get_name(), paramVec); } else if (type == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) { - return foxglove::Parameter(p.get_name(), p.as_integer_array()); + std::vector paramVec; + for (const auto value : p.as_integer_array()) { + paramVec.push_back(value); + } + return foxglove::Parameter(p.get_name(), paramVec); } else if (type == rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY) { - return foxglove::Parameter(p.get_name(), p.as_double_array()); + std::vector paramVec; + for (const auto value : p.as_double_array()) { + paramVec.push_back(value); + } + return foxglove::Parameter(p.get_name(), paramVec); } else if (type == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) { - return foxglove::Parameter(p.get_name(), p.as_string_array()); + std::vector paramVec; + for (const auto& value : p.as_string_array()) { + paramVec.push_back(value); + } + return foxglove::Parameter(p.get_name(), paramVec); } else { throw std::runtime_error("Unsupported parameter type"); } diff --git a/ros2_foxglove_bridge/tests/smoke_test.cpp b/ros2_foxglove_bridge/tests/smoke_test.cpp index 956b339..69e3029 100644 --- a/ros2_foxglove_bridge/tests/smoke_test.cpp +++ b/ros2_foxglove_bridge/tests/smoke_test.cpp @@ -254,16 +254,22 @@ TEST_F(ParameterTest, testGetParameters) { return param.getName() == p2; }); ASSERT_NE(p1Iter, params.end()); - EXPECT_EQ(PARAM_1_DEFAULT_VALUE, p1Iter->getValue()); + EXPECT_EQ(PARAM_1_DEFAULT_VALUE, p1Iter->getValue().getValue()); ASSERT_NE(p2Iter, params.end()); - EXPECT_EQ(PARAM_2_DEFAULT_VALUE, p2Iter->getValue()); + + std::vector int_array_val; + const auto array_params = p2Iter->getValue().getValue>(); + for (const auto& paramValue : array_params) { + int_array_val.push_back(paramValue.getValue()); + } + EXPECT_EQ(int_array_val, PARAM_2_DEFAULT_VALUE); } TEST_F(ParameterTest, testSetParameters) { const auto p1 = NODE_1_NAME + "." + PARAM_1_NAME; const auto p2 = NODE_2_NAME + "." + PARAM_2_NAME; const PARAM_1_TYPE newP1value = "world"; - const PARAM_2_TYPE newP2value = {4, 5, 6}; + const std::vector newP2value = {4, 5, 6}; const std::vector parameters = { foxglove::Parameter(p1, newP1value), @@ -285,9 +291,16 @@ TEST_F(ParameterTest, testSetParameters) { return param.getName() == p2; }); ASSERT_NE(p1Iter, params.end()); - EXPECT_EQ(newP1value, p1Iter->getValue()); + EXPECT_EQ(newP1value, p1Iter->getValue().getValue()); ASSERT_NE(p2Iter, params.end()); - EXPECT_EQ(newP2value, p2Iter->getValue()); + + std::vector int_array_val; + const auto array_params = p2Iter->getValue().getValue>(); + for (const auto& paramValue : array_params) { + int_array_val.push_back(paramValue.getValue()); + } + const std::vector expected_value = {4, 5, 6}; + EXPECT_EQ(int_array_val, expected_value); } TEST_F(ParameterTest, testSetParametersWithReqId) {