Skip to content

Commit

Permalink
Fixes service server segfaulting (#10) and adds a new test to cover t…
Browse files Browse the repository at this point in the history
…his.

(cherry picked from commit 9c4ee99)
(cherry picked from commit e46eb1e)
StefanFabian committed Dec 30, 2024
1 parent fa1099c commit 8ac2476
Showing 4 changed files with 26 additions and 15 deletions.
6 changes: 3 additions & 3 deletions ros_babel_fish/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -155,9 +155,9 @@ if (BUILD_TESTING)
ament_target_dependencies(test_message_encoding geometry_msgs ros_babel_fish_test_msgs)
target_link_libraries(test_message_encoding ${PROJECT_NAME})

ament_add_gtest(test_service_client test/service_client.cpp)
ament_target_dependencies(test_service_client example_interfaces ros_babel_fish_test_msgs)
target_link_libraries(test_service_client ${PROJECT_NAME})
ament_add_gtest(test_service test/service.cpp)
ament_target_dependencies(test_service example_interfaces ros_babel_fish_test_msgs)
target_link_libraries(test_service ${PROJECT_NAME})

ament_add_gtest(test_action_client test/action_client.cpp)
ament_target_dependencies(test_action_client action_tutorials_interfaces example_interfaces ros_babel_fish_test_msgs)
Original file line number Diff line number Diff line change
@@ -19,7 +19,8 @@ struct BabelFishService {
};
} // namespace impl

class BabelFishService : public rclcpp::ServiceBase, std::enable_shared_from_this<BabelFishService>
class BabelFishService : public rclcpp::ServiceBase,
public std::enable_shared_from_this<BabelFishService>
{
public:
RCLCPP_SMART_PTR_DEFINITIONS( BabelFishService )
3 changes: 1 addition & 2 deletions ros_babel_fish/src/detail/babel_fish_service.cpp
Original file line number Diff line number Diff line change
@@ -5,7 +5,6 @@
#include "ros_babel_fish/idl/serialization.hpp"

#include <rosidl_typesupport_introspection_cpp/service_introspection.hpp>
#include <utility>

namespace ros_babel_fish
{
@@ -76,7 +75,7 @@ std::shared_ptr<void> BabelFishService::create_request()

std::shared_ptr<rmw_request_id_t> BabelFishService::create_request_header()
{
return std::shared_ptr<rmw_request_id_t>();
return std::make_shared<rmw_request_id_t>();
}

void BabelFishService::handle_request( std::shared_ptr<rmw_request_id_t> request_header,
Original file line number Diff line number Diff line change
@@ -33,7 +33,7 @@ TEST( ServiceClientTest, tests )
( *req )["a"] = 512;
( *req )["b"] = 314;
BabelFishServiceClient::SharedPtr client = fish.create_service_client(
*node, "/test_service_client/two_ints", "example_interfaces/srv/AddTwoInts" );
*node, "/test_service_client/two_ints_client", "example_interfaces/srv/AddTwoInts" );
ASSERT_TRUE( client->wait_for_service( 5s ) );
std::shared_future<CompoundMessage::SharedPtr> response_future = client->async_send_request( req );
ASSERT_EQ( response_future.wait_for( 5s ), std::future_status::ready );
@@ -45,13 +45,24 @@ TEST( ServiceClientTest, tests )

TEST( ServiceTest, server )
{
// ros::service::waitForService( "/test_service_server/subscribers" );
// rosapi::Subscribers subscribers;
// subscribers.request_template.topic = "First test";
// EXPECT_TRUE( ros::service::call( "/test_service_server/subscribers", subscribers ));
// ASSERT_EQ( subscribers.response.subscribers.size(), 2UL );
// EXPECT_EQ( subscribers.response.subscribers[0], "First test" );
// EXPECT_EQ( subscribers.response.subscribers[1], "The answer to everything is:" );
BabelFish fish;
auto server = fish.create_service(
*node, "/test_service_server/two_ints_server", "example_interfaces/srv/AddTwoInts",
[]( CompoundMessage::SharedPtr req, CompoundMessage::SharedPtr resp ) {
resp->set( "sum", req->get<int64_t>( "a" ) + req->get<int64_t>( "b" ) + 1337 );
return true;
} );
auto req2 = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
req2->a = 512;
req2->b = 314;
auto client = node->create_client<example_interfaces::srv::AddTwoInts>(
"test_service_server/two_ints_server" );
ASSERT_TRUE( client->wait_for_service( 5s ) );
auto response = client->async_send_request( req2 );
ASSERT_TRUE( response.wait_for( 5s ) == std::future_status::ready );
auto result = response.get();
ASSERT_NE( result, nullptr );
EXPECT_EQ( result->sum, 512 + 314 + 1337 );
}

int main( int argc, char **argv )
@@ -61,7 +72,7 @@ int main( int argc, char **argv )
node = std::make_shared<rclcpp::Node>( "service_client_test" );
std::thread spinner( []() { rclcpp::spin( node ); } );
auto service_two_ints = node->create_service<example_interfaces::srv::AddTwoInts>(
"/test_service_client/two_ints", &twoIntServiceCallback );
"/test_service_client/two_ints_client", &twoIntServiceCallback );
int result = RUN_ALL_TESTS();
rclcpp::shutdown();
spinner.join();

0 comments on commit 8ac2476

Please sign in to comment.