Skip to content

Commit

Permalink
Refactored action client and added new tests.
Browse files Browse the repository at this point in the history 
(cherry picked from commit 1f86a81)
  • Loading branch information
@StefanFabian
StefanFabian committed Oct 25, 2024
1 parent 2a2ba89 commit 39a3792
Show file tree
Hide file tree
Showing 3 changed files with 130 additions and 103 deletions.
29 changes: 19 additions & 10 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 @@ -70,11 +70,10 @@ class ArrayMessageBase : public Message
template<typename T, bool BOUNDED, bool FIXED_LENGTH>
class ArrayMessage_ final : public ArrayMessageBase
{
protected:
typedef typename message_type_traits::array_type<T, FIXED_LENGTH>::Reference Reference;
typedef typename message_type_traits::array_type<T, FIXED_LENGTH>::ReturnType ReturnType;
typedef typename message_type_traits::array_type<T, FIXED_LENGTH>::ConstReturnType ConstReturnType;
typedef typename message_type_traits::array_type<T, FIXED_LENGTH>::ArgumentType ArgumentType;
using Reference = typename message_type_traits::array_type<T, FIXED_LENGTH>::Reference;
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!" );

Expand All @@ -94,11 +93,11 @@ class ArrayMessage_ final : public ArrayMessageBase
using Message::operator[];

template<typename ENABLED = T>
typename std::enable_if<FIXED_LENGTH || !std::is_same<ENABLED, bool>::value, Reference>::type
typename std::enable_if_t<FIXED_LENGTH || !std::is_same_v<ENABLED, bool>, Reference>
operator[]( size_t index )
{
using Container =
typename std::conditional<FIXED_LENGTH, std::array<T, 987654321000>, std::vector<T>>::type;
typename std::conditional_t<FIXED_LENGTH, std::array<T, 987654321000>, std::vector<T>>;
if ( member_->get_function == nullptr ) {
if ( index >= size() )
throw std::out_of_range( "Index was out of range of array!" );
Expand All @@ -108,7 +107,7 @@ class ArrayMessage_ final : public ArrayMessageBase
}

template<typename ENABLED = T>
typename std::enable_if<!FIXED_LENGTH && std::is_same<ENABLED, bool>::value, Reference>::type
typename std::enable_if_t<!FIXED_LENGTH && std::is_same_v<ENABLED, bool>, Reference>
operator[]( size_t index )
{
// Need to specialize for bool because std::vector<bool> is specialized and uses one bit per
Expand All @@ -121,7 +120,7 @@ class ArrayMessage_ final : public ArrayMessageBase
ConstReturnType operator[]( size_t index ) const
{
using Container =
typename std::conditional<FIXED_LENGTH, std::array<T, 987654321000>, std::vector<T>>::type;
typename std::conditional_t<FIXED_LENGTH, std::array<T, 987654321000>, std::vector<T>>;
if ( member_->get_function == nullptr ) {
if ( index >= size() )
throw std::out_of_range( "Index was out of range of array!" );
Expand All @@ -143,7 +142,7 @@ class ArrayMessage_ final : public ArrayMessageBase

//! Method only for fixed length arrays to fill the array with the given value.
template<bool ENABLED = FIXED_LENGTH>
typename std::enable_if<ENABLED, void>::type fill( ArgumentType &value )
typename std::enable_if_t<ENABLED, void> fill( ArgumentType &value )
{
for ( size_t i = 0; i < maxSize(); ++i ) assign( i, value );
}
Expand Down Expand Up @@ -209,6 +208,16 @@ class ArrayMessage_ final : public ArrayMessageBase
}
}

template<typename ArrayT, size_t ArrayLength, bool ENABLED = FIXED_LENGTH>
typename std::enable_if_t<ENABLED, ArrayMessage_<T, BOUNDED, FIXED_LENGTH> &>
operator=( const std::array<ArrayT, ArrayLength> &other )
{
if ( size() != ArrayLength )
throw std::length_error( "Array size does not match!" );
for ( size_t i = 0; i < ArrayLength; ++i ) assign( i, other[i] );
return *this;
}

protected:
void _assign( const ArrayMessageBase &other ) override
{
Expand Down
29 changes: 13 additions & 16 deletions ros_babel_fish/src/detail/babel_fish_action_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,10 @@ Client<ros_babel_fish::impl::BabelFishAction>::async_send_goal( const CompoundMe
auto future = promise->get_future();
CompoundMessage goal_request( type_support_->goal_service_type_support->request() );
auto uuid = this->generate_goal_id();
auto &request_uuid = goal_request["goal_id"]["uuid"].as<FixedLengthArrayMessage<uint8_t>>();
for ( size_t i = 0; i < uuid.size(); ++i ) request_uuid[i] = uuid[i];
goal_request["goal"] = goal;
this->send_goal_request( goal_request.type_erased_message(), [this, promise, options = options,
uuid]( std::shared_ptr<void> response ) {
goal_request["goal_id"]["uuid"].as<FixedLengthArrayMessage<uint8_t>>() = uuid;
goal_request["goal"].as<CompoundMessage>() = goal;
this->send_goal_request( goal_request.type_erased_message(), [this, promise, options, uuid](
std::shared_ptr<void> response ) {
CompoundMessage goal_response( type_support_->goal_service_type_support->response(), response );
if ( !goal_response["accepted"].value<bool>() ) {
promise->set_value( nullptr );
Expand All @@ -61,7 +60,7 @@ Client<ros_babel_fish::impl::BabelFishAction>::async_send_goal( const CompoundMe
std::shared_ptr<GoalHandle> goal_handle(
new GoalHandle( goal_info, options.feedback_callback, options.result_callback ) );
{
std::lock_guard<std::mutex> guard( goal_handles_mutex_ );
std::lock_guard guard( goal_handles_mutex_ );
goal_handles_[uuid] = goal_handle;
}
promise->set_value( goal_handle );
Expand All @@ -74,7 +73,7 @@ Client<ros_babel_fish::impl::BabelFishAction>::async_send_goal( const CompoundMe

// Clean old goal handles
{
std::lock_guard<std::mutex> guard( goal_handles_mutex_ );
std::lock_guard guard( goal_handles_mutex_ );
auto it = goal_handles_.begin();
while ( it != goal_handles_.end() ) {
if ( !it->second.lock() )
Expand All @@ -90,7 +89,7 @@ std::shared_future<rclcpp_action::ClientGoalHandle<ros_babel_fish::impl::BabelFi
Client<impl::BabelFishAction>::async_get_result( typename GoalHandle::SharedPtr goal_handle,
ResultCallback result_callback )
{
std::lock_guard<std::mutex> lock( goal_handles_mutex_ );
std::lock_guard lock( goal_handles_mutex_ );
if ( goal_handles_.find( goal_handle->get_goal_id() ) == goal_handles_.end() )
throw exceptions::UnknownGoalHandleError();

Expand All @@ -109,15 +108,13 @@ std::shared_future<ros_babel_fish::CompoundMessage>
Client<impl::BabelFishAction>::async_cancel_goal( GoalHandle::SharedPtr goal_handle,
CancelCallback cancel_callback )
{
std::lock_guard<std::mutex> lock( goal_handles_mutex_ );
std::lock_guard lock( goal_handles_mutex_ );
if ( goal_handles_.count( goal_handle->get_goal_id() ) == 0 ) {
throw exceptions::UnknownGoalHandleError();
}
CompoundMessage cancel_request( type_support_->cancel_service_type_support->request() );
const auto &uuid = goal_handle->get_goal_id();
auto &request_uuid =
cancel_request["goal_info"]["goal_id"]["uuid"].as<FixedLengthArrayMessage<uint8_t>>();
for ( size_t i = 0; i < uuid.size(); ++i ) request_uuid[i] = uuid[i];
cancel_request["goal_info"]["goal_id"]["uuid"].as<FixedLengthArrayMessage<uint8_t>>() = uuid;
return async_cancel( cancel_request, cancel_callback );
}

Expand Down Expand Up @@ -145,7 +142,7 @@ Client<impl::BabelFishAction>::async_cancel( CompoundMessage cancel_request,
{
// Put promise in the heap to move it around.
auto promise = std::make_shared<std::promise<CancelResponse>>();
std::shared_future<CancelResponse> future( promise->get_future() );
std::shared_future future( promise->get_future() );
this->send_cancel_request(
cancel_request.type_erased_message(),
[this, cancel_callback, promise]( std::shared_ptr<void> response ) mutable {
Expand Down Expand Up @@ -187,7 +184,7 @@ void Client<impl::BabelFishAction>::handle_feedback_message( std::shared_ptr<voi
feedback_message["goal_id"]["uuid"].as<FixedLengthArrayMessage<uint8_t>>();
for ( size_t i = 0; i < goal_id.size(); ++i ) goal_id[i] = feedback_goal_id[i];

std::lock_guard<std::mutex> guard( goal_handles_mutex_ );
std::lock_guard guard( goal_handles_mutex_ );
// Feedback is not for one of our goal handles
auto it = goal_handles_.find( goal_id );
if ( it == goal_handles_.end() )
Expand All @@ -212,7 +209,7 @@ void Client<impl::BabelFishAction>::handle_status_message( std::shared_ptr<void>
{
CompoundMessage status_message( *type_support_->status_message_type_support, message );

std::lock_guard<std::mutex> guard( goal_handles_mutex_ );
std::lock_guard guard( goal_handles_mutex_ );
const auto &status_list = status_message["status_list"].as<CompoundArrayMessage>();
for ( size_t i = 0; i < status_list.size(); ++i ) {
const auto &status = status_list[i];
Expand Down Expand Up @@ -255,7 +252,7 @@ void Client<ros_babel_fish::impl::BabelFishAction>::make_result_aware( GoalHandl
wrapped_result.goal_id = goal_handle->get_goal_id();
wrapped_result.code = static_cast<ResultCode>( response["status"].value<int8_t>() );
goal_handle->set_result( wrapped_result );
std::lock_guard<std::mutex> lock( goal_handles_mutex_ );
std::lock_guard lock( goal_handles_mutex_ );
goal_handles_.erase( goal_handle->get_goal_id() );
} );
} catch ( rclcpp::exceptions::RCLError &ex ) {
Expand Down
175 changes: 98 additions & 77 deletions ros_babel_fish/test/action_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <ros_babel_fish_test_msgs/action/simple_test.hpp>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rosidl_typesupport_c/type_support_map.h>

using namespace ros_babel_fish;
Expand Down Expand Up @@ -45,7 +46,6 @@ TEST( ActionClientTest, actionLookup )

const rosidl_action_type_support_t *ts = rosidl_typesupport_cpp::get_action_type_support_handle<
ros_babel_fish_test_msgs::action::SimpleTest>();
// EXPECT_EQ(type_support->type_support_handle, *ts);
EXPECT_NE( ts, nullptr );
EXPECT_TRUE( Equal( type_support->type_support_handle, *ts ) );

Expand All @@ -59,89 +59,110 @@ TEST( ActionClientTest, actionLookup )
EXPECT_EQ( ts_map, map );
}

TEST( ActionClientTest, simpleActionClient )
TEST( ActionClientTest, actionClient )
{
// auto provider = std::make_shared<MessageOnlyDescriptionProvider>();
// BabelFish fish( provider );
// provider->registerMessageByDefinition( ros::message_traits::datatype<SimpleTestAction>(),
// ros::message_traits::definition<SimpleTestAction>());
// MessageDescription::ConstSharedPtr goal_description = provider->getMessageDescription(
// ros::message_traits::datatype<SimpleTestActionGoal>());
// actionlib::SimpleActionClient <BabelFishAction> client( goal_description, "simple" );
// if ( !client.waitForServer( ros::Duration( 10 )))
// FAIL() << "ActionServer did not start within 10 seconds!";
// ASSERT_TRUE( client.isServerConnected());
//
// // This goal should succeed
// Message::SharedPtr goal = fish.createMessage( "ros_babel_fish_test_msgs/SimpleTestGoal" );
// (*goal)["goal"] = 5;
// CompoundMessage::ConstSharedPtr goal_msg = fish.translateMessage( goal );
// actionlib::SimpleClientGoalState state = client.sendGoalAndWait( *goal_msg, ros::Duration( 10 ));
// EXPECT_EQ( state, actionlib::SimpleClientGoalState::SUCCEEDED );
// CompoundMessage::ConstSharedPtr result = client.getResult();
// TranslatedMessage::ConstSharedPtr translated = fish.translateMessage( result );
// EXPECT_EQ((*translated->translated_message)["result"].value<int32_t>(), 4 );
//
// // This goal should abort after 10
// goal = fish.createMessage( "ros_babel_fish_test_msgs/SimpleTestGoal" );
// (*goal)["goal"] = 20;
// goal_msg = fish.translateMessage( goal );
// std::vector<int> feedback_values;
// client.sendGoal( *goal_msg, {}, {}, boost::function < void(
// const CompoundMessage::ConstSharedPtr & )>(
// [ & ]( const CompoundMessage::ConstSharedPtr &feedback )
// {
// TranslatedMessage::ConstSharedPtr translated = fish.translateMessage( feedback );
// feedback_values.push_back((*translated->translated_message)["feedback"].value<int32_t>());
// }));
// ASSERT_EQ( client.getState(), actionlib::SimpleClientGoalState::PENDING );
// if ( !client.waitForResult( ros::Duration( 10 )))
// {
// FAIL() << "ActionServer did not finish in 10 seconds!";
// }
// ASSERT_EQ( feedback_values.size(), 10U );
// for ( int i = 0; i < 10; ++i )
// {
// if ( feedback_values[i] != i ) FAIL() << "Feedback at " << i << " should be " << i << "!";
// }
// EXPECT_EQ( client.getState(), actionlib::SimpleClientGoalState::ABORTED );
// result = client.getResult();
// translated = fish.translateMessage( result );
// EXPECT_EQ((*translated->translated_message)["result"].value<int32_t>(), 10 );
//
// // This goal should be preempted
// goal = fish.createMessage( "ros_babel_fish_test_msgs/SimpleTestGoal" );
// (*goal)["goal"] = 1000;
// goal_msg = fish.translateMessage( goal );
// feedback_values.clear();
// client.sendGoal( *goal_msg, {}, {}, boost::function < void(
// const CompoundMessage::ConstSharedPtr & )>(
// [ & ]( const CompoundMessage::ConstSharedPtr &feedback )
// {
// TranslatedMessage::ConstSharedPtr translated = fish.translateMessage( feedback );
// feedback_values.push_back((*translated->translated_message)["feedback"].value<int32_t>());
// }));
// usleep( 500000 ); // Sleep for 500ms
// client.cancelGoal();
// if ( !client.waitForResult( ros::Duration( 1 )))
// FAIL() << "ActionServer did not preempt in 1 second!";
// int last_feedback = 0;
// for ( size_t i = 0; i < feedback_values.size(); ++i )
// {
// if ( feedback_values[i] != int( i )) FAIL() << "Feedback at " << i << " should be " << i << "!";
// last_feedback = feedback_values[i];
// }
// EXPECT_EQ( client.getState(), actionlib::SimpleClientGoalState::PREEMPTED );
// result = client.getResult();
// translated = fish.translateMessage( result );
// EXPECT_EQ((*translated->translated_message)["result"].value<int32_t>(), last_feedback );
using namespace std::chrono_literals;
using Action = ros_babel_fish_test_msgs::action::SimpleTest;
rclcpp_action::Server<Action>::SharedPtr server = rclcpp_action::create_server<Action>(
node, "ros_babel_fish_client_test_server",
[]( const rclcpp_action::GoalUUID &, const std::shared_ptr<const Action::Goal> & ) {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
},
[]( const std::shared_ptr<rclcpp_action::ServerGoalHandle<Action>> & ) {
return rclcpp_action::CancelResponse::ACCEPT;
},
[]( const std::shared_ptr<rclcpp_action::ServerGoalHandle<Action>> &handle ) {
std::thread t( [handle]() {
auto result = std::make_shared<Action::Result>();
for ( int i = 0; i < handle->get_goal()->goal; ++i ) {
if ( i >= 10 && i >= handle->get_goal()->goal / 2 ) {
result->result = i;
handle->abort( result );
return;
}
std::this_thread::sleep_for( 50ms );
auto feedback = std::make_shared<Action::Feedback>();
feedback->feedback = i;
handle->publish_feedback( feedback );
if ( handle->is_canceling() ) {
result->result = i;
handle->canceled( result );
return;
}
}
result->result = handle->get_goal()->goal;
handle->succeed( result );
} );
t.detach();
} );

BabelFish fish;
auto client = fish.create_action_client( *node, "ros_babel_fish_client_test_server",
"ros_babel_fish_test_msgs/action/SimpleTest" );
ASSERT_TRUE( client->wait_for_action_server( 5s ) );
ASSERT_TRUE( client->action_server_is_ready() );

// This goal should succeed
auto goal = client->create_goal();
goal["goal"] = 2;
auto gh_future = client->async_send_goal( goal );
ASSERT_EQ( gh_future.wait_for( 30s ), std::future_status::ready );
auto goal_handle = gh_future.get();
ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_ACCEPTED );
auto result = client->async_get_result( goal_handle );
ASSERT_EQ( result.wait_for( 3s ), std::future_status::ready );
ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_SUCCEEDED );
auto result_msg = result.get();
EXPECT_EQ( ( *result_msg.result )["result"].value<int32_t>(), 2 );

// This goal should abort after 10
goal = client->create_goal();
goal["goal"] = 20;
std::vector<int> feedback_values;
gh_future = client->async_send_goal(
goal, { {},
[&feedback_values]( const BabelFishActionClient::GoalHandle::SharedPtr &,
const CompoundMessage::ConstSharedPtr &feedback ) {
feedback_values.push_back( ( *feedback )["feedback"].value<int32_t>() );
},
{} } );
ASSERT_EQ( gh_future.wait_for( 3s ), std::future_status::ready );
goal_handle = gh_future.get();
ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_ACCEPTED );
result = client->async_get_result( goal_handle );
ASSERT_EQ( result.wait_for( 10s ), std::future_status::ready );
ASSERT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_ABORTED );
result_msg = result.get();
EXPECT_EQ( ( *result_msg.result )["result"].value<int32_t>(), 10 );

ASSERT_EQ( feedback_values.size(), 10U );
for ( int i = 0; i < 10; ++i ) {
if ( feedback_values[i] != i )
FAIL() << "Feedback at " << i << " should be " << i << "!";
}

// This goal should be preempted
goal = client->create_goal();
goal["goal"] = 200;
gh_future = client->async_send_goal( goal );
ASSERT_EQ( gh_future.wait_for( 3s ), std::future_status::ready );
goal_handle = gh_future.get();
std::this_thread::sleep_for( 200ms );
auto cancel_response_future = client->async_cancel_goal( goal_handle );
ASSERT_EQ( cancel_response_future.wait_for( 1s ), std::future_status::ready )
<< "ActionServer did not cancel in 1 second!";
result = client->async_get_result( goal_handle );
ASSERT_EQ( result.wait_for( 10s ), std::future_status::ready );
EXPECT_EQ( goal_handle->get_status(), action_msgs::msg::GoalStatus::STATUS_CANCELED );
result_msg = result.get();
EXPECT_LT( ( *result_msg.result )["result"].value<int32_t>(), 200 );
}

int main( int argc, char **argv )
{
testing::InitGoogleTest( &argc, argv );
rclcpp::init( argc, argv );
node = std::make_shared<rclcpp::Node>( "service_client_test" );
node = std::make_shared<rclcpp::Node>( "action_client_test" );
std::thread spinner( []() { rclcpp::spin( node ); } );
int result = RUN_ALL_TESTS();
rclcpp::shutdown();
Expand Down

0 comments on commit 39a3792

Please sign in to comment.