Create maliput ros node (#8)

* 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 <ag.albachicar@gmail.com> * Addresses comments. Signed-off-by: Agustin Alba Chicar <ag.albachicar@gmail.com> Signed-off-by: Agustin Alba Chicar <ag.albachicar@gmail.com>
maliput · Jan 9, 2023 · 7e6d5aa · 7e6d5aa
1 parent f1452b7
commit 7e6d5aa
Show file tree

Hide file tree

Showing 10 changed files with 626 additions and 0 deletions.
diff --git 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,30 @@ endif()
 # Export
 ##############################################################################
 
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+ament_export_include_directories(include)
+
+install(DIRECTORY
+  launch
+  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)
+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
@@ -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 <memory>
+#include <utility>
+
+#include <maliput/api/road_geometry.h>
+#include <maliput/api/road_network.h>
+#include <maliput/common/maliput_throw.h>
+
+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<maliput::api::RoadNetwork> 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<maliput::api::RoadNetwork> 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
@@ -0,0 +1,152 @@
+// 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 <atomic>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include <maliput_ros_interfaces/srv/road_geometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+
+#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 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. The YAML file must have the following structure:
+/// @code {.yaml}
+/// maliput:"
+///   backend: <backend_name> "
+///   parameters: "
+///      <key_1>: <value_1> "
+///      <key_2>: <value_2> "
+///      // ...
+///      <key_N>: <value_N> "
+/// @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.
+/// - 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 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_ = "",
+                   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<maliput_ros_interfaces::srv::RoadGeometry::Request> request,
+                            std::shared_ptr<maliput_ros_interfaces::srv::RoadGeometry::Response> 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<bool> is_active_;
+  // /road_geometry service.
+  rclcpp::Service<maliput_ros_interfaces::srv::RoadGeometry>::SharedPtr road_geometry_srv_;
+  // Proxy to a maliput::api::RoadNetwork queries.
+  std::unique_ptr<maliput_ros::ros::MaliputQuery> 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
@@ -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 maliput_query_server node.
+    maliput_query_server_node = launch_ros.actions.LifecycleNode(
+        package='maliput_ros', executable='maliput_ros', name='maliput_query_server',
+        namespace='maliput_ros', output='screen',
+        parameters=[{"yaml_configuration_path": maliput_yaml_path}])
+
+    # When the maliput_query_server reaches the 'inactive' state, make it take the 'activate' transition.
+    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 '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,
+                )),
+            ],
+        )
+    )
+
+    # 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 'maliput_query_server' reached the 'active' state."),
+            ],
+        )
+    )
+
+    # 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,
+        )
+    )
+
+    return launch.LaunchDescription([
+        maliput_yaml_path_arg,
+        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_maliput_query_server_does_configure_transition,
+    ])
diff --git a/maliput_ros/package.xml b/maliput_ros/package.xml
@@ -11,9 +11,18 @@
 
   <doc_depend>ament_cmake_doxygen</doc_depend>
 
+
   <depend>maliput</depend>
+  <depend>maliput_ros_interfaces</depend>
+  <depend>maliput_ros_translation</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>yaml-cpp</depend>
 
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>lifecycle_msgs</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>

diff --git a/maliput_ros/resources/maliput_malidrive_plugin.yml b/maliput_ros/resources/maliput_malidrive_plugin.yml
@@ -0,0 +1,16 @@
+# Example configuration for maliput_malidrive and to load the TShapeRoad.xodr map.
+maliput:
+  backend: "maliput_malidrive"
+  parameters:
+    road_geometry_id: "t_shape_road"
+    opendrive_file: "/opt/ros/foxy/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
@@ -1 +1,2 @@
+add_subdirectory(ros)
 add_subdirectory(utils)