From 39ac9ea3d0597ad1929c74b930dc66f4977c308d Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Thu, 29 Dec 2022 20:26:11 +0100 Subject: [PATCH 01/13] Creates a base package to execute queries. - Creates MaliputQueryNode as a ROS2 proxy to make RoadNetwork queries. - Provides a sample configuration file use maliput_malidrive. Signed-off-by: Agustin Alba Chicar --- maliput_ros/CMakeLists.txt | 21 +++ .../include/maliput_ros/ros/maliput_query.h | 61 +++++++ .../maliput_ros/ros/maliput_query_node.h | 151 ++++++++++++++++++ maliput_ros/launch/maliput_ros.launch.py | 90 +++++++++++ maliput_ros/package.xml | 9 ++ .../resources/maliput_malidrive_plugin.yml | 17 ++ maliput_ros/src/maliput_ros/CMakeLists.txt | 1 + .../src/maliput_ros/ros/CMakeLists.txt | 71 ++++++++ .../src/maliput_ros/ros/maliput_query_node.cc | 133 +++++++++++++++ .../maliput_ros/ros/maliput_query_server.cc | 67 ++++++++ 10 files changed, 621 insertions(+) create mode 100644 maliput_ros/include/maliput_ros/ros/maliput_query.h create mode 100644 maliput_ros/include/maliput_ros/ros/maliput_query_node.h create mode 100644 maliput_ros/launch/maliput_ros.launch.py create mode 100644 maliput_ros/resources/maliput_malidrive_plugin.yml create mode 100644 maliput_ros/src/maliput_ros/ros/CMakeLists.txt create mode 100644 maliput_ros/src/maliput_ros/ros/maliput_query_node.cc create mode 100644 maliput_ros/src/maliput_ros/ros/maliput_query_server.cc diff --git a/maliput_ros/CMakeLists.txt b/maliput_ros/CMakeLists.txt index 99d89ed..36a7571 100644 --- a/maliput_ros/CMakeLists.txt +++ b/maliput_ros/CMakeLists.txt @@ -13,6 +13,10 @@ message(STATUS "\n\n====== Finding 3rd Party Packages ======\n") find_package(ament_cmake REQUIRED) find_package(maliput REQUIRED) +find_package(maliput_ros_interfaces REQUIRED) +find_package(maliput_ros_translation REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(yaml-cpp REQUIRED) ############################################################################## @@ -66,8 +70,25 @@ endif() # Export ############################################################################## +install( + DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + ament_environment_hooks(setup.sh.in) ament_export_dependencies(ament_cmake) +ament_export_dependencies(maliput) +ament_export_dependencies(maliput_ros_interfaces) +ament_export_dependencies(maliput_ros_translation) +ament_export_dependencies(rclcpp) +ament_export_dependencies(rclcpp_lifecycle) ament_export_dependencies(yaml-cpp) ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET) ament_package() diff --git a/maliput_ros/include/maliput_ros/ros/maliput_query.h b/maliput_ros/include/maliput_ros/ros/maliput_query.h new file mode 100644 index 0000000..8a45b24 --- /dev/null +++ b/maliput_ros/include/maliput_ros/ros/maliput_query.h @@ -0,0 +1,61 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include +#include + +#include +#include +#include + +namespace maliput_ros { +namespace ros { + +/// @brief Proxy to a maliput::api::RoadNetwork to make queries to it. +class MaliputQuery final { + public: + /// @brief Constructs a MaliputQuery. + /// @param[in] road_network The maliput::api::RoadNetwork to hold. It must not be nullptr. + /// @throws maliput::common::assertion_error When @p road_network is nullptr. + explicit MaliputQuery(std::unique_ptr road_network) + : road_network_(std::move(road_network)) { + MALIPUT_THROW_UNLESS(road_network_ != nullptr); + } + + /// @return The maliput::api::RoadGeometry. + inline const maliput::api::RoadGeometry* road_geometry() const { return road_network_->road_geometry(); } + + private: + std::unique_ptr road_network_{}; +}; + +} // namespace ros +} // namespace maliput_ros diff --git a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h new file mode 100644 index 0000000..1ec8d89 --- /dev/null +++ b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h @@ -0,0 +1,151 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "maliput_ros/ros/maliput_query.h" + +namespace maliput_ros { +namespace ros { + +/// MaliputQueryNode is a LifecycleNode that serves as proxy to a maliput::api::RoadNetwork +/// set of queries. +/// +/// It defines the following parameters: +/// - yaml_configuration_path: a string which contains the path to a YAML file. The YAML is parsed +/// with maliput_ros::utils::LoadYamlConfigFile() to obtain a maliput_ros::utils::MaliputRoadNetworkConfiguration +/// and then use it to create a maliput::api::RoadNetwork from it. +/// +/// The following depicts what this node does on each state transtion: +/// - MaliputQueryNode::on_configure(): the maliput::api::RoadNetwork, the MaliputQuery and the services are created. +/// - MaliputQueryNode::on_activate(): the services are enabled to respond to queries. +/// - MaliputQueryNode::on_deactivate(): the services are disabled to respond to queries. +/// - MaliputQueryNode::on_cleanup(): the maliput::api::RoadNetwork, the MaliputQuery, and the services are torn down. +/// +/// This query server offers: +/// - /road_geometry: responds the maliput::api::RoadGeometry configuration. +class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { + public: + using LifecyleNodeCallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + /// Create a new MaliputQueryNode node with the specified name. + /// + /// Forwards a call: MaliputQueryNode(node_name, "", options) to signal empty namespace. + /// @param[in] node_name Name of the node. + /// @param[in] options Additional options to control creation of the node. + MaliputQueryNode(const std::string& node_name, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) + : MaliputQueryNode(node_name, "", options) {} + + /// Create a node based on the node name and a rclcpp::Context. + /// + /// @param[in] node_name Name of the node. + /// @param[in] namespace_ Namespace of the node. + /// @param[in] options Additional options to control creation of the node. + MaliputQueryNode(const std::string& node_name, const std::string& namespace_, + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + static constexpr const char* kRoadGeometryServiceName = "road_geometry"; + static constexpr const char* kYamlConfigurationPath = "yaml_configuration_path"; + static constexpr const char* kYamlConfigurationPathDescription = + "File path to the yaml file containing the maliput plugin RoadNework loader."; + + // @return The path to the YAMl file containing the maliput plugin configuration from the node parameter. + std::string GetMaliputYamlFilePath() const; + + // @brief Responds the maliput::api::RoadGeometry configuration. + // @pre The node must be in the ACTIVE state. + // @param[in] request Unused. + // @param[out] response Loads the maliput::api::RoadGeometry description. + void RoadGeometryCallback(const std::shared_ptr request, + std::shared_ptr response) const; + + // @brief Loads the maliput::api::RoadNetwork from the yaml_configuration_path contents. + // @return true When the load procedure is successful. + bool LoadMaliputQuery(); + + // @brief Deletes the maliput::api::RoadNetwork and the MaliputQuery. + void TearDownMaliputQuery(); + + // @brief Creates all services of this node. + // @return true + bool InitializeAllServices(); + + // @brief Creates all services of this node. + // @return true + void TearDownAllServices(); + + // @brief Loads the maliput_query_ and the services. + // @param[in] state Unused. + // @return LifecyleNodeCallbackReturn::SUCCESS When the load procedure is successful. + // Otherwise, LifecyleNodeCallbackReturn::FAILURE. + LifecyleNodeCallbackReturn on_configure(const rclcpp_lifecycle::State& state) override; + + // @brief Enables queries to all services. + // @param[in] state Unused. + // @return LifecyleNodeCallbackReturn::SUCCESS. + LifecyleNodeCallbackReturn on_activate(const rclcpp_lifecycle::State& state) override; + + // @brief Disables queries to all services. + // @param[in] state Unused. + // @return LifecyleNodeCallbackReturn::SUCCESS. + LifecyleNodeCallbackReturn on_deactivate(const rclcpp_lifecycle::State& state) override; + + // @brief Deletes all services and deletes maliput_query_. + // @param[in] state Unused. + // @return LifecyleNodeCallbackReturn::SUCCESS. + LifecyleNodeCallbackReturn on_cleanup(const rclcpp_lifecycle::State& state) override; + + // @brief No-op. + // @param[in] state Unused. + // @return LifecyleNodeCallbackReturn::SUCCESS. + LifecyleNodeCallbackReturn on_shutdown(const rclcpp_lifecycle::State& state) override { + RCLCPP_INFO(get_logger(), "on_shutdown"); + return LifecyleNodeCallbackReturn::SUCCESS; + } + + // Works as a barrier to all service callbacks. When it is true, callbacks can operate. + std::atomic is_active_; + // /road_geometry service. + rclcpp::Service::SharedPtr road_geometry_srv_; + // Proxy to a maliput::api::RoadNetwork queries. + std::unique_ptr maliput_query_; +}; + +} // namespace ros +} // namespace maliput_ros diff --git a/maliput_ros/launch/maliput_ros.launch.py b/maliput_ros/launch/maliput_ros.launch.py new file mode 100644 index 0000000..ce5683f --- /dev/null +++ b/maliput_ros/launch/maliput_ros.launch.py @@ -0,0 +1,90 @@ +# BSD 3-Clause License +# +# Copyright (c) 2022, Woven Planet. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +import launch +import launch_ros +import lifecycle_msgs + +def generate_launch_description(): + # YAML configuration path to load the maliput plugin configuration. + maliput_yaml_path = launch.substitutions.LaunchConfiguration('maliput_yaml_path') + + # YAML configuration path argument, to let the user configure it. + # The 'maliput_yaml_path' variable will hold this value. + maliput_yaml_path_arg = launch.actions.DeclareLaunchArgument( + 'maliput_yaml_path', + default_value='', + description='Path to the YAML file containing the maliput plugin configuration.') + + # The query server node. + maliput_query_server_node = launch_ros.actions.LifecycleNode( + package='maliput_ros', executable='maliput_ros', name='query_server', output='screen', + parameters=[{"yaml_configuration_path": maliput_yaml_path}]) + + # When the query_server reaches the 'inactive' state, make it take the 'activate' transition. + register_event_handler_for_query_server_reaches_inactive_state = launch.actions.RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=maliput_query_server_node, goal_state='inactive', + entities=[ + launch.actions.LogInfo( + msg="node 'query_server' reached the 'inactive' state, 'activating'."), + launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action(maliput_query_server_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + )), + ], + ) + ) + + # When the query_server node reaches the 'active' state, log a message. + register_event_handler_for_query_server_reaches_active_state = launch.actions.RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=maliput_query_server_node, goal_state='active', + entities=[ + launch.actions.LogInfo( + msg="node 'query_server' reached the 'active' state, launching 'listener'."), + ], + ) + ) + + # Make the talker node take the 'configure' transition. + emit_event_to_request_that_query_server_does_configure_transition = launch.actions.EmitEvent( + event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action(maliput_query_server_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + + return launch.LaunchDescription([ + maliput_yaml_path_arg, + register_event_handler_for_query_server_reaches_inactive_state, + register_event_handler_for_query_server_reaches_active_state, + maliput_query_server_node, + emit_event_to_request_that_query_server_does_configure_transition, + ]) diff --git a/maliput_ros/package.xml b/maliput_ros/package.xml index 0dba8f0..032f0ae 100644 --- a/maliput_ros/package.xml +++ b/maliput_ros/package.xml @@ -11,9 +11,18 @@ ament_cmake_doxygen + maliput + maliput_ros_interfaces + maliput_ros_translation + rclcpp + rclcpp_lifecycle yaml-cpp + launch_ros + lifecycle_msgs + ros2launch + ament_cmake_clang_format ament_cmake_gmock ament_cmake_gtest diff --git a/maliput_ros/resources/maliput_malidrive_plugin.yml b/maliput_ros/resources/maliput_malidrive_plugin.yml new file mode 100644 index 0000000..254b870 --- /dev/null +++ b/maliput_ros/resources/maliput_malidrive_plugin.yml @@ -0,0 +1,17 @@ +# Example configuration for maliput_malidrive and to load the TShapeRoad.xodr map. +# TODO: fix the path to the node. +maliput: + backend: "maliput_malidrive" + parameters: + road_geometry_id: "t_shape_road" + opendrive_file: "/home/agalbachicar/maliput_ws_focal/install/maliput_malidrive/share/maliput_malidrive/resources/odr/TShapeRoad.xodr" + linear_tolerance: "0.001" + max_linear_tolerance: "0.01" + angular_tolerance: "0.001" + scale_length: "1.0" + inertial_to_backend_frame_translation: "{0, 0, 0}" + build_policy: "sequential" + num_threads: "1" + simplification_policy: "none" + standard_strictness_policy: "strict" + omit_nondrivable_lanes: "true" diff --git a/maliput_ros/src/maliput_ros/CMakeLists.txt b/maliput_ros/src/maliput_ros/CMakeLists.txt index 512d2b1..7407adb 100644 --- a/maliput_ros/src/maliput_ros/CMakeLists.txt +++ b/maliput_ros/src/maliput_ros/CMakeLists.txt @@ -1 +1,2 @@ +add_subdirectory(ros) add_subdirectory(utils) diff --git a/maliput_ros/src/maliput_ros/ros/CMakeLists.txt b/maliput_ros/src/maliput_ros/ros/CMakeLists.txt new file mode 100644 index 0000000..b30f3b9 --- /dev/null +++ b/maliput_ros/src/maliput_ros/ros/CMakeLists.txt @@ -0,0 +1,71 @@ +############################################################################## +# Sources +############################################################################## + +add_library(maliput_ros_node + maliput_query_node.cc +) + +add_library(maliput_ros::maliput_ros_node ALIAS maliput_ros_node) + +set_target_properties(maliput_ros_node + PROPERTIES + OUTPUT_NAME maliput_ros_node +) + +target_include_directories(maliput_ros_node + PUBLIC + $ + $ + $ +) + +ament_target_dependencies(maliput_ros_node + maliput + maliput_ros_interfaces + maliput_ros_translation + rclcpp + rclcpp_lifecycle +) + +target_link_libraries(maliput_ros_node + maliput_ros::utils +) + +add_executable(maliput_ros + maliput_query_server.cc +) + +target_include_directories(maliput_ros + PUBLIC + $ + $ + $ +) + +ament_target_dependencies(maliput_ros + rclcpp + rclcpp_lifecycle +) + +target_link_libraries(maliput_ros + maliput_ros::maliput_ros_node +) + +############################################################################## +# Export +############################################################################## + +install( + TARGETS maliput_ros_node + EXPORT ${PROJECT_NAME}-targets + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +install( + TARGETS maliput_ros + EXPORT ${PROJECT_NAME}-targets + DESTINATION lib/${PROJECT_NAME} +) diff --git a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc new file mode 100644 index 0000000..2de3b45 --- /dev/null +++ b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc @@ -0,0 +1,133 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput_ros/ros/maliput_query_node.h" + +#include +#include + +#include +#include +#include +#include + +#include "maliput_ros/utils/yaml_parser.h" + +namespace maliput_ros { +namespace ros { + +MaliputQueryNode::MaliputQueryNode(const std::string& node_name, const std::string& namespace_, + const rclcpp::NodeOptions& options) + : rclcpp_lifecycle::LifecycleNode(node_name, namespace_, options), is_active_(false) { + RCLCPP_INFO(get_logger(), "MaliputQueryNode"); + // Initialize the parameter for the YAML file path configuration. + rcl_interfaces::msg::ParameterDescriptor maliput_plugin_yaml_file_path_descriptor; + maliput_plugin_yaml_file_path_descriptor.name = kYamlConfigurationPath; + maliput_plugin_yaml_file_path_descriptor.description = kYamlConfigurationPathDescription; + maliput_plugin_yaml_file_path_descriptor.read_only = true; + this->declare_parameter(maliput_plugin_yaml_file_path_descriptor.name, rclcpp::ParameterValue(std::string{}), + maliput_plugin_yaml_file_path_descriptor); +} + +void MaliputQueryNode::RoadGeometryCallback( + const std::shared_ptr, + std::shared_ptr response) const { + RCLCPP_INFO(get_logger(), "RoadGeometryCallback"); + if (!is_active_.load()) { + RCLCPP_WARN(get_logger(), "The node is not active yet."); + return; + } + response->road_geometry = maliput_ros_translation::ToRosMessage(maliput_query_->road_geometry()); +} + +std::string MaliputQueryNode::GetMaliputYamlFilePath() const { + return this->get_parameter(kYamlConfigurationPath).get_parameter_value().get(); +} + +bool MaliputQueryNode::LoadMaliputQuery() { + RCLCPP_INFO(get_logger(), "LoadMaliputQuery"); + RCLCPP_INFO(get_logger(), "File path: " + GetMaliputYamlFilePath()); + try { + const maliput_ros::utils::MaliputRoadNetworkConfiguration configurations = + maliput_ros::utils::LoadYamlConfigFile(GetMaliputYamlFilePath()); + std::unique_ptr road_network = + maliput::plugin::CreateRoadNetwork(configurations.backend_name, configurations.backend_parameters); + maliput_query_ = std::make_unique(std::move(road_network)); + } catch (std::runtime_error& e) { + RCLCPP_ERROR(get_logger(), std::string{"Error loading maliput RoadNetwork: "} + e.what()); + return false; + } + return true; +} + +void MaliputQueryNode::TearDownMaliputQuery() { + RCLCPP_INFO(get_logger(), "TearDownMaliputQuery"); + maliput_query_.reset(); +} + +bool MaliputQueryNode::InitializeAllServices() { + RCLCPP_INFO(get_logger(), "InitializeAllServices"); + road_geometry_srv_ = this->create_service( + kRoadGeometryServiceName, + std::bind(&MaliputQueryNode::RoadGeometryCallback, this, std::placeholders::_1, std::placeholders::_2)); + return true; +} + +void MaliputQueryNode::TearDownAllServices() { + RCLCPP_INFO(get_logger(), "TearDownAllServices"); + road_geometry_srv_.reset(); +} + +MaliputQueryNode::LifecyleNodeCallbackReturn MaliputQueryNode::on_activate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "on_activate"); + is_active_.store(true); + return LifecyleNodeCallbackReturn::SUCCESS; +} + +MaliputQueryNode::LifecyleNodeCallbackReturn MaliputQueryNode::on_deactivate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "on_deactivate"); + is_active_.store(false); + return LifecyleNodeCallbackReturn::SUCCESS; +} + +MaliputQueryNode::LifecyleNodeCallbackReturn MaliputQueryNode::on_configure(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "on_configure"); + const bool configure_result = LoadMaliputQuery() && InitializeAllServices(); + return configure_result ? LifecyleNodeCallbackReturn::SUCCESS : LifecyleNodeCallbackReturn::FAILURE; +} + +MaliputQueryNode::LifecyleNodeCallbackReturn MaliputQueryNode::on_cleanup(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "on_cleanup"); + TearDownAllServices(); + TearDownMaliputQuery(); + return LifecyleNodeCallbackReturn::SUCCESS; +} + +} // namespace ros +} // namespace maliput_ros diff --git a/maliput_ros/src/maliput_ros/ros/maliput_query_server.cc b/maliput_ros/src/maliput_ros/ros/maliput_query_server.cc new file mode 100644 index 0000000..86f8f57 --- /dev/null +++ b/maliput_ros/src/maliput_ros/ros/maliput_query_server.cc @@ -0,0 +1,67 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include +#include +#include + +#include + +#include "maliput_ros/ros/maliput_query_node.h" + +namespace maliput_ros { +namespace ros { +namespace node { +namespace { + +static constexpr const char* kNodeName = "maliput_query_server"; + +// force flush of the stdout buffer. +// this ensures a correct sync of all prints +// even when executed simultaneously within the launch file. +void ForceStdoutFlush() { setvbuf(stdout, NULL, _IONBF, BUFSIZ); } + +} // namespace + +int Main(int argc, char* argv[]) { + ForceStdoutFlush(); + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(kNodeName); + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + return 0; +} + +} // namespace node +} // namespace ros +} // namespace maliput_ros + +int main(int argc, char* argv[]) { return maliput_ros::ros::node::Main(argc, argv); } From 10e54a10c7c3ea3eba8a0838115e25eac27a9635 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Tue, 3 Jan 2023 10:42:52 +0100 Subject: [PATCH 02/13] Addresses comments. Signed-off-by: Agustin Alba Chicar --- maliput_ros/CMakeLists.txt | 5 ++++ .../maliput_ros/ros/maliput_query_node.h | 25 ++++++++++--------- maliput_ros/launch/maliput_ros.launch.py | 24 +++++++++--------- .../resources/maliput_malidrive_plugin.yml | 2 +- 4 files changed, 31 insertions(+), 25 deletions(-) diff --git a/maliput_ros/CMakeLists.txt b/maliput_ros/CMakeLists.txt index 36a7571..37eb444 100644 --- a/maliput_ros/CMakeLists.txt +++ b/maliput_ros/CMakeLists.txt @@ -82,6 +82,11 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +install(DIRECTORY + resources + DESTINATION share/${PROJECT_NAME}/ +) + ament_environment_hooks(setup.sh.in) ament_export_dependencies(ament_cmake) ament_export_dependencies(maliput) diff --git a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h index 1ec8d89..c347b9d 100644 --- a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h +++ b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h @@ -46,10 +46,19 @@ namespace ros { /// MaliputQueryNode is a LifecycleNode that serves as proxy to a maliput::api::RoadNetwork /// set of queries. /// -/// It defines the following parameters: +/// It defines the following ROS parameters: /// - yaml_configuration_path: a string which contains the path to a YAML file. The YAML is parsed /// with maliput_ros::utils::LoadYamlConfigFile() to obtain a maliput_ros::utils::MaliputRoadNetworkConfiguration -/// and then use it to create a maliput::api::RoadNetwork from it. +/// and then use it to create a maliput::api::RoadNetwork from it. The YAML file must have the following structure: +/// @code {.yaml} +/// maliput:" +/// backend: " +/// parameters: " +/// : " +/// : " +/// // ... +/// : " +/// @endcode /// /// The following depicts what this node does on each state transtion: /// - MaliputQueryNode::on_configure(): the maliput::api::RoadNetwork, the MaliputQuery and the services are created. @@ -63,20 +72,12 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { public: using LifecyleNodeCallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /// Create a new MaliputQueryNode node with the specified name. - /// - /// Forwards a call: MaliputQueryNode(node_name, "", options) to signal empty namespace. - /// @param[in] node_name Name of the node. - /// @param[in] options Additional options to control creation of the node. - MaliputQueryNode(const std::string& node_name, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : MaliputQueryNode(node_name, "", options) {} - - /// Create a node based on the node name and a rclcpp::Context. + /// Create a node based on the node name, namespace and rclcpp::Context. /// /// @param[in] node_name Name of the node. /// @param[in] namespace_ Namespace of the node. /// @param[in] options Additional options to control creation of the node. - MaliputQueryNode(const std::string& node_name, const std::string& namespace_, + MaliputQueryNode(const std::string& node_name, const std::string& namespace_ = "", const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: diff --git a/maliput_ros/launch/maliput_ros.launch.py b/maliput_ros/launch/maliput_ros.launch.py index ce5683f..c439f26 100644 --- a/maliput_ros/launch/maliput_ros.launch.py +++ b/maliput_ros/launch/maliput_ros.launch.py @@ -44,16 +44,17 @@ def generate_launch_description(): # The query server node. maliput_query_server_node = launch_ros.actions.LifecycleNode( - package='maliput_ros', executable='maliput_ros', name='query_server', output='screen', + package='maliput_ros', executable='maliput_ros', name='maliput_query_server', + namespace='maliput_ros', output='screen', parameters=[{"yaml_configuration_path": maliput_yaml_path}]) # When the query_server reaches the 'inactive' state, make it take the 'activate' transition. - register_event_handler_for_query_server_reaches_inactive_state = launch.actions.RegisterEventHandler( + register_event_handler_for_maliput_query_server_reaches_inactive_state = launch.actions.RegisterEventHandler( launch_ros.event_handlers.OnStateTransition( target_lifecycle_node=maliput_query_server_node, goal_state='inactive', entities=[ launch.actions.LogInfo( - msg="node 'query_server' reached the 'inactive' state, 'activating'."), + msg="node 'maliput_query_server' reached the 'inactive' state, 'activating'."), launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( lifecycle_node_matcher=launch.events.matches_action(maliput_query_server_node), transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, @@ -62,19 +63,18 @@ def generate_launch_description(): ) ) - # When the query_server node reaches the 'active' state, log a message. - register_event_handler_for_query_server_reaches_active_state = launch.actions.RegisterEventHandler( + # When the maliput_query_server node reaches the 'active' state, log a message. + register_event_handler_for_maliput_query_server_reaches_active_state = launch.actions.RegisterEventHandler( launch_ros.event_handlers.OnStateTransition( target_lifecycle_node=maliput_query_server_node, goal_state='active', entities=[ - launch.actions.LogInfo( - msg="node 'query_server' reached the 'active' state, launching 'listener'."), + launch.actions.LogInfo(msg="node 'maliput_query_server' reached the 'active' state."), ], ) ) - # Make the talker node take the 'configure' transition. - emit_event_to_request_that_query_server_does_configure_transition = launch.actions.EmitEvent( + # Make the maliput_query_server node take the 'configure' transition. + emit_event_to_request_that_maliput_query_server_does_configure_transition = launch.actions.EmitEvent( event=launch_ros.events.lifecycle.ChangeState( lifecycle_node_matcher=launch.events.matches_action(maliput_query_server_node), transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, @@ -83,8 +83,8 @@ def generate_launch_description(): return launch.LaunchDescription([ maliput_yaml_path_arg, - register_event_handler_for_query_server_reaches_inactive_state, - register_event_handler_for_query_server_reaches_active_state, + register_event_handler_for_maliput_query_server_reaches_inactive_state, + register_event_handler_for_maliput_query_server_reaches_active_state, maliput_query_server_node, - emit_event_to_request_that_query_server_does_configure_transition, + emit_event_to_request_that_maliput_query_server_does_configure_transition, ]) diff --git a/maliput_ros/resources/maliput_malidrive_plugin.yml b/maliput_ros/resources/maliput_malidrive_plugin.yml index 254b870..d8dc20d 100644 --- a/maliput_ros/resources/maliput_malidrive_plugin.yml +++ b/maliput_ros/resources/maliput_malidrive_plugin.yml @@ -1,9 +1,9 @@ # Example configuration for maliput_malidrive and to load the TShapeRoad.xodr map. -# TODO: fix the path to the node. maliput: backend: "maliput_malidrive" parameters: road_geometry_id: "t_shape_road" + # opendrive_file: "/opt/ros/foxy/share/maliput_malidrive/resources/odr/TShapeRoad.xodr" opendrive_file: "/home/agalbachicar/maliput_ws_focal/install/maliput_malidrive/share/maliput_malidrive/resources/odr/TShapeRoad.xodr" linear_tolerance: "0.001" max_linear_tolerance: "0.01" From 4814a2648b40fc216649857b500241556aa4c729 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 2 Jan 2023 19:25:59 +0100 Subject: [PATCH 03/13] Add tests to MaliputQuery. Signed-off-by: Agustin Alba Chicar --- maliput_ros/test/maliput_ros/CMakeLists.txt | 1 + .../test/maliput_ros/ros/CMakeLists.txt | 26 +++ .../test/maliput_ros/ros/maliput_mock.h | 161 ++++++++++++++++++ .../maliput_ros/ros/maliput_query_test.cc | 104 +++++++++++ 4 files changed, 292 insertions(+) create mode 100644 maliput_ros/test/maliput_ros/ros/CMakeLists.txt create mode 100644 maliput_ros/test/maliput_ros/ros/maliput_mock.h create mode 100644 maliput_ros/test/maliput_ros/ros/maliput_query_test.cc diff --git a/maliput_ros/test/maliput_ros/CMakeLists.txt b/maliput_ros/test/maliput_ros/CMakeLists.txt index 512d2b1..7407adb 100644 --- a/maliput_ros/test/maliput_ros/CMakeLists.txt +++ b/maliput_ros/test/maliput_ros/CMakeLists.txt @@ -1 +1,2 @@ +add_subdirectory(ros) add_subdirectory(utils) diff --git a/maliput_ros/test/maliput_ros/ros/CMakeLists.txt b/maliput_ros/test/maliput_ros/ros/CMakeLists.txt new file mode 100644 index 0000000..ae47234 --- /dev/null +++ b/maliput_ros/test/maliput_ros/ros/CMakeLists.txt @@ -0,0 +1,26 @@ +ament_add_gmock(maliput_query_test maliput_query_test.cc) + +macro(add_dependencies_to_test target) + if (TARGET ${target}) + + target_include_directories(${target} + PRIVATE + ${PROJECT_SOURCE_DIR}/include + ${CMAKE_CURRENT_SOURCE_DIR} + ${PROJECT_SOURCE_DIR}/test + ) + + ament_target_dependencies(${target} + maliput + ) + + target_link_libraries(${target} + maliput::test_utilities + maliput_ros::maliput_ros_node + maliput_ros_translation::maliput_ros_translation + ) + + endif() +endmacro() + +add_dependencies_to_test(maliput_query_test) diff --git a/maliput_ros/test/maliput_ros/ros/maliput_mock.h b/maliput_ros/test/maliput_ros/ros/maliput_mock.h new file mode 100644 index 0000000..51318a6 --- /dev/null +++ b/maliput_ros/test/maliput_ros/ros/maliput_mock.h @@ -0,0 +1,161 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace maliput_ros { +namespace ros { +namespace test { + +class RoadGeometryMock final : public maliput::api::RoadGeometry { + public: + MOCK_CONST_METHOD0(do_id, maliput::api::RoadGeometryId()); + MOCK_CONST_METHOD0(do_num_junctions, int()); + MOCK_CONST_METHOD1(do_junction, const maliput::api::Junction*(int)); + MOCK_CONST_METHOD0(do_num_branch_points, int()); + MOCK_CONST_METHOD1(do_branch_point, const maliput::api::BranchPoint*(int)); + MOCK_CONST_METHOD0(DoById, const maliput::api::RoadGeometry::IdIndex&()); + MOCK_CONST_METHOD2(DoToRoadPosition, + maliput::api::RoadPositionResult(const maliput::api::InertialPosition&, + const std::optional&)); + MOCK_CONST_METHOD2(DoFindRoadPositions, + std::vector(const maliput::api::InertialPosition&, double)); + MOCK_CONST_METHOD0(do_linear_tolerance, double()); + MOCK_CONST_METHOD0(do_angular_tolerance, double()); + MOCK_CONST_METHOD0(do_scale_length, double()); + MOCK_CONST_METHOD2(DoSampleAheadWaypoints, + std::vector(const maliput::api::LaneSRoute&, double)); + MOCK_CONST_METHOD0(do_inertial_to_backend_frame_translation, maliput::math::Vector3()); +}; + +class RoadRulebookMock final : public maliput::api::rules::RoadRulebook { + public: + MOCK_CONST_METHOD2(DoFindRules, + maliput::api::rules::RoadRulebook::QueryResults(const std::vector&, + double)); + MOCK_CONST_METHOD0(DoRules, maliput::api::rules::RoadRulebook::QueryResults()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + MOCK_CONST_METHOD1(DoGetRule, maliput::api::rules::RightOfWayRule(const maliput::api::rules::RightOfWayRule::Id&)); + MOCK_CONST_METHOD1(DoGetRule, maliput::api::rules::SpeedLimitRule(const maliput::api::rules::SpeedLimitRule::Id&)); + MOCK_CONST_METHOD1(DoGetRule, + maliput::api::rules::DirectionUsageRule(const maliput::api::rules::DirectionUsageRule::Id&)); +#pragma GCC diagnostic pop + MOCK_CONST_METHOD1(DoGetDiscreteValueRule, + maliput::api::rules::DiscreteValueRule(const maliput::api::rules::Rule::Id&)); + MOCK_CONST_METHOD1(DoGetRangeValueRule, maliput::api::rules::RangeValueRule(const maliput::api::rules::Rule::Id&)); +}; + +class TrafficLightBookMock final : public maliput::api::rules::TrafficLightBook { + public: + MOCK_CONST_METHOD1(DoGetTrafficLight, + const maliput::api::rules::TrafficLight*(const maliput::api::rules::TrafficLight::Id&)); + MOCK_CONST_METHOD0(DoTrafficLights, std::vector()); +}; + +class IntersectionBookMock final : public maliput::api::IntersectionBook { + public: + MOCK_METHOD0(DoGetIntersections, std::vector()); + MOCK_METHOD1(DoGetIntersection, maliput::api::Intersection*(const maliput::api::Intersection::Id&)); + MOCK_METHOD1(DoGetFindIntersection, maliput::api::Intersection*(const maliput::api::rules::TrafficLight::Id&)); + MOCK_METHOD1(DoGetFindIntersection, maliput::api::Intersection*(const maliput::api::rules::DiscreteValueRule::Id&)); + MOCK_METHOD1(DoGetFindIntersection, maliput::api::Intersection*(const maliput::api::InertialPosition&)); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + MOCK_METHOD1(DoGetFindIntersection, maliput::api::Intersection*(const maliput::api::rules::RightOfWayRule::Id&)); +#pragma GCC diagnostic pop +}; + +class PhaseRingBookMock : public maliput::api::rules::PhaseRingBook { + public: + MOCK_CONST_METHOD0(DoGetPhaseRings, std::vector()); + MOCK_CONST_METHOD1(DoGetPhaseRing, + std::optional(const maliput::api::rules::PhaseRing::Id&)); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + MOCK_CONST_METHOD1(DoFindPhaseRing, + std::optional(const maliput::api::rules::RightOfWayRule::Id&)); +#pragma GCC diagnostic pop + MOCK_CONST_METHOD1(DoFindPhaseRing, + std::optional(const maliput::api::rules::Rule::Id&)); +}; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +class RightOfWayRuleStateProviderMock final : public maliput::api::rules::RightOfWayRuleStateProvider { + public: + MOCK_CONST_METHOD1(DoGetState, std::optional( + const maliput::api::rules::RightOfWayRule::Id&)); +}; +#pragma GCC diagnostic pop + +class PhaseProviderMock final : public maliput::api::rules::PhaseProvider { + public: + MOCK_CONST_METHOD1( + DoGetPhase, std::optional(const maliput::api::rules::PhaseRing::Id&)); +}; + +class DiscreteValueRuleStateProviderMock final : public maliput::api::rules::DiscreteValueRuleStateProvider { + public: + MOCK_CONST_METHOD1(DoGetState, std::optional( + const maliput::api::rules::Rule::Id&)); + MOCK_CONST_METHOD3(DoGetState, + std::optional( + const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double)); +}; + +class RangeValueRuleStateProviderMock final : public maliput::api::rules::RangeValueRuleStateProvider { + public: + MOCK_CONST_METHOD1(DoGetState, std::optional( + const maliput::api::rules::Rule::Id&)); + MOCK_CONST_METHOD3(DoGetState, + std::optional( + const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double)); +}; + +} // namespace test +} // namespace ros +} // namespace maliput_ros diff --git a/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc new file mode 100644 index 0000000..aef404e --- /dev/null +++ b/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc @@ -0,0 +1,104 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput_ros/ros/maliput_query.h" + +#include +#include + +#include +#include +#include +#include + +#include "maliput_ros/ros/maliput_mock.h" + +namespace maliput_ros { +namespace ros { +namespace test { +namespace { + +class MaliputQueryTest : public ::testing::Test { + public: + void SetUp() override { + auto road_geometry = std::make_unique(); + road_geometry_ptr_ = road_geometry.get(); + auto road_rulebook = std::make_unique(); + road_rulebook_ptr_ = road_rulebook.get(); + auto traffic_light_book = std::make_unique(); + traffic_light_book_ptr_ = traffic_light_book.get(); + auto intersection_book = std::make_unique(); + intersection_book_ptr_ = intersection_book.get(); + auto phase_ring_book = std::make_unique(); + phase_ring_book_ptr_ = phase_ring_book.get(); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + auto right_of_way_rule_state_provider = std::make_unique(); + right_of_way_rule_state_provider_ptr_ = right_of_way_rule_state_provider.get(); +#pragma GCC diagnostic pop + auto phase_provider = std::make_unique(); + phase_provider_ptr_ = phase_provider.get(); + auto discrete_value_rule_state_provider = std::make_unique(); + discrete_value_rule_state_provider_ptr_ = discrete_value_rule_state_provider.get(); + auto range_value_rule_state_provider = std::make_unique(); + range_value_rule_state_provider_ptr_ = range_value_rule_state_provider.get(); + + auto road_network = std::make_unique( + std::move(road_geometry), std::move(road_rulebook), std::move(traffic_light_book), std::move(intersection_book), + std::move(phase_ring_book), std::move(right_of_way_rule_state_provider), std::move(phase_provider), + std::make_unique(), std::move(discrete_value_rule_state_provider), + std::move(range_value_rule_state_provider)); + + dut_ = std::make_unique(std::move(road_network)); + } + + RoadGeometryMock* road_geometry_ptr_{}; + RoadRulebookMock* road_rulebook_ptr_{}; + TrafficLightBookMock* traffic_light_book_ptr_{}; + IntersectionBookMock* intersection_book_ptr_{}; + PhaseRingBookMock* phase_ring_book_ptr_{}; + RightOfWayRuleStateProviderMock* right_of_way_rule_state_provider_ptr_{}; + PhaseProviderMock* phase_provider_ptr_{}; + DiscreteValueRuleStateProviderMock* discrete_value_rule_state_provider_ptr_{}; + RangeValueRuleStateProviderMock* range_value_rule_state_provider_ptr_{}; + std::unique_ptr dut_; +}; + +TEST_F(MaliputQueryTest, ConstructorValidation) { + std::unique_ptr road_network{}; + ASSERT_THROW({ MaliputQuery(std::move(road_network)); }, maliput::common::assertion_error); +} + +TEST_F(MaliputQueryTest, RoadGeometry) { + ASSERT_EQ(dut_->road_geometry(), static_cast(road_geometry_ptr_)); +} + +} // namespace +} // namespace test +} // namespace ros +} // namespace maliput_ros From b6df3edf9291224ae758c960d41c556c0a48dcfe Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Tue, 3 Jan 2023 11:34:50 +0100 Subject: [PATCH 04/13] Add tests to MaliputQueryNode. Signed-off-by: Agustin Alba Chicar --- maliput_ros/CMakeLists.txt | 2 + maliput_ros/package.xml | 3 +- .../src/maliput_ros/ros/maliput_query_node.cc | 4 +- .../test/maliput_ros/ros/CMakeLists.txt | 73 ++++ .../test/maliput_ros/ros/maliput_mock.h | 9 + .../ros/maliput_plugin_config_test.cc | 69 ++++ .../ros/maliput_plugin_config_test.h | 67 ++++ .../ros/maliput_query_node_test.cc | 368 ++++++++++++++++++ .../maliput_ros/ros/maliput_query_test.cc | 19 +- .../ros/maliput_ros_road_network_plugin.cc | 58 +++ .../maliput_ros/ros/test_maliput_plugin.yml | 4 + 11 files changed, 657 insertions(+), 19 deletions(-) create mode 100644 maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.cc create mode 100644 maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h create mode 100644 maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc create mode 100644 maliput_ros/test/maliput_ros/ros/maliput_ros_road_network_plugin.cc create mode 100644 maliput_ros/test/maliput_ros/ros/test_maliput_plugin.yml diff --git a/maliput_ros/CMakeLists.txt b/maliput_ros/CMakeLists.txt index 37eb444..be1450f 100644 --- a/maliput_ros/CMakeLists.txt +++ b/maliput_ros/CMakeLists.txt @@ -13,6 +13,7 @@ message(STATUS "\n\n====== Finding 3rd Party Packages ======\n") find_package(ament_cmake REQUIRED) find_package(maliput REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(maliput_ros_interfaces REQUIRED) find_package(maliput_ros_translation REQUIRED) find_package(rclcpp REQUIRED) @@ -89,6 +90,7 @@ install(DIRECTORY ament_environment_hooks(setup.sh.in) ament_export_dependencies(ament_cmake) +ament_export_dependencies(lifecycle_msgs) ament_export_dependencies(maliput) ament_export_dependencies(maliput_ros_interfaces) ament_export_dependencies(maliput_ros_translation) diff --git a/maliput_ros/package.xml b/maliput_ros/package.xml index 032f0ae..58511ba 100644 --- a/maliput_ros/package.xml +++ b/maliput_ros/package.xml @@ -11,16 +11,15 @@ ament_cmake_doxygen - maliput maliput_ros_interfaces maliput_ros_translation rclcpp rclcpp_lifecycle + lifecycle_msgs yaml-cpp launch_ros - lifecycle_msgs ros2launch ament_cmake_clang_format diff --git a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc index 2de3b45..d07af66 100644 --- a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc +++ b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc @@ -50,7 +50,9 @@ MaliputQueryNode::MaliputQueryNode(const std::string& node_name, const std::stri rcl_interfaces::msg::ParameterDescriptor maliput_plugin_yaml_file_path_descriptor; maliput_plugin_yaml_file_path_descriptor.name = kYamlConfigurationPath; maliput_plugin_yaml_file_path_descriptor.description = kYamlConfigurationPathDescription; - maliput_plugin_yaml_file_path_descriptor.read_only = true; + // TODO(agalbachicar): this has been enabled for testing. We should pass in the proper context + // to the node to make it read only. + maliput_plugin_yaml_file_path_descriptor.read_only = false; this->declare_parameter(maliput_plugin_yaml_file_path_descriptor.name, rclcpp::ParameterValue(std::string{}), maliput_plugin_yaml_file_path_descriptor); } diff --git a/maliput_ros/test/maliput_ros/ros/CMakeLists.txt b/maliput_ros/test/maliput_ros/ros/CMakeLists.txt index ae47234..b6e73dd 100644 --- a/maliput_ros/test/maliput_ros/ros/CMakeLists.txt +++ b/maliput_ros/test/maliput_ros/ros/CMakeLists.txt @@ -1,4 +1,66 @@ +# It is necessary to find gmock via this macro so GMOCK_INCLUDE_DIRS and GMOCK_LIBRARIES are populated. +# That way, we can use gmock linked to libraries that will be used by test executables later on. +ament_find_gmock() + +add_library(maliput_ros_plugin_config_test + maliput_plugin_config_test.cc +) + +add_library(maliput_ros::maliput_ros_plugin_config_test ALIAS maliput_ros_plugin_config_test) + +set_target_properties(maliput_ros_plugin_config_test + PROPERTIES + OUTPUT_NAME maliput_ros_plugin_config_test +) + +target_link_libraries(maliput_ros_plugin_config_test + maliput::api + maliput::plugin + ${GMOCK_LIBRARIES} +) + +target_include_directories(maliput_ros_plugin_config_test + PRIVATE + ${PROJECT_SOURCE_DIR}/test + "${GMOCK_INCLUDE_DIRS}" +) + +add_library(maliput_ros_road_network_plugin + maliput_ros_road_network_plugin.cc +) + +add_library(maliput_ros::maliput_ros_road_network_plugin ALIAS maliput_ros_road_network_plugin) + +set_target_properties(maliput_ros_road_network_plugin + PROPERTIES + OUTPUT_NAME maliput_ros_road_network_plugin +) + +target_link_libraries(maliput_ros_road_network_plugin + maliput::api + maliput::plugin + maliput_ros_plugin_config_test + ${GMOCK_LIBRARIES} +) + +target_include_directories(maliput_ros_road_network_plugin + PRIVATE + ${PROJECT_SOURCE_DIR}/test + "${GMOCK_INCLUDE_DIRS}" +) + +set(TEST_PLUGIN_INSTALL_DIR "/tmp/maliput_ros/test/plugins/") +set(TEST_YAML_CONFIGURATION_PLUGIN_INSTALL_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test_maliput_plugin.yml") + +install( + TARGETS maliput_ros_road_network_plugin + ARCHIVE DESTINATION ${TEST_PLUGIN_INSTALL_DIR} + LIBRARY DESTINATION ${TEST_PLUGIN_INSTALL_DIR} + RUNTIME DESTINATION ${TEST_PLUGIN_INSTALL_DIR} +) + ament_add_gmock(maliput_query_test maliput_query_test.cc) +ament_add_gmock(maliput_query_node_test maliput_query_node_test.cc) macro(add_dependencies_to_test target) if (TARGET ${target}) @@ -11,16 +73,27 @@ macro(add_dependencies_to_test target) ) ament_target_dependencies(${target} + lifecycle_msgs maliput + rclcpp + rclcpp_lifecycle ) target_link_libraries(${target} + maliput::plugin maliput::test_utilities maliput_ros::maliput_ros_node + maliput_ros::maliput_ros_plugin_config_test maliput_ros_translation::maliput_ros_translation ) + target_compile_definitions(${target} + PRIVATE + TEST_MALIPUT_PLUGIN_LIBDIR="${TEST_PLUGIN_INSTALL_DIR}" + TEST_YAML_CONFIGURATION_PLUGIN_INSTALL_PATH="${TEST_YAML_CONFIGURATION_PLUGIN_INSTALL_PATH}" + ) endif() endmacro() add_dependencies_to_test(maliput_query_test) +add_dependencies_to_test(maliput_query_node_test) diff --git a/maliput_ros/test/maliput_ros/ros/maliput_mock.h b/maliput_ros/test/maliput_ros/ros/maliput_mock.h index 51318a6..f3fce92 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_mock.h +++ b/maliput_ros/test/maliput_ros/ros/maliput_mock.h @@ -50,6 +50,7 @@ namespace maliput_ros { namespace ros { namespace test { +/// @brief Google mock maliput::api::RoadGeometry. class RoadGeometryMock final : public maliput::api::RoadGeometry { public: MOCK_CONST_METHOD0(do_id, maliput::api::RoadGeometryId()); @@ -71,6 +72,7 @@ class RoadGeometryMock final : public maliput::api::RoadGeometry { MOCK_CONST_METHOD0(do_inertial_to_backend_frame_translation, maliput::math::Vector3()); }; +/// @brief Google mock maliput::api::rules::RoadRulebook. class RoadRulebookMock final : public maliput::api::rules::RoadRulebook { public: MOCK_CONST_METHOD2(DoFindRules, @@ -89,6 +91,7 @@ class RoadRulebookMock final : public maliput::api::rules::RoadRulebook { MOCK_CONST_METHOD1(DoGetRangeValueRule, maliput::api::rules::RangeValueRule(const maliput::api::rules::Rule::Id&)); }; +/// @brief Google mock maliput::api::rules::TrafficLightBook. class TrafficLightBookMock final : public maliput::api::rules::TrafficLightBook { public: MOCK_CONST_METHOD1(DoGetTrafficLight, @@ -96,6 +99,7 @@ class TrafficLightBookMock final : public maliput::api::rules::TrafficLightBook MOCK_CONST_METHOD0(DoTrafficLights, std::vector()); }; +/// @brief Google mock maliput::api::IntersectionBook. class IntersectionBookMock final : public maliput::api::IntersectionBook { public: MOCK_METHOD0(DoGetIntersections, std::vector()); @@ -109,6 +113,7 @@ class IntersectionBookMock final : public maliput::api::IntersectionBook { #pragma GCC diagnostic pop }; +/// @brief Google mock maliput::api::rules::PhaseRingBook. class PhaseRingBookMock : public maliput::api::rules::PhaseRingBook { public: MOCK_CONST_METHOD0(DoGetPhaseRings, std::vector()); @@ -125,6 +130,7 @@ class PhaseRingBookMock : public maliput::api::rules::PhaseRingBook { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" +/// @brief Google mock maliput::api::rules::RightOfWayRuleStateProvider. class RightOfWayRuleStateProviderMock final : public maliput::api::rules::RightOfWayRuleStateProvider { public: MOCK_CONST_METHOD1(DoGetState, std::optional( @@ -132,12 +138,14 @@ class RightOfWayRuleStateProviderMock final : public maliput::api::rules::RightO }; #pragma GCC diagnostic pop +/// @brief Google mock maliput::api::rules::PhaseProvider. class PhaseProviderMock final : public maliput::api::rules::PhaseProvider { public: MOCK_CONST_METHOD1( DoGetPhase, std::optional(const maliput::api::rules::PhaseRing::Id&)); }; +/// @brief Google mock maliput::api::rules::DiscreteValueRuleStateProvider. class DiscreteValueRuleStateProviderMock final : public maliput::api::rules::DiscreteValueRuleStateProvider { public: MOCK_CONST_METHOD1(DoGetState, std::optional( @@ -147,6 +155,7 @@ class DiscreteValueRuleStateProviderMock final : public maliput::api::rules::Dis const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double)); }; +/// @brief Google mock maliput::api::rules::RangeValueRuleStateProvider. class RangeValueRuleStateProviderMock final : public maliput::api::rules::RangeValueRuleStateProvider { public: MOCK_CONST_METHOD1(DoGetState, std::optional( diff --git a/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.cc new file mode 100644 index 0000000..ae891fd --- /dev/null +++ b/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.cc @@ -0,0 +1,69 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput_ros/ros/maliput_plugin_config_test.h" + +#include + +namespace maliput_ros { +namespace ros { +namespace test { +namespace { + +RoadNetworkMockPointers road_network_ptrs_{}; +std::unique_ptr road_network_{}; + +} // namespace + +RoadNetworkMockPointers GetRoadNetworkMockPointers() { return road_network_ptrs_; } + +void RegisterRoadNetworkForPlugin(std::unique_ptr road_network) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + road_network_ptrs_ = RoadNetworkMockPointers{ + dynamic_cast(const_cast(road_network->road_geometry())), + dynamic_cast(const_cast(road_network->rulebook())), + dynamic_cast( + const_cast(road_network->traffic_light_book())), + dynamic_cast(road_network->intersection_book()), + dynamic_cast( + const_cast(road_network->phase_ring_book())), + dynamic_cast(road_network->right_of_way_rule_state_provider()), + dynamic_cast(road_network->phase_provider()), + dynamic_cast(road_network->discrete_value_rule_state_provider()), + dynamic_cast(road_network->range_value_rule_state_provider()), + }; +#pragma GCC diagnostic pop + road_network_ = std::move(road_network); +} + +std::unique_ptr GetRoadNetworkForPlugin() { return std::move(road_network_); } + +} // namespace test +} // namespace ros +} // namespace maliput_ros diff --git a/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h b/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h new file mode 100644 index 0000000..3be4572 --- /dev/null +++ b/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h @@ -0,0 +1,67 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include + +#include + +#include "maliput_ros/ros/maliput_mock.h" + +namespace maliput_ros { +namespace ros { +namespace test { + +/// Holds the pointers to all the mocked entities in a maliput::api::RoadGeometry class. +struct RoadNetworkMockPointers { + RoadGeometryMock* road_geometry{}; + RoadRulebookMock* road_rulebook{}; + TrafficLightBookMock* traffic_light_book{}; + IntersectionBookMock* intersection_book{}; + PhaseRingBookMock* phase_ring_book{}; + RightOfWayRuleStateProviderMock* right_of_way_state_provider{}; + PhaseProviderMock* phase_provider{}; + DiscreteValueRuleStateProviderMock* discrete_value_rule_state_provider{}; + RangeValueRuleStateProviderMock* range_value_rule_state_provider{}; +}; + +/// Register a maliput::api::RoadNetwork. +/// +/// @pre All the entities in the maliput::api::RoadNetwork are mocks. +void RegisterRoadNetworkForPlugin(std::unique_ptr road_network); + +/// @return A copy of the registered pointers via RegisterRoadNetworkForPlugin(). +RoadNetworkMockPointers GetRoadNetworkMockPointers(); + +/// @return The maliput::api::RoadNetwork registered via RegisterRoadNetworkForPlugin(). +std::unique_ptr GetRoadNetworkForPlugin(); + +} // namespace test +} // namespace ros +} // namespace maliput_ros \ No newline at end of file diff --git a/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc new file mode 100644 index 0000000..283139c --- /dev/null +++ b/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc @@ -0,0 +1,368 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput_ros/ros/maliput_query_node.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "maliput_ros/ros/maliput_mock.h" +#include "maliput_ros/ros/maliput_plugin_config_test.h" + +namespace maliput_ros { +namespace ros { +namespace test { +namespace { + +using ::testing::Return; + +// @return A maliput::api::RoadNetwork populated with gmock versions of it, except maliput::api::rules::RuleRegistry. +std::unique_ptr MakeRoadNetworkMock() { + auto road_geometry = std::make_unique(); + auto road_rulebook = std::make_unique(); + auto traffic_light_book = std::make_unique(); + auto intersection_book = std::make_unique(); + auto phase_ring_book = std::make_unique(); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + auto right_of_way_rule_state_provider = std::make_unique(); +#pragma GCC diagnostic pop + auto phase_provider = std::make_unique(); + auto discrete_value_rule_state_provider = std::make_unique(); + auto range_value_rule_state_provider = std::make_unique(); + + return std::make_unique( + std::move(road_geometry), std::move(road_rulebook), std::move(traffic_light_book), std::move(intersection_book), + std::move(phase_ring_book), std::move(right_of_way_rule_state_provider), std::move(phase_provider), + std::make_unique(), std::move(discrete_value_rule_state_provider), + std::move(range_value_rule_state_provider)); +} + +// Base test class for MaliputQueryNode. +class MaliputQueryNodeTest : public ::testing::Test { + public: + static constexpr const char* kNodeName = "my_name"; + static constexpr const char* kNodeNamespace = "/my_namespace"; + static constexpr const char* kYamlConfigurationPathParameterName = "yaml_configuration_path"; + static constexpr rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn kCallbackSuccess{ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS}; + static constexpr rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn kCallbackFailure{ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE}; + + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void SetUp() override { executor_ = std::make_shared(); } + + void TearDown() override { + executor_->cancel(); + if (spin_thread_.joinable()) { + spin_thread_.join(); + } + } + + // Adds a node to a the executor_ and starts a thread to spin the executor. + // Allows to process the service calls in this test while the futures are waited. + // + // @param[in] node The node to add to the executor_. + void AddNodeToExecutorAndSpin(std::shared_ptr node) { + executor_->add_node(node->get_node_base_interface()); + spin_thread_ = std::thread([this]() { executor_->spin(); }); + } + + std::thread spin_thread_; + std::shared_ptr executor_; +}; + +// Makes sure the name and the namespace are properly passed to the parent rclcpp_lifecycle::LifecycleNode class. +TEST_F(MaliputQueryNodeTest, ConstructorArguments) { + auto dut = std::make_shared(kNodeName, kNodeNamespace); + + ASSERT_EQ(std::string{kNodeName}, dut->get_name()); + ASSERT_EQ(std::string{kNodeNamespace}, dut->get_namespace()); +} + +// Makes sure that after construction, the node adds the "yaml_configuration_path" parameter. +// "use_sim_time" has been added by the parent node. +TEST_F(MaliputQueryNodeTest, DefineYamlConfigurationPathParameter) { + const std::set expected_parameter_names_set = { + kYamlConfigurationPathParameterName, // Defined by MaliputQueryNode + "use_sim_time", // defined by LifecycleNode + }; + + auto dut = std::make_shared(kNodeName, kNodeNamespace); + const std::vector dut_parameter_names_vector = dut->list_parameters({}, 0u).names; + const std::set dut_parameter_names_set{dut_parameter_names_vector.begin(), + dut_parameter_names_vector.end()}; + + ASSERT_EQ(expected_parameter_names_set, dut_parameter_names_set); +} + +// Makes sure the "yaml_configuration_path" parameter can be set. +// TODO(agalbachicar): this has been enabled for testing. We should pass in the proper context +// to the node to make it read only. +TEST_F(MaliputQueryNodeTest, CanSetYamlConfigurationPathParameter) { + const rclcpp::Parameter yaml_configuration_path_parameter{kYamlConfigurationPathParameterName, + rclcpp::ParameterValue("a_path")}; + // Callback for parameter setting. + bool parameter_was_set{false}; + auto callback_parameter_set = [¶meter_was_set](const std::vector& parameters) { + parameter_was_set = true; + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + }; + + auto dut = std::make_shared(kNodeName, kNodeNamespace); + auto unused_callback_handle = dut->add_on_set_parameters_callback(callback_parameter_set); + + ASSERT_TRUE(dut->set_parameter(yaml_configuration_path_parameter).successful); + ASSERT_TRUE(parameter_was_set); +} + +// Evaluates that the transition to the configure state tries to load the YAML configuration. +// Upon a bad file, this transition fails. +TEST_F(MaliputQueryNodeTest, WrongYamlConfigurationPathMakesTheConfigurationStageToFail) { + const rclcpp::Parameter yaml_configuration_path_parameter{kYamlConfigurationPathParameterName, + rclcpp::ParameterValue("wrong_file_path")}; + auto ret = kCallbackFailure; + + auto dut = std::make_shared(kNodeName, kNodeNamespace); + ASSERT_TRUE(dut->set_parameter(yaml_configuration_path_parameter).successful); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, dut->get_current_state().id()); + dut->trigger_transition(rclcpp_lifecycle::Transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE), ret); + ASSERT_EQ(kCallbackFailure, ret); +} + +// This test class is used when the node transitions successfully from the UNCONFIGURED to the ACTIVE stage passing by +// the CONFIGURATION transition. We provide a proper plugin configuration and each test must initialize the mocks +// according to the type of query. +class MaliputQueryNodeAfterConfigurationTest : public MaliputQueryNodeTest { + public: + static constexpr int kReplace{1}; + static constexpr const char* kEnvName = "MALIPUT_PLUGIN_PATH"; + static constexpr const char* kRoadNetowrkMockPluginPath = TEST_MALIPUT_PLUGIN_LIBDIR; + static constexpr const char* kRoadGeometryServiceName = "/my_namespace/road_geometry"; + static constexpr const char* kRoadGeometryServiceType = "maliput_ros_interfaces/srv/RoadGeometry"; + const std::string kYamlFilePath{TEST_YAML_CONFIGURATION_PLUGIN_INSTALL_PATH}; + const std::chrono::nanoseconds kTimeout = std::chrono::seconds(1); + const std::chrono::nanoseconds kTimeoutServiceCall = std::chrono::seconds(1); + const std::chrono::nanoseconds kSleepPeriod = std::chrono::milliseconds(100); + + void SetUp() override { + MaliputQueryNodeTest::SetUp(); + + // Configure the mock RoadNetwork plugin. + auto road_network = MakeRoadNetworkMock(); + RegisterRoadNetworkForPlugin(std::move(road_network)); + road_network_ptrs_ = GetRoadNetworkMockPointers(); + + // Configure the maliput plugin environment variable. + back_up_env_ = maliput::common::Filesystem::get_env_path(kEnvName); + ASSERT_TRUE(setenv(kEnvName, kRoadNetowrkMockPluginPath, kReplace) == 0); + + // Load the YAML configuration file for the node. + dut_ = std::make_shared(kNodeName, kNodeNamespace); + ASSERT_TRUE(dut_->set_parameter(kYamlConfigurationPathParameter).successful); + } + + void TearDown() override { + // Restore the maliput plugin environment variable. + ASSERT_TRUE(setenv(kEnvName, back_up_env_.c_str(), kReplace) == 0); + MaliputQueryNodeTest::TearDown(); + } + + // Transitions from UNCONFIGURED to INACTIVE by CONFIGURING the node. + void TransitionToConfigureFromUnconfigured() { + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, dut_->get_current_state().id()); + auto ret = kCallbackFailure; + dut_->trigger_transition(rclcpp_lifecycle::Transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE), ret); + ASSERT_EQ(kCallbackSuccess, ret); + } + + // Transitions from INACTIVE to ACTIVE the node. + void TransitionToActiveFromConfigured() { + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, dut_->get_current_state().id()); + auto ret = kCallbackFailure; + dut_->trigger_transition(rclcpp_lifecycle::Transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE), ret); + ASSERT_EQ(kCallbackSuccess, ret); + } + + RoadNetworkMockPointers road_network_ptrs_{}; + std::shared_ptr dut_{}; + + private: + const rclcpp::Parameter kYamlConfigurationPathParameter{kYamlConfigurationPathParameterName, + rclcpp::ParameterValue(kYamlFilePath)}; + std::string back_up_env_{}; +}; + +// This function waits for an event to happen in the node graph. +// It has been adapted from: +// https://github.com/ros2/rclcpp/blob/rolling/rclcpp_lifecycle/test/test_lifecycle_node.cpp#L40-L60 +// @return true When @p predicate becomes true before the timeout. Otherwise, false. +bool WaitForEvent(std::shared_ptr node, std::function predicate, + std::chrono::nanoseconds timeout, std::chrono::nanoseconds sleep_period) { + auto start = std::chrono::steady_clock::now(); + std::chrono::microseconds time_slept(0); + + bool predicate_result{}; + while (!(predicate_result = predicate()) && + time_slept < std::chrono::duration_cast(timeout)) { + rclcpp::Event::SharedPtr graph_event = node->get_graph_event(); + node->wait_for_graph_change(graph_event, sleep_period); + time_slept = std::chrono::duration_cast(std::chrono::steady_clock::now() - start); + } + return predicate_result; +} + +// This function waits for a service to become available in the node graph. +// It has been adapted from: +// https://github.com/ros2/rclcpp/blob/rolling/rclcpp_lifecycle/test/test_lifecycle_node.cpp#L62-L78 +// @return true When @p service_name service becomes available before @p timeout by iteratively asking every @p +// sleep_period. Otherwise, false. +bool WaitForService(std::shared_ptr node, const std::string& service_name, + std::chrono::nanoseconds timeout, std::chrono::nanoseconds sleep_period) { + return WaitForEvent( + node, + [node, service_name]() { + const auto service_names_and_types = node->get_service_names_and_types(); + return service_names_and_types.end() != service_names_and_types.find(service_name); + }, + timeout, sleep_period); +} + +// Makes sure the transition from UNCONFIGURED via CONFIGURE is successful. +TEST_F(MaliputQueryNodeAfterConfigurationTest, CorrectYamlConfigurationPathMakesTheConfigurationStageToPass) { + TransitionToConfigureFromUnconfigured(); +} + +// Makes sure the services are advertised when passing the configuration stage. +TEST_F(MaliputQueryNodeAfterConfigurationTest, ConfigureStateAdvertisesServices) { + TransitionToConfigureFromUnconfigured(); + ASSERT_TRUE(WaitForService(dut_, kRoadGeometryServiceName, kTimeout, kSleepPeriod)); + + auto service_names_and_types = dut_->get_service_names_and_types(); + + ASSERT_STREQ(service_names_and_types[kRoadGeometryServiceName][0].c_str(), kRoadGeometryServiceType); +} + +// Makes sure services don't process the request when the node is not ACTIVE. +TEST_F(MaliputQueryNodeAfterConfigurationTest, CallingServiceBeforeActiveYieldsToFailure) { + AddNodeToExecutorAndSpin(dut_); + TransitionToConfigureFromUnconfigured(); + ASSERT_TRUE(WaitForService(dut_, kRoadGeometryServiceName, kTimeout, kSleepPeriod)); + + auto road_geometry_service = dut_->create_client(kRoadGeometryServiceName); + auto request = std::make_shared(); + auto future_result = road_geometry_service->async_send_request(request); + const auto future_status = future_result.wait_for(kTimeoutServiceCall); + + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + ASSERT_TRUE(response->road_geometry.id.id.empty()); +} + +// Makes sure the node can transtion to the ACTIVE state. +TEST_F(MaliputQueryNodeAfterConfigurationTest, TransitionToActiveIsSuccessful) { + TransitionToConfigureFromUnconfigured(); + TransitionToActiveFromConfigured(); +} + +// Test class used to hold the configuration of the RoadGeometryMock and validate the result of the service call. +class TestRoadGeometryServiceCall : public MaliputQueryNodeAfterConfigurationTest { + public: + static constexpr int kSizeJunctions{0}; + static constexpr int kSizeBranchPoints{0}; + static constexpr double kLinearTolerance{1e-12}; + static constexpr double kAngularTolerance{5e-12}; + static constexpr double kScaleLength{1.}; + const maliput::math::Vector3 kInertialToBackendFrameTranslation{1., 2., 3.}; + const maliput::api::RoadGeometryId kRoadGeometryId{"road_geometry_id"}; + + void SetUp() override { + MaliputQueryNodeAfterConfigurationTest::SetUp(); + AddNodeToExecutorAndSpin(dut_); + TransitionToConfigureFromUnconfigured(); + TransitionToActiveFromConfigured(); + } +}; + +TEST_F(TestRoadGeometryServiceCall, RoadGeometryRequestInActiveIsSuccessful) { + // Note: there is no point in populating Junctions and BranchPoints, it is already covered + // in maliput_ros_translation package. + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_id()).WillRepeatedly(Return(kRoadGeometryId)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_linear_tolerance()).WillRepeatedly(Return(kLinearTolerance)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_angular_tolerance()).WillRepeatedly(Return(kAngularTolerance)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_scale_length()).WillRepeatedly(Return(kScaleLength)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_inertial_to_backend_frame_translation()) + .WillRepeatedly(Return(kInertialToBackendFrameTranslation)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_num_junctions()).WillRepeatedly(Return(kSizeJunctions)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_num_branch_points()).WillRepeatedly(Return(kSizeBranchPoints)); + + auto road_geometry_service = dut_->create_client(kRoadGeometryServiceName); + ASSERT_TRUE(road_geometry_service->wait_for_service(kTimeout)); + auto request = std::make_shared(); + auto future_result = road_geometry_service->async_send_request(request); + auto future_status = future_result.wait_for(kTimeoutServiceCall); + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + + ASSERT_EQ(response->road_geometry.id.id, kRoadGeometryId.string()); + ASSERT_EQ(response->road_geometry.linear_tolerance, kLinearTolerance); + ASSERT_EQ(response->road_geometry.angular_tolerance, kAngularTolerance); + ASSERT_EQ(response->road_geometry.scale_length, kScaleLength); + ASSERT_EQ(response->road_geometry.inertial_to_backend_frame_translation.x, kInertialToBackendFrameTranslation.x()); + ASSERT_EQ(response->road_geometry.inertial_to_backend_frame_translation.y, kInertialToBackendFrameTranslation.y()); + ASSERT_EQ(response->road_geometry.inertial_to_backend_frame_translation.z, kInertialToBackendFrameTranslation.z()); + ASSERT_EQ(response->road_geometry.junction_ids.size(), static_cast(kSizeJunctions)); + ASSERT_EQ(response->road_geometry.branch_point_ids.size(), static_cast(kSizeJunctions)); +} + +} // namespace +} // namespace test +} // namespace ros +} // namespace maliput_ros diff --git a/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc index aef404e..243ad54 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc +++ b/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc @@ -43,30 +43,23 @@ namespace ros { namespace test { namespace { +// Test class for MaliputQuery. class MaliputQueryTest : public ::testing::Test { public: void SetUp() override { auto road_geometry = std::make_unique(); road_geometry_ptr_ = road_geometry.get(); auto road_rulebook = std::make_unique(); - road_rulebook_ptr_ = road_rulebook.get(); auto traffic_light_book = std::make_unique(); - traffic_light_book_ptr_ = traffic_light_book.get(); auto intersection_book = std::make_unique(); - intersection_book_ptr_ = intersection_book.get(); auto phase_ring_book = std::make_unique(); - phase_ring_book_ptr_ = phase_ring_book.get(); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" auto right_of_way_rule_state_provider = std::make_unique(); - right_of_way_rule_state_provider_ptr_ = right_of_way_rule_state_provider.get(); #pragma GCC diagnostic pop auto phase_provider = std::make_unique(); - phase_provider_ptr_ = phase_provider.get(); auto discrete_value_rule_state_provider = std::make_unique(); - discrete_value_rule_state_provider_ptr_ = discrete_value_rule_state_provider.get(); auto range_value_rule_state_provider = std::make_unique(); - range_value_rule_state_provider_ptr_ = range_value_rule_state_provider.get(); auto road_network = std::make_unique( std::move(road_geometry), std::move(road_rulebook), std::move(traffic_light_book), std::move(intersection_book), @@ -78,22 +71,16 @@ class MaliputQueryTest : public ::testing::Test { } RoadGeometryMock* road_geometry_ptr_{}; - RoadRulebookMock* road_rulebook_ptr_{}; - TrafficLightBookMock* traffic_light_book_ptr_{}; - IntersectionBookMock* intersection_book_ptr_{}; - PhaseRingBookMock* phase_ring_book_ptr_{}; - RightOfWayRuleStateProviderMock* right_of_way_rule_state_provider_ptr_{}; - PhaseProviderMock* phase_provider_ptr_{}; - DiscreteValueRuleStateProviderMock* discrete_value_rule_state_provider_ptr_{}; - RangeValueRuleStateProviderMock* range_value_rule_state_provider_ptr_{}; std::unique_ptr dut_; }; +// Makes sure the maliput::api::RoadNetwork is not nullptr when constructing MaliputQuery. TEST_F(MaliputQueryTest, ConstructorValidation) { std::unique_ptr road_network{}; ASSERT_THROW({ MaliputQuery(std::move(road_network)); }, maliput::common::assertion_error); } +// Validates the maliput::api::RoadGeometry pointer. TEST_F(MaliputQueryTest, RoadGeometry) { ASSERT_EQ(dut_->road_geometry(), static_cast(road_geometry_ptr_)); } diff --git a/maliput_ros/test/maliput_ros/ros/maliput_ros_road_network_plugin.cc b/maliput_ros/test/maliput_ros/ros/maliput_ros_road_network_plugin.cc new file mode 100644 index 0000000..4fd8a4c --- /dev/null +++ b/maliput_ros/test/maliput_ros/ros/maliput_ros_road_network_plugin.cc @@ -0,0 +1,58 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include +#include +#include + +#include + +#include "maliput_ros/ros/maliput_plugin_config_test.h" + +namespace maliput_ros { +namespace ros { +namespace test { +namespace { + +// Plugin that simply returns the maliput::api::RoadNetwork registered via +// maliput_ros::test::RegisterRoadNetworkForPlugin(). +class RoadNetworkLoaderTest : public maliput::plugin::RoadNetworkLoader { + public: + std::unique_ptr operator()(const std::map&) const override { + return GetRoadNetworkForPlugin(); + } + + std::map GetDefaultParameters() const override { return {}; } +}; + +REGISTER_ROAD_NETWORK_LOADER_PLUGIN("maliput_ros_road_network_plugin", RoadNetworkLoaderTest); + +} // namespace +} // namespace test +} // namespace ros +} // namespace maliput_ros diff --git a/maliput_ros/test/maliput_ros/ros/test_maliput_plugin.yml b/maliput_ros/test/maliput_ros/ros/test_maliput_plugin.yml new file mode 100644 index 0000000..b0e9441 --- /dev/null +++ b/maliput_ros/test/maliput_ros/ros/test_maliput_plugin.yml @@ -0,0 +1,4 @@ +maliput: + backend: "maliput_ros_road_network_plugin" + parameters: + prop1: "value1" From f320184999c0113f20bbe73f4841ec389183a340 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sat, 7 Jan 2023 09:51:08 +0100 Subject: [PATCH 05/13] Replaces gmock defines for the new versions. Signed-off-by: Agustin Alba Chicar --- .../test/maliput_ros/ros/maliput_mock.h | 115 +++++++++--------- 1 file changed, 57 insertions(+), 58 deletions(-) diff --git a/maliput_ros/test/maliput_ros/ros/maliput_mock.h b/maliput_ros/test/maliput_ros/ros/maliput_mock.h index f3fce92..91a4773 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_mock.h +++ b/maliput_ros/test/maliput_ros/ros/maliput_mock.h @@ -53,79 +53,80 @@ namespace test { /// @brief Google mock maliput::api::RoadGeometry. class RoadGeometryMock final : public maliput::api::RoadGeometry { public: - MOCK_CONST_METHOD0(do_id, maliput::api::RoadGeometryId()); - MOCK_CONST_METHOD0(do_num_junctions, int()); - MOCK_CONST_METHOD1(do_junction, const maliput::api::Junction*(int)); - MOCK_CONST_METHOD0(do_num_branch_points, int()); - MOCK_CONST_METHOD1(do_branch_point, const maliput::api::BranchPoint*(int)); - MOCK_CONST_METHOD0(DoById, const maliput::api::RoadGeometry::IdIndex&()); - MOCK_CONST_METHOD2(DoToRoadPosition, - maliput::api::RoadPositionResult(const maliput::api::InertialPosition&, - const std::optional&)); - MOCK_CONST_METHOD2(DoFindRoadPositions, - std::vector(const maliput::api::InertialPosition&, double)); - MOCK_CONST_METHOD0(do_linear_tolerance, double()); - MOCK_CONST_METHOD0(do_angular_tolerance, double()); - MOCK_CONST_METHOD0(do_scale_length, double()); - MOCK_CONST_METHOD2(DoSampleAheadWaypoints, - std::vector(const maliput::api::LaneSRoute&, double)); - MOCK_CONST_METHOD0(do_inertial_to_backend_frame_translation, maliput::math::Vector3()); + MOCK_METHOD(maliput::api::RoadGeometryId, do_id, (), (const)); + MOCK_METHOD(int, do_num_junctions, (), (const)); + MOCK_METHOD(const maliput::api::Junction*, do_junction, (int), (const)); + MOCK_METHOD(int, do_num_branch_points, (), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, do_branch_point, (int), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry::IdIndex&, DoById, (), (const)); + MOCK_METHOD(maliput::api::RoadPositionResult, DoToRoadPosition, + (const maliput::api::InertialPosition&, const std::optional&), (const)); + MOCK_METHOD(std::vector, DoFindRoadPositions, + (const maliput::api::InertialPosition&, double), (const)); + MOCK_METHOD(double, do_linear_tolerance, (), (const)); + MOCK_METHOD(double, do_angular_tolerance, (), (const)); + MOCK_METHOD(double, do_scale_length, (), (const)); + MOCK_METHOD(std::vector, DoSampleAheadWaypoints, + (const maliput::api::LaneSRoute&, double), (const)); + MOCK_METHOD(maliput::math::Vector3, do_inertial_to_backend_frame_translation, (), (const)); }; /// @brief Google mock maliput::api::rules::RoadRulebook. class RoadRulebookMock final : public maliput::api::rules::RoadRulebook { public: - MOCK_CONST_METHOD2(DoFindRules, - maliput::api::rules::RoadRulebook::QueryResults(const std::vector&, - double)); - MOCK_CONST_METHOD0(DoRules, maliput::api::rules::RoadRulebook::QueryResults()); + MOCK_METHOD(maliput::api::rules::RoadRulebook::QueryResults, DoFindRules, + (const std::vector&, double), (const)); + MOCK_METHOD(maliput::api::rules::RoadRulebook::QueryResults, DoRules, (), (const)); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - MOCK_CONST_METHOD1(DoGetRule, maliput::api::rules::RightOfWayRule(const maliput::api::rules::RightOfWayRule::Id&)); - MOCK_CONST_METHOD1(DoGetRule, maliput::api::rules::SpeedLimitRule(const maliput::api::rules::SpeedLimitRule::Id&)); - MOCK_CONST_METHOD1(DoGetRule, - maliput::api::rules::DirectionUsageRule(const maliput::api::rules::DirectionUsageRule::Id&)); + MOCK_METHOD(maliput::api::rules::RightOfWayRule, DoGetRule, (const maliput::api::rules::RightOfWayRule::Id&), + (const)); + MOCK_METHOD(maliput::api::rules::SpeedLimitRule, DoGetRule, (const maliput::api::rules::SpeedLimitRule::Id&), + (const)); + MOCK_METHOD(maliput::api::rules::DirectionUsageRule, DoGetRule, (const maliput::api::rules::DirectionUsageRule::Id&), + (const)); #pragma GCC diagnostic pop - MOCK_CONST_METHOD1(DoGetDiscreteValueRule, - maliput::api::rules::DiscreteValueRule(const maliput::api::rules::Rule::Id&)); - MOCK_CONST_METHOD1(DoGetRangeValueRule, maliput::api::rules::RangeValueRule(const maliput::api::rules::Rule::Id&)); + MOCK_METHOD(maliput::api::rules::DiscreteValueRule, DoGetDiscreteValueRule, (const maliput::api::rules::Rule::Id&), + (const)); + MOCK_METHOD(maliput::api::rules::RangeValueRule, DoGetRangeValueRule, (const maliput::api::rules::Rule::Id&), + (const)); }; /// @brief Google mock maliput::api::rules::TrafficLightBook. class TrafficLightBookMock final : public maliput::api::rules::TrafficLightBook { public: - MOCK_CONST_METHOD1(DoGetTrafficLight, - const maliput::api::rules::TrafficLight*(const maliput::api::rules::TrafficLight::Id&)); - MOCK_CONST_METHOD0(DoTrafficLights, std::vector()); + MOCK_METHOD(const maliput::api::rules::TrafficLight*, DoGetTrafficLight, + (const maliput::api::rules::TrafficLight::Id&), (const)); + MOCK_METHOD(std::vector, DoTrafficLights, (), (const)); }; /// @brief Google mock maliput::api::IntersectionBook. class IntersectionBookMock final : public maliput::api::IntersectionBook { public: - MOCK_METHOD0(DoGetIntersections, std::vector()); - MOCK_METHOD1(DoGetIntersection, maliput::api::Intersection*(const maliput::api::Intersection::Id&)); - MOCK_METHOD1(DoGetFindIntersection, maliput::api::Intersection*(const maliput::api::rules::TrafficLight::Id&)); - MOCK_METHOD1(DoGetFindIntersection, maliput::api::Intersection*(const maliput::api::rules::DiscreteValueRule::Id&)); - MOCK_METHOD1(DoGetFindIntersection, maliput::api::Intersection*(const maliput::api::InertialPosition&)); + MOCK_METHOD(std::vector, DoGetIntersections, ()); + MOCK_METHOD(maliput::api::Intersection*, DoGetIntersection, (const maliput::api::Intersection::Id&)); + MOCK_METHOD(maliput::api::Intersection*, DoGetFindIntersection, (const maliput::api::rules::TrafficLight::Id&)); + MOCK_METHOD(maliput::api::Intersection*, DoGetFindIntersection, (const maliput::api::rules::DiscreteValueRule::Id&)); + MOCK_METHOD(maliput::api::Intersection*, DoGetFindIntersection, (const maliput::api::InertialPosition&)); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - MOCK_METHOD1(DoGetFindIntersection, maliput::api::Intersection*(const maliput::api::rules::RightOfWayRule::Id&)); + MOCK_METHOD(maliput::api::Intersection*, DoGetFindIntersection, (const maliput::api::rules::RightOfWayRule::Id&)); #pragma GCC diagnostic pop }; /// @brief Google mock maliput::api::rules::PhaseRingBook. class PhaseRingBookMock : public maliput::api::rules::PhaseRingBook { public: - MOCK_CONST_METHOD0(DoGetPhaseRings, std::vector()); - MOCK_CONST_METHOD1(DoGetPhaseRing, - std::optional(const maliput::api::rules::PhaseRing::Id&)); + MOCK_METHOD(std::vector, DoGetPhaseRings, (), (const)); + MOCK_METHOD(std::optional, DoGetPhaseRing, + (const maliput::api::rules::PhaseRing::Id&), (const)); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - MOCK_CONST_METHOD1(DoFindPhaseRing, - std::optional(const maliput::api::rules::RightOfWayRule::Id&)); + MOCK_METHOD(std::optional, DoFindPhaseRing, + (const maliput::api::rules::RightOfWayRule::Id&), (const)); #pragma GCC diagnostic pop - MOCK_CONST_METHOD1(DoFindPhaseRing, - std::optional(const maliput::api::rules::Rule::Id&)); + MOCK_METHOD(std::optional, DoFindPhaseRing, (const maliput::api::rules::Rule::Id&), + (const)); }; #pragma GCC diagnostic push @@ -133,36 +134,34 @@ class PhaseRingBookMock : public maliput::api::rules::PhaseRingBook { /// @brief Google mock maliput::api::rules::RightOfWayRuleStateProvider. class RightOfWayRuleStateProviderMock final : public maliput::api::rules::RightOfWayRuleStateProvider { public: - MOCK_CONST_METHOD1(DoGetState, std::optional( - const maliput::api::rules::RightOfWayRule::Id&)); + MOCK_METHOD(std::optional, DoGetState, + (const maliput::api::rules::RightOfWayRule::Id&), (const)); }; #pragma GCC diagnostic pop /// @brief Google mock maliput::api::rules::PhaseProvider. class PhaseProviderMock final : public maliput::api::rules::PhaseProvider { public: - MOCK_CONST_METHOD1( - DoGetPhase, std::optional(const maliput::api::rules::PhaseRing::Id&)); + MOCK_METHOD(std::optional, DoGetPhase, + (const maliput::api::rules::PhaseRing::Id&), (const)); }; /// @brief Google mock maliput::api::rules::DiscreteValueRuleStateProvider. class DiscreteValueRuleStateProviderMock final : public maliput::api::rules::DiscreteValueRuleStateProvider { public: - MOCK_CONST_METHOD1(DoGetState, std::optional( - const maliput::api::rules::Rule::Id&)); - MOCK_CONST_METHOD3(DoGetState, - std::optional( - const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double)); + MOCK_METHOD(std::optional, DoGetState, + (const maliput::api::rules::Rule::Id&), (const)); + MOCK_METHOD(std::optional, DoGetState, + (const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double), (const)); }; /// @brief Google mock maliput::api::rules::RangeValueRuleStateProvider. class RangeValueRuleStateProviderMock final : public maliput::api::rules::RangeValueRuleStateProvider { public: - MOCK_CONST_METHOD1(DoGetState, std::optional( - const maliput::api::rules::Rule::Id&)); - MOCK_CONST_METHOD3(DoGetState, - std::optional( - const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double)); + MOCK_METHOD(std::optional, DoGetState, + (const maliput::api::rules::Rule::Id&), (const)); + MOCK_METHOD(std::optional, DoGetState, + (const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double), (const)); }; } // namespace test From e4cfd6a84662b5dc38259cb375daee3e3132e8bf Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sat, 7 Jan 2023 09:55:48 +0100 Subject: [PATCH 06/13] Missing end of line. Signed-off-by: Agustin Alba Chicar --- maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h b/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h index 3be4572..92e970b 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h +++ b/maliput_ros/test/maliput_ros/ros/maliput_plugin_config_test.h @@ -64,4 +64,4 @@ std::unique_ptr GetRoadNetworkForPlugin(); } // namespace test } // namespace ros -} // namespace maliput_ros \ No newline at end of file +} // namespace maliput_ros From 6dd66cd000edd0ebd3b1ead3de98973f78370de3 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sat, 7 Jan 2023 10:17:45 +0100 Subject: [PATCH 07/13] Solves the non-mutable YAML configuration parameter. Signed-off-by: Agustin Alba Chicar --- .../src/maliput_ros/ros/maliput_query_node.cc | 4 +- .../ros/maliput_query_node_test.cc | 37 +++++++------------ 2 files changed, 14 insertions(+), 27 deletions(-) diff --git a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc index d07af66..2de3b45 100644 --- a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc +++ b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc @@ -50,9 +50,7 @@ MaliputQueryNode::MaliputQueryNode(const std::string& node_name, const std::stri rcl_interfaces::msg::ParameterDescriptor maliput_plugin_yaml_file_path_descriptor; maliput_plugin_yaml_file_path_descriptor.name = kYamlConfigurationPath; maliput_plugin_yaml_file_path_descriptor.description = kYamlConfigurationPathDescription; - // TODO(agalbachicar): this has been enabled for testing. We should pass in the proper context - // to the node to make it read only. - maliput_plugin_yaml_file_path_descriptor.read_only = false; + maliput_plugin_yaml_file_path_descriptor.read_only = true; this->declare_parameter(maliput_plugin_yaml_file_path_descriptor.name, rclcpp::ParameterValue(std::string{}), maliput_plugin_yaml_file_path_descriptor); } diff --git a/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc index 283139c..6f9a533 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc +++ b/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc @@ -142,36 +142,26 @@ TEST_F(MaliputQueryNodeTest, DefineYamlConfigurationPathParameter) { } // Makes sure the "yaml_configuration_path" parameter can be set. -// TODO(agalbachicar): this has been enabled for testing. We should pass in the proper context -// to the node to make it read only. TEST_F(MaliputQueryNodeTest, CanSetYamlConfigurationPathParameter) { - const rclcpp::Parameter yaml_configuration_path_parameter{kYamlConfigurationPathParameterName, - rclcpp::ParameterValue("a_path")}; - // Callback for parameter setting. - bool parameter_was_set{false}; - auto callback_parameter_set = [¶meter_was_set](const std::vector& parameters) { - parameter_was_set = true; - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - return result; - }; + static constexpr const char* kParameterValue = "a_path"; + const rclcpp::NodeOptions kNodeOptions = rclcpp::NodeOptions().append_parameter_override( + kYamlConfigurationPathParameterName, rclcpp::ParameterValue(kParameterValue)); - auto dut = std::make_shared(kNodeName, kNodeNamespace); - auto unused_callback_handle = dut->add_on_set_parameters_callback(callback_parameter_set); + auto dut = std::make_shared(kNodeName, kNodeNamespace, kNodeOptions); - ASSERT_TRUE(dut->set_parameter(yaml_configuration_path_parameter).successful); - ASSERT_TRUE(parameter_was_set); + ASSERT_EQ(kParameterValue, dut->get_parameter(kYamlConfigurationPathParameterName).as_string()); } // Evaluates that the transition to the configure state tries to load the YAML configuration. // Upon a bad file, this transition fails. TEST_F(MaliputQueryNodeTest, WrongYamlConfigurationPathMakesTheConfigurationStageToFail) { - const rclcpp::Parameter yaml_configuration_path_parameter{kYamlConfigurationPathParameterName, - rclcpp::ParameterValue("wrong_file_path")}; + static constexpr const char* kParameterValue = "wrong_file_path"; + const rclcpp::NodeOptions kNodeOptions = rclcpp::NodeOptions().append_parameter_override( + kYamlConfigurationPathParameterName, rclcpp::ParameterValue(kParameterValue)); + auto ret = kCallbackFailure; - auto dut = std::make_shared(kNodeName, kNodeNamespace); - ASSERT_TRUE(dut->set_parameter(yaml_configuration_path_parameter).successful); + auto dut = std::make_shared(kNodeName, kNodeNamespace, kNodeOptions); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, dut->get_current_state().id()); dut->trigger_transition(rclcpp_lifecycle::Transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE), ret); ASSERT_EQ(kCallbackFailure, ret); @@ -205,8 +195,7 @@ class MaliputQueryNodeAfterConfigurationTest : public MaliputQueryNodeTest { ASSERT_TRUE(setenv(kEnvName, kRoadNetowrkMockPluginPath, kReplace) == 0); // Load the YAML configuration file for the node. - dut_ = std::make_shared(kNodeName, kNodeNamespace); - ASSERT_TRUE(dut_->set_parameter(kYamlConfigurationPathParameter).successful); + dut_ = std::make_shared(kNodeName, kNodeNamespace, kNodeOptions); } void TearDown() override { @@ -235,8 +224,8 @@ class MaliputQueryNodeAfterConfigurationTest : public MaliputQueryNodeTest { std::shared_ptr dut_{}; private: - const rclcpp::Parameter kYamlConfigurationPathParameter{kYamlConfigurationPathParameterName, - rclcpp::ParameterValue(kYamlFilePath)}; + const rclcpp::NodeOptions kNodeOptions = rclcpp::NodeOptions().append_parameter_override( + kYamlConfigurationPathParameterName, rclcpp::ParameterValue(kYamlFilePath)); std::string back_up_env_{}; }; From bf1e6edb646add827633f9a9a571b224b42dfd44 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sat, 7 Jan 2023 10:28:45 +0100 Subject: [PATCH 08/13] Adds dependencies to CI. Signed-off-by: Agustin Alba Chicar --- .github/dependencies.repos | 9 +++++++++ .github/workflows/build.yml | 1 + 2 files changed, 10 insertions(+) create mode 100644 .github/dependencies.repos diff --git a/.github/dependencies.repos b/.github/dependencies.repos new file mode 100644 index 0000000..5eff6f7 --- /dev/null +++ b/.github/dependencies.repos @@ -0,0 +1,9 @@ +repositories: + ament_cmake_doxygen: + type: git + url: https://github.com/ToyotaResearchInstitute/ament_cmake_doxygen + version: main + maliput: + type: git + url: https://github.com/maliput/maliput + version: main diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ab71f0f..3df4032 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -25,3 +25,4 @@ jobs: with: package-name: ${{ env.PACKAGE_NAMES }} target-ros2-distro: ${{ env.ROS_DISTRO }} + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/dependencies.repos From 663fec723a74cfab3de0c2e1f47a4bcb1b89257a Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sat, 7 Jan 2023 15:08:38 +0100 Subject: [PATCH 09/13] Adds new mocks and standardizes the macros. Signed-off-by: Agustin Alba Chicar --- .../test/maliput_ros/ros/maliput_mock.h | 61 ++++++++ .../maliput_ros_translation/convert_test.cc | 134 ++++++++++-------- 2 files changed, 139 insertions(+), 56 deletions(-) diff --git a/maliput_ros/test/maliput_ros/ros/maliput_mock.h b/maliput_ros/test/maliput_ros/ros/maliput_mock.h index 91a4773..3209142 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_mock.h +++ b/maliput_ros/test/maliput_ros/ros/maliput_mock.h @@ -164,6 +164,67 @@ class RangeValueRuleStateProviderMock final : public maliput::api::rules::RangeV (const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double), (const)); }; +/// @brief Google mock maliput::api::BranchPoint. +class BranchPointMock final : public maliput::api::BranchPoint { + public: + MOCK_METHOD(maliput::api::BranchPointId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry*, do_road_geometry, (), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetConfluentBranches, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetOngoingBranches, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(std::optional, DoGetDefaultBranch, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetASide, (), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetBSide, (), (const)); +}; + +/// @brief Google mock maliput::api::Lane. +class LaneMock final : public maliput::api::Lane { + public: + MOCK_METHOD(maliput::api::LaneId, do_id, (), (const)); + MOCK_METHOD(int, do_index, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, do_segment, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_to_left, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_to_right, (), (const)); + MOCK_METHOD(double, do_length, (), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, DoGetBranchPoint, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(std::optional, DoGetDefaultBranch, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(maliput::api::RBounds, do_lane_bounds, (double), (const)); + MOCK_METHOD(maliput::api::RBounds, do_segment_bounds, (double), (const)); + MOCK_METHOD(maliput::api::HBounds, do_elevation_bounds, (double, double), (const)); + MOCK_METHOD(maliput::api::InertialPosition, DoToInertialPosition, (const maliput::api::LanePosition&), (const)); + MOCK_METHOD(maliput::api::Rotation, DoGetOrientation, (const maliput::api::LanePosition&), (const)); + MOCK_METHOD(maliput::api::LanePosition, DoEvalMotionDerivatives, + (const maliput::api::LanePosition&, const maliput::api::IsoLaneVelocity&), (const)); + MOCK_METHOD(maliput::api::LanePositionResult, DoToLanePosition, (const maliput::api::InertialPosition&), (const)); + MOCK_METHOD(maliput::api::LanePositionResult, DoToSegmentPosition, (const maliput::api::InertialPosition&), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetConfluentBranches, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetOngoingBranches, (const maliput::api::LaneEnd::Which), (const)); +}; + +/// @brief Google mock maliput::api::LaneEndSet. +class LaneEndSetMock final : public maliput::api::LaneEndSet { + public: + MOCK_METHOD(int, do_size, (), (const)); + MOCK_METHOD(const maliput::api::LaneEnd&, do_get, (int), (const)); +}; + +/// @brief Google mock maliput::api::SegmentMock. +class SegmentMock final : public maliput::api::Segment { + public: + MOCK_METHOD(maliput::api::SegmentId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::Junction*, do_junction, (), (const)); + MOCK_METHOD(int, do_num_lanes, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_lane, (int), (const)); +}; + +/// @brief Google mock maliput::api::JunctionMock. +class JunctionMock final : public maliput::api::Junction { + public: + MOCK_METHOD(maliput::api::JunctionId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry*, do_road_geometry, (), (const)); + MOCK_METHOD(int, do_num_segments, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, do_segment, (int), (const)); +}; + } // namespace test } // namespace ros } // namespace maliput_ros diff --git a/maliput_ros_translation/test/maliput_ros_translation/convert_test.cc b/maliput_ros_translation/test/maliput_ros_translation/convert_test.cc index 594d52c..405c70f 100644 --- a/maliput_ros_translation/test/maliput_ros_translation/convert_test.cc +++ b/maliput_ros_translation/test/maliput_ros_translation/convert_test.cc @@ -45,64 +45,86 @@ namespace { using ::testing::Return; using ::testing::ReturnRef; -class RoadGeometryMock final : public maliput::api::test::MockRoadGeometry { +/// @brief Google mock maliput::api::RoadGeometry. +class RoadGeometryMock final : public maliput::api::RoadGeometry { public: - explicit RoadGeometryMock(const maliput::api::RoadGeometryId& id) : maliput::api::test::MockRoadGeometry(id) {} - MOCK_CONST_METHOD0(do_id, maliput::api::RoadGeometryId()); - MOCK_CONST_METHOD0(do_num_junctions, int()); - MOCK_CONST_METHOD1(do_junction, const maliput::api::Junction*(int)); - MOCK_CONST_METHOD0(do_num_branch_points, int()); - MOCK_CONST_METHOD1(do_branch_point, const maliput::api::BranchPoint*(int)); - MOCK_CONST_METHOD0(do_linear_tolerance, double()); - MOCK_CONST_METHOD0(do_angular_tolerance, double()); - MOCK_CONST_METHOD0(do_scale_length, double()); - MOCK_CONST_METHOD0(do_inertial_to_backend_frame_translation, maliput::math::Vector3()); + MOCK_METHOD(maliput::api::RoadGeometryId, do_id, (), (const)); + MOCK_METHOD(int, do_num_junctions, (), (const)); + MOCK_METHOD(const maliput::api::Junction*, do_junction, (int), (const)); + MOCK_METHOD(int, do_num_branch_points, (), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, do_branch_point, (int), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry::IdIndex&, DoById, (), (const)); + MOCK_METHOD(maliput::api::RoadPositionResult, DoToRoadPosition, + (const maliput::api::InertialPosition&, const std::optional&), (const)); + MOCK_METHOD(std::vector, DoFindRoadPositions, + (const maliput::api::InertialPosition&, double), (const)); + MOCK_METHOD(double, do_linear_tolerance, (), (const)); + MOCK_METHOD(double, do_angular_tolerance, (), (const)); + MOCK_METHOD(double, do_scale_length, (), (const)); + MOCK_METHOD(std::vector, DoSampleAheadWaypoints, + (const maliput::api::LaneSRoute&, double), (const)); + MOCK_METHOD(maliput::math::Vector3, do_inertial_to_backend_frame_translation, (), (const)); }; +/// @brief Google mock maliput::api::BranchPoint. class BranchPointMock final : public maliput::api::BranchPoint { public: - MOCK_CONST_METHOD0(do_id, maliput::api::BranchPointId()); - MOCK_CONST_METHOD0(do_road_geometry, const maliput::api::RoadGeometry*()); - MOCK_CONST_METHOD1(DoGetConfluentBranches, const maliput::api::LaneEndSet*(const maliput::api::LaneEnd& end)); - MOCK_CONST_METHOD1(DoGetOngoingBranches, const maliput::api::LaneEndSet*(const maliput::api::LaneEnd& end)); - MOCK_CONST_METHOD1(DoGetDefaultBranch, std::optional(const maliput::api::LaneEnd& end)); - MOCK_CONST_METHOD0(DoGetASide, const maliput::api::LaneEndSet*()); - MOCK_CONST_METHOD0(DoGetBSide, const maliput::api::LaneEndSet*()); + MOCK_METHOD(maliput::api::BranchPointId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry*, do_road_geometry, (), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetConfluentBranches, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetOngoingBranches, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(std::optional, DoGetDefaultBranch, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetASide, (), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetBSide, (), (const)); }; -class LaneMock final : public maliput::geometry_base::test::MockLane { +/// @brief Google mock maliput::api::Lane. +class LaneMock final : public maliput::api::Lane { public: - explicit LaneMock(const maliput::api::LaneId& id) : MockLane(id) {} - MOCK_CONST_METHOD0(do_id, maliput::api::LaneId()); - MOCK_CONST_METHOD0(do_index, int()); - MOCK_CONST_METHOD0(do_segment, const maliput::api::Segment*()); - MOCK_CONST_METHOD0(do_to_left, const maliput::api::Lane*()); - MOCK_CONST_METHOD0(do_to_right, const maliput::api::Lane*()); - MOCK_CONST_METHOD0(do_length, double()); - MOCK_CONST_METHOD1(DoGetBranchPoint, const maliput::api::BranchPoint*(const maliput::api::LaneEnd::Which)); - MOCK_CONST_METHOD1(DoGetDefaultBranch, std::optional(const maliput::api::LaneEnd::Which)); + MOCK_METHOD(maliput::api::LaneId, do_id, (), (const)); + MOCK_METHOD(int, do_index, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, do_segment, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_to_left, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_to_right, (), (const)); + MOCK_METHOD(double, do_length, (), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, DoGetBranchPoint, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(std::optional, DoGetDefaultBranch, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(maliput::api::RBounds, do_lane_bounds, (double), (const)); + MOCK_METHOD(maliput::api::RBounds, do_segment_bounds, (double), (const)); + MOCK_METHOD(maliput::api::HBounds, do_elevation_bounds, (double, double), (const)); + MOCK_METHOD(maliput::api::InertialPosition, DoToInertialPosition, (const maliput::api::LanePosition&), (const)); + MOCK_METHOD(maliput::api::Rotation, DoGetOrientation, (const maliput::api::LanePosition&), (const)); + MOCK_METHOD(maliput::api::LanePosition, DoEvalMotionDerivatives, + (const maliput::api::LanePosition&, const maliput::api::IsoLaneVelocity&), (const)); + MOCK_METHOD(maliput::api::LanePositionResult, DoToLanePosition, (const maliput::api::InertialPosition&), (const)); + MOCK_METHOD(maliput::api::LanePositionResult, DoToSegmentPosition, (const maliput::api::InertialPosition&), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetConfluentBranches, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetOngoingBranches, (const maliput::api::LaneEnd::Which), (const)); }; +/// @brief Google mock maliput::api::LaneEndSet. class LaneEndSetMock final : public maliput::api::LaneEndSet { public: - MOCK_CONST_METHOD0(do_size, int()); - MOCK_CONST_METHOD1(do_get, const maliput::api::LaneEnd&(int)); + MOCK_METHOD(int, do_size, (), (const)); + MOCK_METHOD(const maliput::api::LaneEnd&, do_get, (int), (const)); }; +/// @brief Google mock maliput::api::SegmentMock. class SegmentMock final : public maliput::api::Segment { public: - MOCK_CONST_METHOD0(do_id, maliput::api::SegmentId()); - MOCK_CONST_METHOD0(do_junction, const maliput::api::Junction*()); - MOCK_CONST_METHOD0(do_num_lanes, int()); - MOCK_CONST_METHOD1(do_lane, const maliput::api::Lane*(int)); + MOCK_METHOD(maliput::api::SegmentId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::Junction*, do_junction, (), (const)); + MOCK_METHOD(int, do_num_lanes, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_lane, (int), (const)); }; +/// @brief Google mock maliput::api::JunctionMock. class JunctionMock final : public maliput::api::Junction { public: - MOCK_CONST_METHOD0(do_id, maliput::api::JunctionId()); - MOCK_CONST_METHOD0(do_road_geometry, const maliput::api::RoadGeometry*()); - MOCK_CONST_METHOD0(do_num_segments, int()); - MOCK_CONST_METHOD1(do_segment, const maliput::api::Segment*(int)); + MOCK_METHOD(maliput::api::JunctionId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry*, do_road_geometry, (), (const)); + MOCK_METHOD(int, do_num_segments, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, do_segment, (int), (const)); }; class IdsToRosMessageTest : public ::testing::Test { @@ -146,7 +168,7 @@ class LaneEndToRosMessageTest : public ::testing::Test { }; TEST_F(LaneEndToRosMessageTest, LaneEndAtStart) { - LaneMock lane(kLaneId); + LaneMock lane; EXPECT_CALL(lane, do_id()).WillRepeatedly(Return(kLaneId)); const maliput::api::LaneEnd lane_end(&lane, maliput::api::LaneEnd::Which::kStart); @@ -157,7 +179,7 @@ TEST_F(LaneEndToRosMessageTest, LaneEndAtStart) { } TEST_F(LaneEndToRosMessageTest, LaneEndAtFinish) { - LaneMock lane(kLaneId); + LaneMock lane; EXPECT_CALL(lane, do_id()).WillRepeatedly(Return(kLaneId)); const maliput::api::LaneEnd lane_end(&lane, maliput::api::LaneEnd::Which::kFinish); @@ -179,9 +201,9 @@ GTEST_TEST(LaneEndSetToRosMessageTest, ValidLaneEndSet) { const maliput::api::LaneId kLaneIdB{"lane_id_b"}; const maliput::api::LaneEnd::Which kWhichA{maliput::api::LaneEnd::Which::kStart}; const maliput::api::LaneEnd::Which kWhichB{maliput::api::LaneEnd::Which::kFinish}; - LaneMock lane_a(kLaneIdA); + LaneMock lane_a; EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA)); - LaneMock lane_b(kLaneIdB); + LaneMock lane_b; EXPECT_CALL(lane_b, do_id()).WillRepeatedly(Return(kLaneIdB)); const maliput::api::LaneEnd lane_end_a(&lane_a, kWhichA); const maliput::api::LaneEnd lane_end_b(&lane_b, kWhichB); @@ -218,9 +240,9 @@ class BranchPointToRosMessageTest : public ::testing::Test { }; TEST_F(BranchPointToRosMessageTest, ValidBranchPoint) { - LaneMock lane_a(kLaneIdA); + LaneMock lane_a; EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA)); - LaneMock lane_b(kLaneIdB); + LaneMock lane_b; EXPECT_CALL(lane_b, do_id()).WillRepeatedly(Return(kLaneIdB)); const maliput::api::LaneEnd lane_end_a(&lane_a, kWhichA); const maliput::api::LaneEnd lane_end_b(&lane_b, kWhichB); @@ -230,7 +252,7 @@ TEST_F(BranchPointToRosMessageTest, ValidBranchPoint) { LaneEndSetMock lane_end_set_b; EXPECT_CALL(lane_end_set_b, do_size()).WillRepeatedly(Return(1)); EXPECT_CALL(lane_end_set_b, do_get(0)).WillRepeatedly(ReturnRef(lane_end_b)); - RoadGeometryMock road_geometry(kRoadGeometryId); + RoadGeometryMock road_geometry; EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId)); BranchPointMock branch_point; EXPECT_CALL(branch_point, do_id()).WillRepeatedly(Return(kBranchPointId)); @@ -251,13 +273,13 @@ TEST_F(BranchPointToRosMessageTest, ValidBranchPoint) { } TEST_F(BranchPointToRosMessageTest, BranchPointWithEmptyBSide) { - LaneMock lane_a(kLaneIdA); + LaneMock lane_a; EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA)); const maliput::api::LaneEnd lane_end_a(&lane_a, kWhichA); LaneEndSetMock lane_end_set_a; EXPECT_CALL(lane_end_set_a, do_size()).WillRepeatedly(Return(1)); EXPECT_CALL(lane_end_set_a, do_get(0)).WillRepeatedly(ReturnRef(lane_end_a)); - RoadGeometryMock road_geometry(kRoadGeometryId); + RoadGeometryMock road_geometry; EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId)); BranchPointMock branch_point; EXPECT_CALL(branch_point, do_id()).WillRepeatedly(Return(kBranchPointId)); @@ -288,9 +310,9 @@ GTEST_TEST(SegmentToRosMessageTest, ValidSegment) { const maliput::api::LaneId kLaneIdB{"lane_id_b"}; const maliput::api::JunctionId kJunctionId{"junction_id"}; const maliput::api::SegmentId kSegmentId{"segment_id"}; - LaneMock lane_a(kLaneIdA); + LaneMock lane_a; EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA)); - LaneMock lane_b(kLaneIdB); + LaneMock lane_b; EXPECT_CALL(lane_b, do_id()).WillRepeatedly(Return(kLaneIdB)); JunctionMock junction; EXPECT_CALL(junction, do_id()).WillRepeatedly(Return(kJunctionId)); @@ -328,7 +350,7 @@ GTEST_TEST(JunctionToRosMessageTest, ValidJunction) { EXPECT_CALL(segment_a, do_id()).WillRepeatedly(Return(kSgmentIdA)); SegmentMock segment_b; EXPECT_CALL(segment_b, do_id()).WillRepeatedly(Return(kSgmentIdB)); - RoadGeometryMock road_geometry(kRoadGeometryId); + RoadGeometryMock road_geometry; EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId)); JunctionMock junction; EXPECT_CALL(junction, do_id()).WillRepeatedly(Return(kJunctionId)); @@ -374,7 +396,7 @@ GTEST_TEST(RoadGeometryToRosMessageTest, ValidRoadGeometry) { EXPECT_CALL(branch_point_a, do_id()).WillRepeatedly(Return(kBranchPointIdA)); BranchPointMock branch_point_b; EXPECT_CALL(branch_point_b, do_id()).WillRepeatedly(Return(kBranchPointIdB)); - RoadGeometryMock road_geometry(kRoadGeometryId); + RoadGeometryMock road_geometry; EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId)); EXPECT_CALL(road_geometry, do_linear_tolerance()).WillRepeatedly(Return(kLinearTolerance)); EXPECT_CALL(road_geometry, do_angular_tolerance()).WillRepeatedly(Return(kAngularTolerance)); @@ -451,13 +473,13 @@ class LaneToRosMessageTest : public ::testing::Test { } SegmentMock segment; - LaneMock left_lane{kLeftLaneId}; - LaneMock right_lane{kRightLaneId}; - LaneMock default_start_lane{kDefaultStartLaneId}; - LaneMock default_finish_lane{kDefaultFinishLaneId}; + LaneMock left_lane; + LaneMock right_lane; + LaneMock default_start_lane; + LaneMock default_finish_lane; BranchPointMock start_branch_point; BranchPointMock finish_branch_point; - LaneMock lane{kLaneId}; + LaneMock lane; }; TEST_F(LaneToRosMessageTest, FullLane) { From e38289e6687be435f642e4d562384a2ba375f230 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sat, 7 Jan 2023 18:53:16 +0100 Subject: [PATCH 10/13] Adds test to MaliputQuery. Signed-off-by: Agustin Alba Chicar --- .../include/maliput_ros/ros/maliput_query.h | 8 ++++++++ .../test/maliput_ros/ros/maliput_mock.h | 19 +++++++++++++++++-- .../maliput_ros/ros/maliput_query_test.cc | 14 ++++++++++++++ 3 files changed, 39 insertions(+), 2 deletions(-) diff --git a/maliput_ros/include/maliput_ros/ros/maliput_query.h b/maliput_ros/include/maliput_ros/ros/maliput_query.h index 8a45b24..9fd7d4b 100644 --- a/maliput_ros/include/maliput_ros/ros/maliput_query.h +++ b/maliput_ros/include/maliput_ros/ros/maliput_query.h @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,13 @@ class MaliputQuery final { /// @return The maliput::api::RoadGeometry. inline const maliput::api::RoadGeometry* road_geometry() const { return road_network_->road_geometry(); } + /// Finds a maliput::api::Junction by its ID. + /// @param[in] id The maliput::api::JunctionId. + /// @return A maliput::api::Junction when @p id refers to a valid maliput::api::Junction. Otherwise, nullptr.. + inline const maliput::api::Junction* GetJunctionBy(const maliput::api::JunctionId& id) const { + return road_network_->road_geometry()->ById().GetJunction(id); + } + private: std::unique_ptr road_network_{}; }; diff --git a/maliput_ros/test/maliput_ros/ros/maliput_mock.h b/maliput_ros/test/maliput_ros/ros/maliput_mock.h index 3209142..7dd79f1 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_mock.h +++ b/maliput_ros/test/maliput_ros/ros/maliput_mock.h @@ -29,10 +29,14 @@ #pragma once #include +#include #include #include +#include #include +#include +#include #include #include #include @@ -45,6 +49,7 @@ #include #include #include +#include namespace maliput_ros { namespace ros { @@ -207,7 +212,7 @@ class LaneEndSetMock final : public maliput::api::LaneEndSet { MOCK_METHOD(const maliput::api::LaneEnd&, do_get, (int), (const)); }; -/// @brief Google mock maliput::api::SegmentMock. +/// @brief Google mock maliput::api::Segment. class SegmentMock final : public maliput::api::Segment { public: MOCK_METHOD(maliput::api::SegmentId, do_id, (), (const)); @@ -216,7 +221,7 @@ class SegmentMock final : public maliput::api::Segment { MOCK_METHOD(const maliput::api::Lane*, do_lane, (int), (const)); }; -/// @brief Google mock maliput::api::JunctionMock. +/// @brief Google mock maliput::api::Junction. class JunctionMock final : public maliput::api::Junction { public: MOCK_METHOD(maliput::api::JunctionId, do_id, (), (const)); @@ -225,6 +230,16 @@ class JunctionMock final : public maliput::api::Junction { MOCK_METHOD(const maliput::api::Segment*, do_segment, (int), (const)); }; +/// @brief Google mock maliput::api::RoadGeometry::IdIndex. +class IdIndexMock final : public maliput::api::RoadGeometry::IdIndex { + public: + MOCK_METHOD(const maliput::api::Lane*, DoGetLane, (const maliput::api::LaneId&), (const)); + MOCK_METHOD((const std::unordered_map&), DoGetLanes, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, DoGetSegment, (const maliput::api::SegmentId&), (const)); + MOCK_METHOD(const maliput::api::Junction*, DoGetJunction, (const maliput::api::JunctionId&), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, DoGetBranchPoint, (const maliput::api::BranchPointId&), (const)); +}; + } // namespace test } // namespace ros } // namespace maliput_ros diff --git a/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc index 243ad54..a9420f0 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc +++ b/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc @@ -43,6 +43,9 @@ namespace ros { namespace test { namespace { +using ::testing::Return; +using ::testing::ReturnRef; + // Test class for MaliputQuery. class MaliputQueryTest : public ::testing::Test { public: @@ -85,6 +88,17 @@ TEST_F(MaliputQueryTest, RoadGeometry) { ASSERT_EQ(dut_->road_geometry(), static_cast(road_geometry_ptr_)); } +// Validates MaliputQuery redirects the query via the IdIndex. +TEST_F(MaliputQueryTest, GetJunctionById) { + const maliput::api::JunctionId kJunctionId{"junction_id"}; + const JunctionMock junction; + IdIndexMock id_index; + EXPECT_CALL(id_index, DoGetJunction(kJunctionId)).WillRepeatedly(Return(&junction)); + EXPECT_CALL(*road_geometry_ptr_, DoById()).WillRepeatedly(ReturnRef(id_index)); + + ASSERT_EQ(&junction, dut_->GetJunctionBy(kJunctionId)); +} + } // namespace } // namespace test } // namespace ros From b1331e1bfc2e8095e6c3e24e26b942207e042008 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sat, 7 Jan 2023 19:21:12 +0100 Subject: [PATCH 11/13] Adds the junction service call and its tests. Signed-off-by: Agustin Alba Chicar --- .../maliput_ros/ros/maliput_query_node.h | 11 ++ .../src/maliput_ros/ros/maliput_query_node.cc | 18 +++ .../ros/maliput_query_node_test.cc | 123 ++++++++++++++++-- 3 files changed, 142 insertions(+), 10 deletions(-) diff --git a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h index c347b9d..3348c63 100644 --- a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h +++ b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h @@ -34,6 +34,7 @@ #include #include +#include #include #include #include @@ -81,6 +82,7 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: + static constexpr const char* kJunctionServiceName = "junction"; static constexpr const char* kRoadGeometryServiceName = "road_geometry"; static constexpr const char* kYamlConfigurationPath = "yaml_configuration_path"; static constexpr const char* kYamlConfigurationPathDescription = @@ -89,6 +91,13 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { // @return The path to the YAMl file containing the maliput plugin configuration from the node parameter. std::string GetMaliputYamlFilePath() const; + // @brief Responds the maliput::api::Junction configuration. + // @pre The node must be in the ACTIVE state. + // @param[in] request Holds the maliput::api::JunctionId to query. + // @param[out] response Loads the maliput::api::Junction description. + void JunctionCallback(const std::shared_ptr request, + std::shared_ptr response) const; + // @brief Responds the maliput::api::RoadGeometry configuration. // @pre The node must be in the ACTIVE state. // @param[in] request Unused. @@ -142,6 +151,8 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { // Works as a barrier to all service callbacks. When it is true, callbacks can operate. std::atomic is_active_; + // /junction service. + rclcpp::Service::SharedPtr junction_srv_; // /road_geometry service. rclcpp::Service::SharedPtr road_geometry_srv_; // Proxy to a maliput::api::RoadNetwork queries. diff --git a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc index 2de3b45..4663e6f 100644 --- a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc +++ b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -66,6 +67,22 @@ void MaliputQueryNode::RoadGeometryCallback( response->road_geometry = maliput_ros_translation::ToRosMessage(maliput_query_->road_geometry()); } +void MaliputQueryNode::JunctionCallback( + const std::shared_ptr request, + std::shared_ptr response) const { + RCLCPP_INFO(get_logger(), "JunctionCallback"); + if (!is_active_.load()) { + RCLCPP_WARN(get_logger(), "The node is not active yet."); + return; + } + if (request->id.id.empty()) { + RCLCPP_ERROR(get_logger(), "Request /junction with invalid value for JunctionId."); + return; + } + response->junction = maliput_ros_translation::ToRosMessage( + maliput_query_->GetJunctionBy(maliput_ros_translation::FromRosMessage(request->id))); +} + std::string MaliputQueryNode::GetMaliputYamlFilePath() const { return this->get_parameter(kYamlConfigurationPath).get_parameter_value().get(); } @@ -93,6 +110,7 @@ void MaliputQueryNode::TearDownMaliputQuery() { bool MaliputQueryNode::InitializeAllServices() { RCLCPP_INFO(get_logger(), "InitializeAllServices"); + road_geometry_srv_ = this->create_service( kRoadGeometryServiceName, std::bind(&MaliputQueryNode::RoadGeometryCallback, this, std::placeholders::_1, std::placeholders::_2)); diff --git a/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc index 6f9a533..e4b49ca 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc +++ b/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ #include "maliput_ros/ros/maliput_mock.h" #include "maliput_ros/ros/maliput_plugin_config_test.h" +#include "maliput_ros_translation/convert.h" namespace maliput_ros { namespace ros { @@ -57,6 +59,7 @@ namespace test { namespace { using ::testing::Return; +using ::testing::ReturnRef; // @return A maliput::api::RoadNetwork populated with gmock versions of it, except maliput::api::rules::RuleRegistry. std::unique_ptr MakeRoadNetworkMock() { @@ -177,6 +180,9 @@ class MaliputQueryNodeAfterConfigurationTest : public MaliputQueryNodeTest { static constexpr const char* kRoadNetowrkMockPluginPath = TEST_MALIPUT_PLUGIN_LIBDIR; static constexpr const char* kRoadGeometryServiceName = "/my_namespace/road_geometry"; static constexpr const char* kRoadGeometryServiceType = "maliput_ros_interfaces/srv/RoadGeometry"; + static constexpr const char* kJunctionServiceName = "/my_namespace/junction"; + static constexpr const char* kJunctionServiceType = "maliput_ros_interfaces/srv/Junction"; + const std::string kYamlFilePath{TEST_YAML_CONFIGURATION_PLUGIN_INSTALL_PATH}; const std::chrono::nanoseconds kTimeout = std::chrono::seconds(1); const std::chrono::nanoseconds kTimeoutServiceCall = std::chrono::seconds(1); @@ -277,22 +283,39 @@ TEST_F(MaliputQueryNodeAfterConfigurationTest, ConfigureStateAdvertisesServices) auto service_names_and_types = dut_->get_service_names_and_types(); ASSERT_STREQ(service_names_and_types[kRoadGeometryServiceName][0].c_str(), kRoadGeometryServiceType); + ASSERT_STREQ(service_names_and_types[kJunctionServiceName][0].c_str(), kJunctionServiceType); } // Makes sure services don't process the request when the node is not ACTIVE. TEST_F(MaliputQueryNodeAfterConfigurationTest, CallingServiceBeforeActiveYieldsToFailure) { AddNodeToExecutorAndSpin(dut_); TransitionToConfigureFromUnconfigured(); - ASSERT_TRUE(WaitForService(dut_, kRoadGeometryServiceName, kTimeout, kSleepPeriod)); - auto road_geometry_service = dut_->create_client(kRoadGeometryServiceName); - auto request = std::make_shared(); - auto future_result = road_geometry_service->async_send_request(request); - const auto future_status = future_result.wait_for(kTimeoutServiceCall); + { + ASSERT_TRUE(WaitForService(dut_, kRoadGeometryServiceName, kTimeout, kSleepPeriod)); - ASSERT_TRUE(future_status == std::future_status::ready); - const auto response = future_result.get(); - ASSERT_TRUE(response->road_geometry.id.id.empty()); + auto road_geometry_service = + dut_->create_client(kRoadGeometryServiceName); + auto request = std::make_shared(); + auto future_result = road_geometry_service->async_send_request(request); + const auto future_status = future_result.wait_for(kTimeoutServiceCall); + + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + ASSERT_TRUE(response->road_geometry.id.id.empty()); + } + { + ASSERT_TRUE(WaitForService(dut_, kJunctionServiceName, kTimeout, kSleepPeriod)); + + auto service = dut_->create_client(kJunctionServiceName); + auto request = std::make_shared(); + auto future_result = service->async_send_request(request); + const auto future_status = future_result.wait_for(kTimeoutServiceCall); + + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + ASSERT_TRUE(response->junction.id.id.empty()); + } } // Makes sure the node can transtion to the ACTIVE state. @@ -302,7 +325,7 @@ TEST_F(MaliputQueryNodeAfterConfigurationTest, TransitionToActiveIsSuccessful) { } // Test class used to hold the configuration of the RoadGeometryMock and validate the result of the service call. -class TestRoadGeometryServiceCall : public MaliputQueryNodeAfterConfigurationTest { +class RoadGeometryServiceCallTest : public MaliputQueryNodeAfterConfigurationTest { public: static constexpr int kSizeJunctions{0}; static constexpr int kSizeBranchPoints{0}; @@ -320,7 +343,7 @@ class TestRoadGeometryServiceCall : public MaliputQueryNodeAfterConfigurationTes } }; -TEST_F(TestRoadGeometryServiceCall, RoadGeometryRequestInActiveIsSuccessful) { +TEST_F(RoadGeometryServiceCallTest, RoadGeometryRequestInActiveIsSuccessful) { // Note: there is no point in populating Junctions and BranchPoints, it is already covered // in maliput_ros_translation package. EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_id()).WillRepeatedly(Return(kRoadGeometryId)); @@ -351,6 +374,86 @@ TEST_F(TestRoadGeometryServiceCall, RoadGeometryRequestInActiveIsSuccessful) { ASSERT_EQ(response->road_geometry.branch_point_ids.size(), static_cast(kSizeJunctions)); } +// Test class to wrap the tests of /junction service call. +class JunctionByIdServiceCallTest : public MaliputQueryNodeAfterConfigurationTest { + public: + void SetUp() override { + MaliputQueryNodeAfterConfigurationTest::SetUp(); + AddNodeToExecutorAndSpin(dut_); + TransitionToConfigureFromUnconfigured(); + TransitionToActiveFromConfigured(); + } +}; + +TEST_F(JunctionByIdServiceCallTest, ValidResquestAndResponse) { + static constexpr int kSize{2}; + const maliput::api::SegmentId kSgmentIdA{"segment_id_a"}; + const maliput::api::SegmentId kSgmentIdB{"segment_id_b"}; + const maliput::api::RoadGeometryId kRoadGeometryId{"road_geometry_id"}; + const maliput::api::JunctionId kJunctionId{"junction_id"}; + SegmentMock segment_a; + EXPECT_CALL(segment_a, do_id()).WillRepeatedly(Return(kSgmentIdA)); + SegmentMock segment_b; + EXPECT_CALL(segment_b, do_id()).WillRepeatedly(Return(kSgmentIdB)); + JunctionMock junction; + EXPECT_CALL(junction, do_id()).WillRepeatedly(Return(kJunctionId)); + EXPECT_CALL(junction, do_road_geometry()).WillRepeatedly(Return(road_network_ptrs_.road_geometry)); + EXPECT_CALL(junction, do_num_segments()).WillRepeatedly(Return(kSize)); + EXPECT_CALL(junction, do_segment(0)).WillRepeatedly(Return(&segment_a)); + EXPECT_CALL(junction, do_segment(1)).WillRepeatedly(Return(&segment_b)); + IdIndexMock id_index; + EXPECT_CALL(id_index, DoGetJunction(kJunctionId)).WillRepeatedly(Return(&junction)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), DoById()).WillRepeatedly(ReturnRef(id_index)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_id()).WillRepeatedly(Return(kRoadGeometryId)); + + auto junction_service = dut_->create_client(kJunctionServiceName); + ASSERT_TRUE(junction_service->wait_for_service(kTimeout)); + auto request = std::make_shared(); + request->id = maliput_ros_translation::ToRosMessage(kJunctionId); + auto future_result = junction_service->async_send_request(request); + auto future_status = future_result.wait_for(kTimeoutServiceCall); + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + + ASSERT_EQ(response->junction.id.id, kJunctionId.string()); + ASSERT_EQ(response->junction.road_geometry_id.id, kRoadGeometryId.string()); + ASSERT_EQ(response->junction.segment_ids.size(), static_cast(kSize)); + ASSERT_EQ(response->junction.segment_ids[0].id, kSgmentIdA.string()); + ASSERT_EQ(response->junction.segment_ids[1].id, kSgmentIdB.string()); +} + +TEST_F(JunctionByIdServiceCallTest, InvalidIdReturnsEmptyResponse) { + const maliput::api::RoadGeometryId kRoadGeometryId{"road_geometry_id"}; + const maliput::api::JunctionId kJunctionId{"invalid_id"}; + IdIndexMock id_index; + EXPECT_CALL(id_index, DoGetJunction(kJunctionId)).WillRepeatedly(Return(nullptr)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), DoById()).WillRepeatedly(ReturnRef(id_index)); + + auto junction_service = dut_->create_client(kJunctionServiceName); + ASSERT_TRUE(junction_service->wait_for_service(kTimeout)); + auto request = std::make_shared(); + request->id = maliput_ros_translation::ToRosMessage(kJunctionId); + auto future_result = junction_service->async_send_request(request); + auto future_status = future_result.wait_for(kTimeoutServiceCall); + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + + ASSERT_TRUE(response->junction.id.id.empty()); +} + +TEST_F(JunctionByIdServiceCallTest, EmptyIdReturnsEmptyResponse) { + auto junction_service = dut_->create_client(kJunctionServiceName); + ASSERT_TRUE(junction_service->wait_for_service(kTimeout)); + auto request = std::make_shared(); + request->id.id = ""; + auto future_result = junction_service->async_send_request(request); + auto future_status = future_result.wait_for(kTimeoutServiceCall); + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + + ASSERT_TRUE(response->junction.id.id.empty()); +} + } // namespace } // namespace test } // namespace ros From b029beffad5fbcfbeb565d0f999967d67cd7567c Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 9 Jan 2023 08:01:47 +0100 Subject: [PATCH 12/13] Fixes docstring. Signed-off-by: Agustin Alba Chicar --- maliput_ros/include/maliput_ros/ros/maliput_query_node.h | 1 + 1 file changed, 1 insertion(+) diff --git a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h index 3348c63..c94322c 100644 --- a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h +++ b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h @@ -68,6 +68,7 @@ namespace ros { /// - MaliputQueryNode::on_cleanup(): the maliput::api::RoadNetwork, the MaliputQuery, and the services are torn down. /// /// This query server offers: +/// - /junction: looks for a maliput::api::Junction by its ID. /// - /road_geometry: responds the maliput::api::RoadGeometry configuration. class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { public: From 9a2dbeb826dda21a7e98944f1d7ed59e53872a71 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Mon, 9 Jan 2023 08:48:23 +0100 Subject: [PATCH 13/13] Adds the service call creation. Signed-off-by: Agustin Alba Chicar --- maliput_ros/src/maliput_ros/ros/maliput_query_node.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc index 4663e6f..3f49863 100644 --- a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc +++ b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc @@ -111,6 +111,9 @@ void MaliputQueryNode::TearDownMaliputQuery() { bool MaliputQueryNode::InitializeAllServices() { RCLCPP_INFO(get_logger(), "InitializeAllServices"); + junction_srv_ = this->create_service( + kJunctionServiceName, + std::bind(&MaliputQueryNode::JunctionCallback, this, std::placeholders::_1, std::placeholders::_2)); road_geometry_srv_ = this->create_service( kRoadGeometryServiceName, std::bind(&MaliputQueryNode::RoadGeometryCallback, this, std::placeholders::_1, std::placeholders::_2)); @@ -120,6 +123,7 @@ bool MaliputQueryNode::InitializeAllServices() { void MaliputQueryNode::TearDownAllServices() { RCLCPP_INFO(get_logger(), "TearDownAllServices"); road_geometry_srv_.reset(); + junction_srv_.reset(); } MaliputQueryNode::LifecyleNodeCallbackReturn MaliputQueryNode::on_activate(const rclcpp_lifecycle::State&) {