この記事は筆者オンリーの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="masanori.suda@gmail.com">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と連携させる
の流れで行ければな。。。と思っています。