Fixes service server segfaulting (#10) and adds a new test to cover t…

…his.

(cherry picked from commit 9c4ee99)
(cherry picked from commit e46eb1e)

ros_babel_fish/CMakeLists.txt

@@ -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)

ros_babel_fish/include/ros_babel_fish/detail/babel_fish_service.hpp

@@ -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 )

ros_babel_fish/src/detail/babel_fish_service.cpp

@@ -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,

ros_babel_fish/test/service_client.cpp → ros_babel_fish/test/service.cpp

@@ -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();