Skip to content

Commit

Permalink
Add support for nested parameters (ROS1)
Browse files Browse the repository at this point in the history
  • Loading branch information
@achim-k
achim-k committed May 1, 2023
1 parent f208f96 commit cc07ce7
Show file tree
Hide file tree
Showing 10 changed files with 266 additions and 216 deletions.
59 changes: 36 additions & 23 deletions foxglove_bridge_base/include/foxglove_bridge/parameter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <any>
#include <stdint.h>
#include <string>
#include <unordered_map>
#include <vector>

namespace foxglove {
Expand All @@ -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<unsigned char>& value);
ParameterValue(const std::vector<ParameterValue>& value);
ParameterValue(const std::unordered_map<std::string, ParameterValue>& value);

inline ParameterType getType() const {
return _type;
}

template <typename T>
inline const T& getValue() const {
return std::any_cast<const T&>(_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<unsigned char>& value);
Parameter(const std::string& name, const std::vector<bool>& value);
Parameter(const std::string& name, const std::vector<int>& value);
Parameter(const std::string& name, const std::vector<int64_t>& value);
Parameter(const std::string& name, const std::vector<double>& value);
Parameter(const std::string& name, const std::vector<std::string>& 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 <typename T>
inline const T& getValue() const {
return std::any_cast<const T&>(_value);
inline const ParameterValue& getValue() const {
return _value;
}

private:
std::string _name;
ParameterType _type;
std::any _value;
ParameterValue _value;
};

} // namespace foxglove
2 changes: 2 additions & 0 deletions foxglove_bridge_base/include/foxglove_bridge/serialization.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
80 changes: 26 additions & 54 deletions foxglove_bridge_base/src/parameter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int64_t>(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<int64_t>(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<unsigned char>& 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<unsigned char>& value)
: _type(ParameterType::PARAMETER_BYTE_ARRAY)
, _value(value) {}

Parameter::Parameter(const std::string& name, const std::vector<bool>& value)
: _name(name)
, _type(ParameterType::PARAMETER_BOOL_ARRAY)
ParameterValue::ParameterValue(const std::vector<ParameterValue>& value)
: _type(ParameterType::PARAMETER_ARRAY)
, _value(value) {}

Parameter::Parameter(const std::string& name, const std::vector<int>& value)
: _name(name)
, _type(ParameterType::PARAMETER_INTEGER_ARRAY)
, _value(std::vector<int64_t>(value.begin(), value.end())) {}

Parameter::Parameter(const std::string& name, const std::vector<int64_t>& value)
: _name(name)
, _type(ParameterType::PARAMETER_INTEGER_ARRAY)
ParameterValue::ParameterValue(const std::unordered_map<std::string, ParameterValue>& value)
: _type(ParameterType::PARAMETER_STRUCT)
, _value(value) {}

Parameter::Parameter(const std::string& name, const std::vector<double>& 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<std::string>& value)
, _value(ParameterValue()) {}
Parameter::Parameter(const std::string& name, const ParameterValue& value)
: _name(name)
, _type(ParameterType::PARAMETER_STRING_ARRAY)
, _value(value) {}

} // namespace foxglove
95 changes: 42 additions & 53 deletions foxglove_bridge_base/src/serialization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,35 +28,56 @@ void from_json(const nlohmann::json& j, Channel& c) {
c = Channel(j["id"].get<ChannelId>(), 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<bool>();
j = p.getValue<bool>();
} else if (paramType == ParameterType::PARAMETER_INTEGER) {
j["value"] = p.getValue<int64_t>();
j = p.getValue<int64_t>();
} else if (paramType == ParameterType::PARAMETER_DOUBLE) {
j["value"] = p.getValue<double>();
j = p.getValue<double>();
} else if (paramType == ParameterType::PARAMETER_STRING) {
j["value"] = p.getValue<std::string>();
j = p.getValue<std::string>();
} else if (paramType == ParameterType::PARAMETER_BYTE_ARRAY) {
const auto& paramValue = p.getValue<std::vector<unsigned char>>();
const std::string_view strValue(reinterpret_cast<const char*>(paramValue.data()),
paramValue.size());
j["value"] = base64Encode(strValue);
j["type"] = "byte_array";
} else if (paramType == ParameterType::PARAMETER_BOOL_ARRAY) {
j["value"] = p.getValue<std::vector<bool>>();
} else if (paramType == ParameterType::PARAMETER_INTEGER_ARRAY) {
j["value"] = p.getValue<std::vector<int64_t>>();
} else if (paramType == ParameterType::PARAMETER_DOUBLE_ARRAY) {
j["value"] = p.getValue<std::vector<double>>();
} else if (paramType == ParameterType::PARAMETER_STRING_ARRAY) {
j["value"] = p.getValue<std::vector<std::string>>();
j = base64Encode(strValue);
} else if (paramType == ParameterType::PARAMETER_STRUCT) {
j = p.getValue<std::unordered_map<std::string, ParameterValue>>();
} else if (paramType == ParameterType::PARAMETER_ARRAY) {
j = p.getValue<std::vector<ParameterValue>>();
} 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<std::string>());
} else if (jsonType == nlohmann::detail::value_t::boolean) {
p = ParameterValue(j.get<bool>());
} else if (jsonType == nlohmann::detail::value_t::number_integer) {
p = ParameterValue(j.get<int64_t>());
} else if (jsonType == nlohmann::detail::value_t::number_unsigned) {
p = ParameterValue(j.get<int64_t>());
} else if (jsonType == nlohmann::detail::value_t::number_float) {
p = ParameterValue(j.get<double>());
} else if (jsonType == nlohmann::detail::value_t::object) {
p = ParameterValue(j.get<std::unordered_map<std::string, ParameterValue>>());
} else if (jsonType == nlohmann::detail::value_t::array) {
p = ParameterValue(j.get<std::vector<ParameterValue>>());
}
}

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) {
Expand All @@ -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<std::string>());
} else if (j["type"] == "byte_array") {
p = Parameter(name, base64Decode(value.get<std::string>()));
} else {
throw std::runtime_error("Unsupported parameter 'type' value: " + j.dump());
}
} else if (jsonType == nlohmann::detail::value_t::boolean) {
p = Parameter(name, value.get<bool>());
} else if (jsonType == nlohmann::detail::value_t::number_integer) {
p = Parameter(name, value.get<int64_t>());
} else if (jsonType == nlohmann::detail::value_t::number_unsigned) {
p = Parameter(name, value.get<int64_t>());
} else if (jsonType == nlohmann::detail::value_t::number_float) {
p = Parameter(name, value.get<double>());
} 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<std::vector<std::string>>());
} else if (value.front().is_boolean()) {
p = Parameter(name, value.get<std::vector<bool>>());
} else if (value.front().is_number_integer()) {
p = Parameter(name, value.get<std::vector<int64_t>>());
} else if (value.front().is_number_unsigned()) {
p = Parameter(name, value.get<std::vector<int64_t>>());
} else if (value.front().is_number_float()) {
p = Parameter(name, value.get<std::vector<double>>());
} 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<std::string>()));
} else {
throw std::runtime_error("Unsupported type");
p = Parameter(name, pValue);
}
}

Expand Down
1 change: 1 addition & 0 deletions ros1_foxglove_bridge/include/foxglove_bridge/param_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::regex> parseRegexPatterns(const std::vector<std::string>& strings);

Expand Down
Loading

0 comments on commit cc07ce7

Please sign in to comment.