Skip to content

Commit

Permalink
Add ROS2 parameter declarator.
Browse files Browse the repository at this point in the history
  • Loading branch information
@asherikov
asherikov committed Jun 23, 2024
1 parent 2002183 commit 2cc2fab
Show file tree
Hide file tree
Showing 12 changed files with 650 additions and 140 deletions.
1 change: 1 addition & 0 deletions extra_visitors/ros2param/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ endif()
add_library(${TGT_ARILES_VISITOR_LIB}
${ARILES_VISITOR_${ARILES_VISITOR}_DIR}/src/reader.cpp
${ARILES_VISITOR_${ARILES_VISITOR}_DIR}/src/writer.cpp
${ARILES_VISITOR_${ARILES_VISITOR}_DIR}/src/declarator.cpp
)

if(NOT ARILES_DEB_TARGETS)
Expand Down
20 changes: 20 additions & 0 deletions extra_visitors/ros2param/ariles2/visitors/ros2param.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@

#include "./ros2param/reader.h"
#include "./ros2param/writer.h"
#include "./ros2param/declarator.h"

namespace ariles2
{
Expand All @@ -39,5 +40,24 @@ namespace ariles2
{
using Reader = ariles2::cfgread::Visitor<ns_ros2param::Reader>;
using Writer = ariles2::cfgwrite::Visitor<ns_ros2param::Writer>;


class ARILES2_VISIBILITY_ATTRIBUTE Declarator
: public aggregate::Visitor<ros2param::Declarator, Defaults, ns_ros2param::Declarator>
{
public:
using AggregateBase = aggregate::Visitor<ros2param::Declarator, Defaults, ns_ros2param::Declarator>;


public:
template <class... t_Initializers>
Declarator(t_Initializers &&...initializers)
: AggregateBase(
std::tuple<>(),
std::forward_as_tuple(std::forward<t_Initializers>(initializers)...))
{
ARILES2_TRACE_FUNCTION;
}
};
};
} // namespace ariles2
54 changes: 54 additions & 0 deletions extra_visitors/ros2param/ariles2/visitors/ros2param/declarator.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
/**
@file
@author Alexander Sherikov
@copyright 2017-2024 Alexander Sherikov, Licensed under the Apache License, Version 2.0.
(see @ref LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
@brief
*/

#pragma once


namespace ariles2
{
namespace ns_ros2param
{
namespace impl
{
class ARILES2_VISIBILITY_ATTRIBUTE Declarator;
}



/**
* @brief Parameter declarator
*/
class ARILES2_VISIBILITY_ATTRIBUTE Declarator
: public serialization::PIMPLVisitor<write::Visitor, impl::Declarator>
{
public:
explicit Declarator(::rclcpp::Node *nh);


void flush();


void startMapEntry(const std::string &child_name);
void endMapEntry();

void startArray(const std::size_t size, const bool /*compact*/ = false);
void startArrayElement();
void endArrayElement();
void endArray();


#define ARILES2_BASIC_TYPE(type) void writeElement(const type &element, const Parameters &param);

ARILES2_MACRO_SUBSTITUTE(ARILES2_BASIC_TYPES_LIST)

#undef ARILES2_BASIC_TYPE
};
} // namespace ns_ros2param
} // namespace ariles2
209 changes: 209 additions & 0 deletions extra_visitors/ros2param/src/declarator.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,209 @@
/**
@file
@author Alexander Sherikov
@copyright 2018 Alexander Sherikov, Licensed under the Apache License, Version 2.0.
(see @ref LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
@brief
*/


#include <variant>
#include <boost/lexical_cast.hpp>

#include <ariles2/visitors/ros2param.h>

#include "modifier.h"


namespace ariles2
{
namespace ns_ros2param
{
namespace impl
{
class ARILES2_VISIBILITY_ATTRIBUTE Declarator : public ModifierImplBase
{
public:
using ModifierImplBase::ModifierImplBase;

[[nodiscard]] bool publishParameters() const
{
for (const rclcpp::Parameter &parameter : parameters_)
{
const rclcpp::ParameterValue &declared_value =
nh_->declare_parameter(parameter.get_name(), parameter.get_parameter_value());

ARILES2_ASSERT(
declared_value.get_type() == parameter.get_type(),
std::string("Parameter type mismatch: ") + parameter.get_name());
}
return (true);
}
};
} // namespace impl
} // namespace ns_ros2param
} // namespace ariles2


namespace ariles2
{
namespace ns_ros2param
{
Declarator::Declarator(::rclcpp::Node *nh)
{
makeImplPtr(nh);
}


void Declarator::flush()
{
ARILES2_TRACE_FUNCTION;
ARILES2_ASSERT(impl_->publishParameters(), "Failed to set parameters.");
}


void Declarator::startMapEntry(const std::string &child_name)
{
ARILES2_TRACE_FUNCTION;
ARILES2_TRACE_VALUE(child_name);
if (impl_->empty())
{
impl_->emplace(child_name);
}
else
{
if (impl_->back().isArray())
{
impl_->concatWithNodeAndEmplace(
impl_->separator_,
boost::lexical_cast<std::string>(impl_->back().index_),
impl_->separator_,
child_name);
}
else
{
impl_->concatWithNodeAndEmplace(impl_->separator_, child_name);
}
}
}

void Declarator::endMapEntry()
{
ARILES2_TRACE_FUNCTION;
impl_->pop();
}


void Declarator::startArray(const std::size_t size, const bool /*compact*/)
{
ARILES2_TRACE_FUNCTION;
if (impl_->back().isArray())
{
impl_->emplace(
impl_->concatWithNode(
impl_->separator_, boost::lexical_cast<std::string>(impl_->back().index_)),
/*index=*/0,
size);
}
else
{
impl_->emplace(impl_->back().node_, /*index=*/0, size);
}
}

void Declarator::startArrayElement()
{
ARILES2_TRACE_FUNCTION;
ARILES2_ASSERT(not impl_->back().isCompleted(), "Internal error: array has more elements than expected.");
}

void Declarator::endArrayElement()
{
ARILES2_TRACE_FUNCTION;
impl_->shiftArray();
}

void Declarator::endArray()
{
ARILES2_TRACE_FUNCTION;
impl_->setParameter();
impl_->pop();
}


void Declarator::writeElement(const unsigned char &element, const Parameters &)
{
ARILES2_TRACE_FUNCTION;
if (not impl_->back().tryPushArray<uint8_t>(element))
{
impl_->setParameter(static_cast<int64_t>(element));
}
}


void Declarator::writeElement(const float &element, const Parameters &)
{
ARILES2_TRACE_FUNCTION;
if (not impl_->back().tryPushArray<double>(element))
{
impl_->setParameter(static_cast<double>(element));
}
}


#define ARILES2_ROS2PARAM_NATIVE_TYPES_LIST \
ARILES2_BASIC_TYPE(bool) \
ARILES2_BASIC_TYPE(double) \
ARILES2_BASIC_TYPE(std::string)


#define ARILES2_BASIC_TYPE(type) \
void Declarator::writeElement(const type &element, const Parameters &) \
{ \
ARILES2_TRACE_FUNCTION; \
if (not impl_->back().tryPushArray(element)) \
{ \
impl_->setParameter(element); \
} \
}

ARILES2_MACRO_SUBSTITUTE(ARILES2_ROS2PARAM_NATIVE_TYPES_LIST)

#undef ARILES2_BASIC_TYPE


#define ARILES2_BASIC_TYPE(type) \
void Declarator::writeElement(const type &element, const Parameters &) \
{ \
ARILES2_TRACE_FUNCTION; \
if (not impl_->back().tryPushArray<int64_t>(element)) \
{ \
impl_->setParameter(static_cast<int64_t>(element)); \
} \
}

ARILES2_MACRO_SUBSTITUTE(ARILES2_BASIC_SIGNED_INTEGER_TYPES_LIST)

#undef ARILES2_BASIC_TYPE


#define ARILES2_BASIC_TYPE(type) \
void Declarator::writeElement(const type &element, const Parameters &) \
{ \
ARILES2_TRACE_FUNCTION; \
ARILES2_ASSERT( \
static_cast<uint64_t>(element) <= static_cast<uint64_t>(std::numeric_limits<int64_t>::max()), \
"Value is too large."); \
if (not impl_->back().tryPushArray<int64_t>(element)) \
{ \
impl_->setParameter(static_cast<int64_t>(element)); \
} \
}

ARILES2_MACRO_SUBSTITUTE(ARILES2_BASIC_UNSIGNED_INTEGER_TYPES_LIST_WITHOUT_BYTE)

#undef ARILES2_BASIC_TYPE
} // namespace ns_ros2param
} // namespace ariles2
62 changes: 62 additions & 0 deletions extra_visitors/ros2param/src/modifier.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
/**
@file
@author Alexander Sherikov
@copyright 2017-2024 Alexander Sherikov, Licensed under the Apache License, Version 2.0.
(see @ref LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
@brief
*/

#include "node_wrapper.h"


namespace ariles2
{
namespace ns_ros2param
{
class ARILES2_VISIBILITY_ATTRIBUTE ModifierImplBase : public serialization::NodeStackBase<ModifierNode>
{
public:
// https://docs.ros2.org/latest/api/rclcpp/classrclcpp_1_1Node.html
rclcpp::Node *nh_;

std::vector<rclcpp::Parameter> parameters_;

const std::string separator_ = ".";

public:
explicit ModifierImplBase(::rclcpp::Node *nh)
{
nh_ = nh;
}


template <class t_Element>
void setParameter(const t_Element element)
{
ARILES2_TRACE_FUNCTION;
ARILES2_TRACE_VALUE(back().node_);
ARILES2_TRACE_TYPE(t_Element);

parameters_.emplace_back(back().node_, element);
}

void setParameter(const std::string &element)
{
ARILES2_TRACE_FUNCTION;
ARILES2_TRACE_VALUE(back().node_);
parameters_.emplace_back(back().node_, element);
}

void setParameter()
{
ARILES2_TRACE_FUNCTION;
if (back().isBuiltinArray())
{
std::visit([this](auto &&arg) { setParameter(arg); }, back().array_values_);
}
}
};
} // namespace ns_ros2param
} // namespace ariles2
Loading

0 comments on commit 2cc2fab

Please sign in to comment.