【14日目】OSS連携してみたい！(11)PythonRoboticsのpure pursuitをAutowareに移植する(1)

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

前回までのあらすじ

Autoware、pythonroboticsのpure pursuitについて調査してみました。
今回は何も考えずに前進するだけのモジュールを作っていきたいと思います。
内容は、autoware-microのpure pursuitのソースコードからpure
pursuitの部分を取り除いて速度、アクセラレーションを固定値にして前進するだけです。

ソースコード

ソースコードはこんな感じ。
30ms間隔でサブスクリプションがmsgを受信しコールバック関数に直打ちした速度、アクセラレーション値で動きます。

my_pure_pursuit.cpp

#include "my_pure_pursuit.hpp"
#include <motion_utils/motion_utils.hpp>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

#include <tf2/utils.h>

#include <algorithm>
#include <memory>
#include <vector>

bool isinitialized = false;

namespace my_pure_pursuit
{
  using std::placeholders::_1;

  MyPurePursuit::MyPurePursuit() : Node("my_pure_pursuit")
  {

    pub_cmd_ = create_publisher<AckermannControlCommand>("/control/command/control_cmd", 1);

    sub_kinematics_ = create_subscription<Odometry>(
        "/localization/kinematic_state", 1, [this](const Odometry::SharedPtr msg)
        { odometry_ = msg; });
    sub_trajectory_ = create_subscription<Trajectory>(
        "/planning/scenario_planning/trajectory", 1, [this](const Trajectory::SharedPtr msg)
        { trajectory_ = msg; });

    using namespace std::literals::chrono_literals;
    timer_ =
        rclcpp::create_timer(this, get_clock(), 30ms, std::bind(&MyPurePursuit::onTimer, this));
  }

  AckermannControlCommand zeroAckermannControlCommand(rclcpp::Time stamp)
  {
    AckermannControlCommand cmd;
    cmd.stamp = stamp;
    cmd.longitudinal.stamp = stamp;
    cmd.longitudinal.speed = 0.0;
    cmd.longitudinal.acceleration = 0.0;
    cmd.lateral.stamp = stamp;
    cmd.lateral.steering_tire_angle = 0.0;
    return cmd;
  }

  bool isInitialized = false;
  std::vector<double> tmp;
  // TargetCourse target_course(tmp, tmp);
  int target_ind = 0;
  void MyPurePursuit::onTimer()
  {
    RCLCPP_INFO(this->get_logger(), "start onTimer()");
    // check data
    if (!subscribeMessageAvailable())
    {
      RCLCPP_INFO(this->get_logger(), "subscribeMessageAvailable not found");
      return;
    }

    // publish zero command
    AckermannControlCommand cmd = zeroAckermannControlCommand(get_clock()->now());

    RCLCPP_INFO(this->get_logger(), "yaw=%f", tf2::getYaw(odometry_->pose.pose.orientation));

    // get closest trajectory point from current position

    // RCLCPP_INFO(this->get_logger(), "I heard in onTimer() %d", target_ind);

    // cmd.lateral.steering_tire_angle = target_ind;
    RCLCPP_INFO(this->get_logger(), "Sending cmd");
    cmd.stamp = get_clock()->now();
    cmd.longitudinal.stamp = get_clock()->now();
    cmd.longitudinal.speed = 100;
    cmd.longitudinal.acceleration = 100;
    cmd.lateral.stamp = get_clock()->now();
    cmd.lateral.steering_tire_angle = 0;

    pub_cmd_->publish(cmd);
  }
  bool MyPurePursuit::subscribeMessageAvailable()
  {
    if (!odometry_)
    {
      RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available");
      return false;
    }
    if (!trajectory_)
    {
      RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available");
      return false;
    }
    return true;
  }

} // namespace my_pure_pursuit

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

パッケージのビルド

次に、Dockerを起動してコードをビルドします。

$ cd /aichallenge/
$ bash clean_build.sh 

パッケージの実行

実行してみます。

$ bash run_autoware.sh
$ bash run_awsim.sh

レーシングカーが直進し続けています。

参考記事

自動運転 AIチャレンジ　~導入編~