diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 48555ed81f..598f37a50f 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -103,6 +103,16 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) endif() + + ament_add_gtest(test_types test/test_types.cpp) + if(TARGET test_types) + ament_target_dependencies(test_types + "test_msgs" + ) + target_link_libraries(test_types + ${PROJECT_NAME} + ) + endif() endif() ament_package() diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 82a6dbda60..e89b19a470 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -270,6 +270,21 @@ TEST_F(TestClient, construction_and_destruction) ASSERT_NO_THROW(rclcpp_action::create_client(client_node, action_name).reset()); } +TEST_F(TestClient, construction_and_destruction_callback_group) +{ + auto group = client_node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + ASSERT_NO_THROW( + rclcpp_action::create_client( + client_node->get_node_base_interface(), + client_node->get_node_graph_interface(), + client_node->get_node_logging_interface(), + client_node->get_node_waitables_interface(), + action_name, + group + ).reset()); +} + TEST_F(TestClient, async_send_goal_no_callbacks) { auto action_client = rclcpp_action::create_client(client_node, action_name); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 8dd20828cd..7c873f5ee1 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -95,6 +95,32 @@ TEST_F(TestServer, construction_and_destruction) (void)as; } +TEST_F(TestServer, construction_and_destruction_callback_group) +{ + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + auto group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(); + + using GoalHandle = rclcpp_action::ServerGoalHandle; + ASSERT_NO_THROW( + rclcpp_action::create_server( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}, + options, + group)); +} + TEST_F(TestServer, handle_goal_called) { auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); diff --git a/rclcpp_action/test/test_types.cpp b/rclcpp_action/test/test_types.cpp new file mode 100644 index 0000000000..f981261105 --- /dev/null +++ b/rclcpp_action/test/test_types.cpp @@ -0,0 +1,61 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include "rclcpp_action/types.hpp" + +TEST(TestActionTypes, goal_uuid_to_string) { + rclcpp_action::GoalUUID goal_id; + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = i; + } + EXPECT_STREQ("0123456789abcdef", rclcpp_action::to_string(goal_id).c_str()); + + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = 16u + i; + } + EXPECT_STREQ("101112131415161718191a1b1c1d1e1f", rclcpp_action::to_string(goal_id).c_str()); + + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = std::numeric_limits::max() - i; + } + EXPECT_STREQ("fffefdfcfbfaf9f8f7f6f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str()); +} + +TEST(TestActionTypes, goal_uuid_to_rcl_action_goal_info) { + rclcpp_action::GoalUUID goal_id; + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = i; + } + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rclcpp_action::convert(goal_id, &goal_info); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); + } +} + +TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) { + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_info.goal_id.uuid[i] = i; + } + + rclcpp_action::GoalUUID goal_id; + rclcpp_action::convert(goal_id, &goal_info); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); + } +}