feat: add dummy_infrastructure package (autowarefoundation#19)

* Feature/add virtual traffic light planner (autowarefoundation#1588)

* Change formatter to clang-format and black (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* delete COLCON_IGNORE (autowarefoundation#540)

* add readme [dummy infrastructure] (autowarefoundation#693)

* add readme dummy infra

* fix lint

* update readme

Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp>

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp>

system/dummy_infrastructure/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5)
+project(dummy_infrastructure)
+
+## Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+## Dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+## Targets
+ament_auto_add_library(dummy_infrastructure_node_component SHARED
+  src/dummy_infrastructure_node/dummy_infrastructure_node.cpp
+)
+
+rclcpp_components_register_node(dummy_infrastructure_node_component
+  PLUGIN "dummy_infrastructure::DummyInfrastructureNode"
+  EXECUTABLE dummy_infrastructure_node
+)
+
+## Tests
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+## Package
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)

system/dummy_infrastructure/README.md

@@ -0,0 +1,40 @@
+# dummy_infrastructure
+
+This is a debug node for infrastructure communication.
+
+## Usage
+
+```sh
+ros2 launch dummy_infrastructure dummy_infrastructure.launch.xml
+ros2 run rqt_reconfigure rqt_reconfigure
+```
+
+## Inputs / Outputs
+
+### Inputs
+
+| Name                    | Type                                                 | Description            |
+| ----------------------- | ---------------------------------------------------- | ---------------------- |
+| `~/input/command_array` | `autoware_v2x_msgs::msg::InfrastructureCommandArray` | Infrastructure command |
+
+### Outputs
+
+| Name                   | Type                                                    | Description                 |
+| ---------------------- | ------------------------------------------------------- | --------------------------- |
+| `~/output/state_array` | `autoware_v2x_msgs::msg::VirtualTrafficLightStateArray` | Virtual traffic light array |
+
+## Parameters
+
+### Node Parameters
+
+| Name                | Type   | Default Value | Explanation                                       |
+| ------------------- | ------ | ------------- | ------------------------------------------------- |
+| `update_rate`       | int    | `10`          | Timer callback period [Hz]                        |
+| `use_first_command` | bool   | `true`        | Consider instrument id or not                     |
+| `instrument_id`     | string | ``            | Used as command id                                |
+| `approval`          | bool   | `false`       | set approval filed to ros param                   |
+| `is_finalized`      | bool   | `false`       | Stop at stop_line if finalization isn't completed |
+
+## Assumptions / Known limits
+
+TBD.

system/dummy_infrastructure/config/dummy_infrastructure.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    update_rate_hz: 10.0

system/dummy_infrastructure/include/dummy_infrastructure/dummy_infrastructure_node.hpp

@@ -0,0 +1,79 @@
+// Copyright 2021 Tier IV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP__
+#define DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP__
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_v2x_msgs/msg/infrastructure_command_array.hpp>
+#include <autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace dummy_infrastructure
+{
+using autoware_v2x_msgs::msg::InfrastructureCommand;
+using autoware_v2x_msgs::msg::InfrastructureCommandArray;
+using autoware_v2x_msgs::msg::VirtualTrafficLightState;
+using autoware_v2x_msgs::msg::VirtualTrafficLightStateArray;
+
+class DummyInfrastructureNode : public rclcpp::Node
+{
+public:
+  explicit DummyInfrastructureNode(const rclcpp::NodeOptions & node_options);
+
+  struct NodeParam
+  {
+    double update_rate_hz{};
+    bool use_first_command{};
+    std::string instrument_id{};
+    bool approval{};
+    bool is_finalized{};
+  };
+
+private:
+  // Subscriber
+  rclcpp::Subscription<InfrastructureCommandArray>::SharedPtr sub_command_array_{};
+
+  // Callback
+  void onCommandArray(const InfrastructureCommandArray::ConstSharedPtr msg);
+
+  // Data Buffer
+  InfrastructureCommandArray::ConstSharedPtr command_array_{};
+
+  // Publisher
+  rclcpp::Publisher<VirtualTrafficLightStateArray>::SharedPtr pub_state_array_{};
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_{};
+
+  bool isDataReady();
+  void onTimer();
+
+  // Parameter Server
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onSetParam(
+    const std::vector<rclcpp::Parameter> & params);
+
+  // Parameter
+  NodeParam node_param_{};
+};
+
+}  // namespace dummy_infrastructure
+
+#endif  // DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP__

system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml

@@ -0,0 +1,22 @@
+<launch>
+  <!-- Input -->
+  <arg name="input/command_array" default="/planning/scenario_planning/status/infrastructure_commands" />
+
+  <!-- Output -->
+  <arg name="output/state_array" default="/system/v2x/virtual_traffic_light_states" />
+
+  <!-- Parameter -->
+  <arg name="config_file" default="$(find-pkg-share dummy_infrastructure)/config/dummy_infrastructure.param.yaml" />
+
+  <!-- Node -->
+  <node pkg="dummy_infrastructure" exec="dummy_infrastructure_node" name="dummy_infrastructure" output="screen">
+    <!-- Input -->
+    <remap from="~/input/command_array" to="$(var input/command_array)"/>
+
+    <!-- Output -->
+    <remap from="~/output/state_array" to="$(var output/state_array)"/>
+
+    <!-- Parameter -->
+    <param from="$(var config_file)" />
+  </node>
+</launch>

system/dummy_infrastructure/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dummy_infrastructure</name>
+  <version>0.0.0</version>
+  <description>dummy_infrastructure</description>
+  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_v2x_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp

@@ -0,0 +1,165 @@
+// Copyright 2021 Tier IV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "dummy_infrastructure/dummy_infrastructure_node.hpp"
+
+#include <boost/optional.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+using namespace std::literals;
+using std::chrono::duration;
+using std::chrono::duration_cast;
+using std::chrono::nanoseconds;
+using std::placeholders::_1;
+
+namespace dummy_infrastructure
+{
+namespace
+{
+template <class T>
+bool update_param(
+  const std::vector<rclcpp::Parameter> & params, const std::string & name, T & value)
+{
+  const auto itr = std::find_if(
+    params.cbegin(), params.cend(),
+    [&name](const rclcpp::Parameter & p) { return p.get_name() == name; });
+
+  // Not found
+  if (itr == params.cend()) {
+    return false;
+  }
+
+  value = itr->template get_value<T>();
+  return true;
+}
+
+boost::optional<InfrastructureCommand> findCommand(
+  const InfrastructureCommandArray & command_array, const std::string & instrument_id,
+  const bool use_first_command)
+{
+  if (use_first_command && !command_array.commands.empty()) {
+    return command_array.commands.front();
+  }
+
+  for (const auto & command : command_array.commands) {
+    if (command.id == instrument_id) {
+      return command;
+    }
+  }
+
+  return {};
+}
+}  // namespace
+
+DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & node_options)
+: Node("dummy_infrastructure", node_options)
+{
+  // Parameter Server
+  set_param_res_ =
+    this->add_on_set_parameters_callback(std::bind(&DummyInfrastructureNode::onSetParam, this, _1));
+
+  // Parameter
+  node_param_.update_rate_hz = declare_parameter<double>("update_rate_hz", 10.0);
+  node_param_.use_first_command = declare_parameter<bool>("use_first_command", true);
+  node_param_.instrument_id = declare_parameter<std::string>("instrument_id", "");
+  node_param_.approval = declare_parameter<bool>("approval", false);
+  node_param_.is_finalized = declare_parameter<bool>("is_finalized", false);
+
+  // Subscriber
+  sub_command_array_ = create_subscription<InfrastructureCommandArray>(
+    "~/input/command_array", rclcpp::QoS{1},
+    std::bind(&DummyInfrastructureNode::onCommandArray, this, _1));
+
+  // Publisher
+  pub_state_array_ = create_publisher<VirtualTrafficLightStateArray>("~/output/state_array", 1);
+
+  // Timer
+  const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period();
+  timer_ = rclcpp::create_timer(
+    this, get_clock(), update_period_ns, std::bind(&DummyInfrastructureNode::onTimer, this));
+}
+
+void DummyInfrastructureNode::onCommandArray(const InfrastructureCommandArray::ConstSharedPtr msg)
+{
+  command_array_ = msg;
+}
+
+rcl_interfaces::msg::SetParametersResult DummyInfrastructureNode::onSetParam(
+  const std::vector<rclcpp::Parameter> & params)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+
+  try {
+    // Copy to local variable
+    auto p = node_param_;
+
+    // Update params
+    update_param(params, "update_rate_hz", p.update_rate_hz);
+    update_param(params, "use_first_command", p.use_first_command);
+    update_param(params, "instrument_id", p.instrument_id);
+    update_param(params, "approval", p.approval);
+    update_param(params, "is_finalized", p.is_finalized);
+
+    // Copy back to member variable
+    node_param_ = p;
+  } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+    result.successful = false;
+    result.reason = e.what();
+    return result;
+  }
+
+  result.successful = true;
+  result.reason = "success";
+  return result;
+}
+
+bool DummyInfrastructureNode::isDataReady()
+{
+  if (!command_array_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for command_array msg...");
+    return false;
+  }
+
+  return true;
+}
+
+void DummyInfrastructureNode::onTimer()
+{
+  if (!isDataReady()) {
+    return;
+  }
+
+  const auto command =
+    findCommand(*command_array_, node_param_.instrument_id, node_param_.use_first_command);
+
+  VirtualTrafficLightState state;
+  state.stamp = get_clock()->now();
+  state.id = command ? command->id : node_param_.instrument_id;
+  state.type = "dummy_infrastructure";
+  state.approval = command ? node_param_.approval : false;
+  state.is_finalized = node_param_.is_finalized;
+
+  VirtualTrafficLightStateArray state_array;
+  state_array.stamp = get_clock()->now();
+  state_array.states.push_back(state);
+  pub_state_array_->publish(state_array);
+}
+
+}  // namespace dummy_infrastructure
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(dummy_infrastructure::DummyInfrastructureNode)