Skip to content

Commit

Permalink
Fixed FixedLengthArray assigns in CompoundMessages (#11).
Browse files Browse the repository at this point in the history
  • Loading branch information
@StefanFabian
StefanFabian committed Jan 27, 2025
1 parent 35ef7d3 commit 2b4f834
Show file tree
Hide file tree
Showing 7 changed files with 58 additions and 54 deletions.
2 changes: 1 addition & 1 deletion ros_babel_fish/include/ros_babel_fish/macros.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -388,7 +388,7 @@
std::remove_const<std::remove_reference<decltype( ARRAY )>::type>::type>::value, \
"Second argument to macro needs to be of type ArrayMessageBase!" ); \
if ( ( ARRAY ).isFixedSize() ) { \
_RBF2_TEMPLATE_CALL_ARRAY_TYPES( FUNCTION, ( ARRAY ), true, true, __VA_ARGS__ ) \
_RBF2_TEMPLATE_CALL_ARRAY_TYPES( FUNCTION, ( ARRAY ), false, true, __VA_ARGS__ ) \
} else if ( ( ARRAY ).isBounded() ) { \
_RBF2_TEMPLATE_CALL_ARRAY_TYPES( FUNCTION, ( ARRAY ), true, false, __VA_ARGS__ ) \
} else { \
Expand Down
74 changes: 35 additions & 39 deletions ros_babel_fish/include/ros_babel_fish/messages/array_message.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,8 +74,8 @@ class ArrayMessage_ final : public ArrayMessageBase
using ReturnType = typename message_type_traits::array_type<T, FIXED_LENGTH>::ReturnType;
using ConstReturnType = typename message_type_traits::array_type<T, FIXED_LENGTH>::ConstReturnType;
using ArgumentType = typename message_type_traits::array_type<T, FIXED_LENGTH>::ArgumentType;
static_assert( ( FIXED_LENGTH && BOUNDED ) || !FIXED_LENGTH,
"Fixed length can only be true if bounded is also true!" );
static_assert( ( FIXED_LENGTH != BOUNDED ) || ( !FIXED_LENGTH && !BOUNDED ),
"Fixed length can only be true if bounded is not true and vice versa!" );

public:
RCLCPP_SMART_PTR_DEFINITIONS( ArrayMessage_<T, BOUNDED, FIXED_LENGTH> )
Expand Down Expand Up @@ -152,12 +152,10 @@ class ArrayMessage_ final : public ArrayMessageBase

void push_back( ArgumentType value )
{
if ( BOUNDED ) {
if ( FIXED_LENGTH )
throw std::length_error( "Can not push back on fixed length array!" );
// This means it is upper bound, otherwise the method would not be enabled / available
if ( member_->array_size_ <= member_->size_function( data_.get() ) )
throw std::length_error( "Exceeded upper bound!" );
if ( FIXED_LENGTH )
throw std::length_error( "Can not push back on fixed length array!" );
if ( BOUNDED && member_->array_size_ <= member_->size_function( data_.get() ) ) {
throw std::length_error( "Exceeded upper bound!" );
}
reinterpret_cast<std::vector<T> *>( data_.get() )->push_back( value );
}
Expand All @@ -176,9 +174,9 @@ class ArrayMessage_ final : public ArrayMessageBase

void resize( size_t length )
{
if ( FIXED_LENGTH )
throw std::length_error( "Can not resize fixed length array!" );
if ( BOUNDED ) {
if ( FIXED_LENGTH )
throw std::length_error( "Can not resize fixed length array!" );
// This means it is upper bound, otherwise the method would not be enabled / available
if ( member_->array_size_ < length )
throw std::length_error( "Exceeded upper bound!" );
Expand Down Expand Up @@ -222,13 +220,13 @@ class ArrayMessage_ final : public ArrayMessageBase
void _assign( const ArrayMessageBase &other ) override
{
if ( other.isBounded() ) {
if ( other.isFixedSize() ) {
_assignImpl<true, true>( other );
return;
}
_assignImpl<true, false>( other );
return;
}
if ( other.isFixedSize() ) {
_assignImpl<false, true>( other );
return;
}
_assignImpl<false, false>( other );
}

Expand All @@ -249,11 +247,11 @@ class ArrayMessage_ final : public ArrayMessageBase
{
const auto &other = o.as<ArrayMessageBase>();
if ( other.isBounded() ) {
if ( other.isFixedSize() ) {
return _isMessageEqualImpl<true, true>( other );
}
return _isMessageEqualImpl<true, false>( other );
}
if ( other.isFixedSize() ) {
return _isMessageEqualImpl<false, true>( other );
}
return _isMessageEqualImpl<false, false>( other );
}

Expand All @@ -275,7 +273,7 @@ template<typename T>
using ArrayMessage = ArrayMessage_<T, false, false>;

template<typename T>
using FixedLengthArrayMessage = ArrayMessage_<T, true, true>;
using FixedLengthArrayMessage = ArrayMessage_<T, false, true>;

template<typename T>
using BoundedArrayMessage = ArrayMessage_<T, true, false>;
Expand All @@ -284,8 +282,8 @@ using BoundedArrayMessage = ArrayMessage_<T, true, false>;
template<bool BOUNDED, bool FIXED_LENGTH>
class CompoundArrayMessage_ final : public ArrayMessageBase
{
static_assert( ( FIXED_LENGTH && BOUNDED ) || !FIXED_LENGTH,
"Fixed length can only be true if bounded is also true!" );
static_assert( ( FIXED_LENGTH != BOUNDED ) || ( !FIXED_LENGTH && !BOUNDED ),
"Fixed length can only be true if bounded is not true and vice versa!" );

public:
RCLCPP_SMART_PTR_DEFINITIONS( CompoundArrayMessage_<BOUNDED, FIXED_LENGTH> )
Expand Down Expand Up @@ -346,11 +344,10 @@ class CompoundArrayMessage_ final : public ArrayMessageBase

void push_back( const CompoundMessage &value )
{
if ( BOUNDED ) {
if ( FIXED_LENGTH )
throw std::length_error( "Can not push back on fixed length array!" );
if ( member_->array_size_ <= member_->size_function( data_.get() ) )
throw std::length_error( "Exceeded upper bound!" );
if ( FIXED_LENGTH )
throw std::length_error( "Can not push back on fixed length array!" );
if ( BOUNDED && member_->array_size_ <= member_->size_function( data_.get() ) ) {
throw std::length_error( "Exceeded upper bound!" );
}
const size_t index = size();
resize( index + 1 );
Expand Down Expand Up @@ -381,11 +378,10 @@ class CompoundArrayMessage_ final : public ArrayMessageBase
{
if ( length == values_.size() )
return;
if ( BOUNDED ) {
if ( FIXED_LENGTH )
throw std::length_error( "Can not resize fixed length array!" );
if ( member_->array_size_ < length )
throw std::length_error( "Exceeded upper bound!" );
if ( FIXED_LENGTH )
throw std::length_error( "Can not resize fixed length array!" );
if ( BOUNDED && member_->array_size_ < length ) {
throw std::length_error( "Exceeded upper bound!" );
}
member_->resize_function( data_.get(), length );
values_.resize( length );
Expand All @@ -396,7 +392,7 @@ class CompoundArrayMessage_ final : public ArrayMessageBase
void *p = member_->get_function( data_.get(), i );
if ( p == values_[i]->data_.get() )
return; // Content was not moved
std::shared_ptr<void> data( p, [parent = data_]( void * ) { (void)parent; } );
std::shared_ptr<void> data( p, [parent = data_]( const void * ) { (void)parent; } );
values_[i]->move( data );
}
}
Expand Down Expand Up @@ -489,13 +485,13 @@ class CompoundArrayMessage_ final : public ArrayMessageBase
void _assign( const ArrayMessageBase &other ) override
{
if ( other.isBounded() ) {
if ( other.isFixedSize() ) {
_assignImpl<true, true>( other );
return;
}
_assignImpl<true, false>( other );
return;
}
if ( other.isFixedSize() ) {
_assignImpl<false, true>( other );
return;
}
_assignImpl<false, false>( other );
}

Expand All @@ -516,11 +512,11 @@ class CompoundArrayMessage_ final : public ArrayMessageBase
{
const auto &other = o.as<ArrayMessageBase>();
if ( other.isBounded() ) {
if ( other.isFixedSize() ) {
return _isMessageEqualImpl<true, true>( other );
}
return _isMessageEqualImpl<true, false>( other );
}
if ( other.isFixedSize() ) {
return _isMessageEqualImpl<false, true>( other );
}
return _isMessageEqualImpl<false, false>( other );
}

Expand All @@ -542,7 +538,7 @@ class CompoundArrayMessage_ final : public ArrayMessageBase

using CompoundArrayMessage = CompoundArrayMessage_<false, false>;

using FixedLengthCompoundArrayMessage = CompoundArrayMessage_<true, true>;
using FixedLengthCompoundArrayMessage = CompoundArrayMessage_<false, true>;

using BoundedCompoundArrayMessage = CompoundArrayMessage_<true, false>;
} // namespace ros_babel_fish
Expand Down
8 changes: 4 additions & 4 deletions ros_babel_fish/include/ros_babel_fish/method_invoke_helpers.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -537,8 +537,8 @@ template<typename Callable, typename... Args>
auto invoke_for_array_message( ArrayMessageBase &msg, Callable &&f, Args &&...args )
{
if ( msg.isFixedSize() )
return impl::call_for_array_message<true, true>( msg, std::forward<Callable>( f ),
std::forward<Args>( args )... );
return impl::call_for_array_message<false, true>( msg, std::forward<Callable>( f ),
std::forward<Args>( args )... );
if ( msg.isBounded() )
return impl::call_for_array_message<true, false>( msg, std::forward<Callable>( f ),
std::forward<Args>( args )... );
Expand All @@ -550,8 +550,8 @@ template<typename Callable, typename... Args>
auto invoke_for_array_message( const ArrayMessageBase &msg, Callable &&f, Args &&...args )
{
if ( msg.isFixedSize() )
return impl::call_for_array_message<true, true>( msg, std::forward<Callable>( f ),
std::forward<Args>( args )... );
return impl::call_for_array_message<false, true>( msg, std::forward<Callable>( f ),
std::forward<Args>( args )... );
if ( msg.isBounded() )
return impl::call_for_array_message<true, false>( msg, std::forward<Callable>( f ),
std::forward<Args>( args )... );
Expand Down
16 changes: 8 additions & 8 deletions ros_babel_fish/src/messages/compound_message.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,7 @@ void initValueMessage( Message::SharedPtr &message, const MessageMemberIntrospec
message = ValueMessage<T>::template make_shared( member, data );
}

template<bool FIXED_LENGTH = false, bool BOUNDED = FIXED_LENGTH>
template<bool FIXED_LENGTH = false, bool BOUNDED = false>
struct ArrayInit {
template<typename T>
static void initArrayMessage( Message::SharedPtr &message, const MessageMemberIntrospection &member,
Expand Down Expand Up @@ -198,9 +198,9 @@ void ArrayInit<false, true>::initArrayMessage<ArrayMessageBase>( Message::Shared

template<>
template<>
void ArrayInit<true, true>::initArrayMessage<ArrayMessageBase>( Message::SharedPtr &,
const MessageMemberIntrospection &,
const std::shared_ptr<void> & )
void ArrayInit<true, false>::initArrayMessage<ArrayMessageBase>( Message::SharedPtr &,
const MessageMemberIntrospection &,
const std::shared_ptr<void> & )
{
throw std::runtime_error(
"Arrays of arrays are not supported by ROS2 (yet)! Please open an issue if this changed!" );
Expand All @@ -226,9 +226,9 @@ void ArrayInit<false, true>::initArrayMessage<CompoundMessage>( Message::SharedP

template<>
template<>
void ArrayInit<true, true>::initArrayMessage<CompoundMessage>( Message::SharedPtr &message,
const MessageMemberIntrospection &member,
const std::shared_ptr<void> &data )
void ArrayInit<true, false>::initArrayMessage<CompoundMessage>( Message::SharedPtr &message,
const MessageMemberIntrospection &member,
const std::shared_ptr<void> &data )
{
message = FixedLengthCompoundArrayMessage::make_shared( member, data );
}
Expand All @@ -250,7 +250,7 @@ void initValue( Message::SharedPtr &message, const MessageMemberIntrospection &m
RBF2_TEMPLATE_CALL( NormalArrayInit::initArrayMessage, member->type_id_, message, member,
sub_data );
} else {
using FixedLengthArrayInit = ArrayInit<true, true>;
using FixedLengthArrayInit = ArrayInit<true, false>;
RBF2_TEMPLATE_CALL( FixedLengthArrayInit::initArrayMessage, member->type_id_, message, member,
sub_data );
}
Expand Down
9 changes: 8 additions & 1 deletion ros_babel_fish/test/message.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -297,8 +297,15 @@ TEST( MessageTest, compoundMessage )
EXPECT_EQ( pose["position"]["x"], 1 );
EXPECT_EQ( pose["position"]["y"], 2 );
}

ASSERT_FALSE( shared_pointer_alive ) << "Data should be destructed after last reference is gone!";

// Bug report test
{
auto test_array = fish.create_message( "ros_babel_fish_test_msgs/msg/TestArray" );
auto subarray = fish.create_message( "ros_babel_fish_test_msgs/msg/TestSubArray" );
EXPECT_NO_THROW( subarray["floats"].as<FixedLengthArrayMessage<double>>()[0] = 12.3 );
EXPECT_NO_THROW( test_array.set<CompoundMessage>( "subarray", subarray ) );
}
}

TEST( MessageTest, arrayMessage )
Expand Down
2 changes: 1 addition & 1 deletion ros_babel_fish_test_msgs/msg/TestArray.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,4 +14,4 @@ builtin_interfaces/Duration[12] durations
string[] strings
TestSubArray[10] subarrays_fixed
TestSubArray[] subarrays

TestSubArray subarray
1 change: 1 addition & 0 deletions ros_babel_fish_test_msgs/msg/TestSubArray.msg
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
int32[] ints
string[<=10] strings
builtin_interfaces/Time[42] times
float64[12] floats

0 comments on commit 2b4f834

Please sign in to comment.