【5日目】OSS連携してみたい！(3) Autoware-Microにエセ自作モジュール追加する

この記事は筆者オンリーのAdvent Calendar 20235日目の記事です。

前回までのあらすじ

ROS2でのHelloWorld的なパブリッシャー、サブスクライバー、Autowareのメッセージのサブスクライブまでを実施しました。
今回はAutowareのメッセージをサブスクライブして、Autowareにそのメッセージをそのまま渡すようなモジュールを作ってみたいと思います。

ビルド、ソースコード作成

前の記事を参考にROSのモジュールを作成。
今回はモジュール名をcpp_pathwithlaneid_pubsubとしてみます。
また、既存のAutowareがサブスクライブする対象のトピックも変更となるためAutoware-micro側の設定も変更します。

ソースコード

autoware_micro_awsim.launch.xml

      <!-- lane_driving -->
      〜割愛〜
        <group>
          <node pkg="cpp_pathwithlaneid_pubsub" exec="cpp_pathwithlaneid_pubsub"/>
        </group> <!-- Simple Pub Sub -->
      </group> <!-- lane_driving -->
      〜割愛〜
      <!-- Customizable -->
      <node pkg="path_to_trajectory" exec="path_to_trajectory_node" name="path_to_trajectory" output="screen">
        <remap from="input" to="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id_modified"/>
      </node>

package.xml

<?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>cpp_pathwithlaneid_pubsub</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="[e-mail]">msuda</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>autoware_cmake</build_depend>

  <depend>autoware_auto_planning_msgs</depend>
  <depend>rclcpp</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 3.14)
project(cpp_pathwithlaneid_pubsub)
find_package(ament_cmake REQUIRED)
find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_add_executable(cpp_pathwithlaneid_pubsub
  src/cpp_pathwithlaneid_pubsub.cpp
  src/cpp_pathwithlaneid_pubsub_impl.cpp
)

ament_auto_package(INSTALL_TO_SHARE)

cpp_pathwithlaneid_pubsub.hpp

#ifndef PATH_SUBSCRIBER_HPP_
#define PATH_SUBSCRIBER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>

using PathWithLaneId = autoware_auto_planning_msgs::msg::PathWithLaneId;

class PathSubscriber : public rclcpp::Node
{
public:
    PathSubscriber(void);
    void topicCallback(const PathWithLaneId::ConstSharedPtr msg);

private:
    rclcpp::Subscription<PathWithLaneId>::ConstSharedPtr sub_path_sub_;
    rclcpp::Publisher<PathWithLaneId>::SharedPtr sub_path_pub_;
};

#endif // PATH_SUBSCRIBER_HPP_

cpp_pathwithlaneid_pubsub.cpp

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "cpp_pathwithlaneid_pubsub.hpp"
#include <memory>

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PathSubscriber>());
  rclcpp::shutdown();
  return 0;
}

cpp_pathwithlaneid_pubsub_impl.cpp

#include "cpp_pathwithlaneid_pubsub.hpp"
#include <memory>

using PathWithLaneId = autoware_auto_planning_msgs::msg::PathWithLaneId;
using std::placeholders::_1;

PathSubscriber::PathSubscriber() : Node("path_subscriber")
{

  sub_path_pub_ = create_publisher<PathWithLaneId>(
      "behavior_planning/path_with_lane_id_modified", 1);

  sub_path_sub_ = create_subscription<PathWithLaneId>("behavior_planning/path_with_lane_id", 10, std::bind(&PathSubscriber::topicCallback, this, _1));
}

void PathSubscriber::topicCallback(const PathWithLaneId::ConstSharedPtr msg)
{
  PathWithLaneId path;

  path = *msg;

  /*ここは本記事とは関係なし
  path.header = msg->header;
  path.left_bound = msg->left_bound;
  path.right_bound = msg->right_bound;
  for (const auto &point : msg->points)
  {
    path.points.push_back(point.point);
  }
  */

  // RCLCPP_INFO(this->get_logger(), "I heard");

  sub_path_pub_->publish(path);
}

実行

通常通りAutowareが動作することを確認しました。

これから

1.メッセージを今回は丸コピだったので、各メンバーごとに渡すように変更する。
2.連携できそうなOSSを調査
3.Autoware以外のOSSと連携させる
の流れで行ければな。。。と思っています。