#4 Use new struct

control/CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(ackermann_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(example_interfaces REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -48,8 +49,9 @@ include_directories(
 add_executable(${PROJECT_NAME}
   src/main.cpp
   src/control.cpp
+  src/controller/pure_pursuit.cpp
 )
-ament_target_dependencies(${PROJECT_NAME} rclcpp nav_msgs ackermann_msgs tf2 tf2_geometry_msgs)
+ament_target_dependencies(${PROJECT_NAME} rclcpp nav_msgs ackermann_msgs tf2 tf2_geometry_msgs example_interfaces)
 
 install(TARGETS
   ${PROJECT_NAME}

control/include/control/control.h

@@ -5,16 +5,15 @@
 #include "controller/pure_pursuit.hpp"
 
 #include "rclcpp/rclcpp.hpp"
-#include "nav_msgs/msg/path.hpp"
 #include "std_msgs/msg/int8.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-// #include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "example_interfaces/msg/float64.hpp"
 
-#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
 #include "tf2/LinearMath/Quaternion.h"
-#include <tf2/LinearMath/Matrix3x3.h>
-#include <tf2/convert.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include "tf2/LinearMath/Matrix3x3.h"
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
 #include <string>
 #include <cmath>
@@ -26,28 +25,31 @@ class Control : public rclcpp::Node {
 private:
     PurePursuit controller;
 
-    std::vector<Pose> refPoses;
+    std::vector<Path> refPoses;
     Pose currPose;
-    int currVel;
-    bool currDirection;
 
     float speedCommand;
     float steerCommand;
 
-    bool velocityValid;
+    bool pathValid;
     bool poseValid;
+    bool velocityValid;
 
-    rclcpp::Publisher<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr drive_publisher_;
+    float target_velocity;
+
+    // Subscribe
     rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_subscription_;
-    rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr velocity_subscription_;
-    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_subscription_;
-    // rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr pose_subscription_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr pose_subscription_;
+    rclcpp::Subscription<example_interfaces::msg::Float64>::SharedPtr velocity_subscription_;
+
+    // Publish
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr drive_publisher_;
+
     rclcpp::TimerBase::SharedPtr publisher_timer_;
 
     void path_callback(const nav_msgs::msg::Path::SharedPtr path_msg);
-    void velocity_callback(const std_msgs::msg::Int8::SharedPtr velocity_msg);
-    void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr pose_msg);
-    // void pose_callback(const nav_msgs::msg::Odometry::SharedPtr pose_msg);
+    void pose_callback(const nav_msgs::msg::Odometry::SharedPtr pose_msg);
+    void velocity_callback(const example_interfaces::msg::Float64::SharedPtr velocity_msg);
     void publisher_timer_callback();
     void publish_drive(float speed, float steer);
     float quat_to_yaw(const geometry_msgs::msg::Quaternion quat_msg);

control/include/control/controller/pure_pursuit.hpp

@@ -1,16 +1,18 @@
 #pragma once
 
+#include "msg_structs.h"
+
 #include <array>
 #include <cmath>
 #include <iostream>
-#include <unistd.h>
+#include <vector>
 
 class PurePursuit {
 
 public:
-    PurePursuit(std::vector<std::vector<int>> route, double speed, std::vector<int> cpoint, double WB, double Kdd, double Ldc);
+    PurePursuit(double WB, double Kdd, double Ldc);
 
-    void purepursuit_control();
+    void purepursuit_control(std::vector<Path> refPoses, double target_v, double x, double y, double yaw, double v);
 
     double getThrottle();
     double getDelta();
@@ -45,7 +47,7 @@ class PurePursuit {
     double delta;
 
     // Path
-    std::vector<std::vector<double>> route;
+    std::vector<Path> refPoses;
     int route_size;
 
     // Target Status
@@ -56,5 +58,5 @@ class PurePursuit {
     double purepursuit_steer_calc();
     int update_target_node();
     void update_state(double x, double y, double yaw, double v);
-    double calc_distance(int point_x, int point_y);
+    double calc_distance(double point_x, double point_y);
 };

control/include/control/msg_structs.h

@@ -2,9 +2,15 @@
 
 #include <string>
 
+struct Path {
+    float x;
+    float y;
+    float yaw;
+};
+
 struct Pose {
     float x;
     float y;
-    float heading;
+    float yaw;
     float v;
 };

control/src/control.cpp

@@ -1,17 +1,19 @@
 #include "control.h"
 
-Control::Control() : rclcpp::Node("vehicle_control") {
+Control::Control() : rclcpp::Node("vehicle_control"), controller(2.7, 0.5, 4.0) {
+    // this->controller = PurePursuit(2.7, 0.5, 4.0);
+
     // Subscribe
     path_subscription_ = this->create_subscription<nav_msgs::msg::Path>(
-        "/planner/path", 10, std::bind(&Control::path_callback, this,  std::placeholders::_1));
-    velocity_subscription_ = this->create_subscription<std_msgs::msg::Int8>(
-        "/planner/velocity", 10, std::bind(&Control::velocity_callback, this,  std::placeholders::_1));
-    pose_subscription_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
-        "/localization/pose", 10, std::bind(&Control::pose_callback, this,  std::placeholders::_1));
+        "/pathplanner/path", 10, std::bind(&Control::path_callback, this,  std::placeholders::_1));
+    velocity_subscription_ = this->create_subscription<example_interfaces::msg::Float64>(
+        "/pathplanner/throttle", 10, std::bind(&Control::velocity_callback, this, std::placeholders::_1));
+    pose_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
+        "/piracer/odom", 10, std::bind(&Control::pose_callback, this,  std::placeholders::_1));
 
     // Publish
-    drive_publisher_ = this->create_publisher<ackermann_msgs::msg::AckermannDriveStamped>(
-        "/controller/drive", 10);
+    drive_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
+        "/piracer/cmd_vel", 10);
 
     publisher_timer_ = this->create_wall_timer(
         std::chrono::milliseconds(100),
@@ -20,40 +22,48 @@ Control::Control() : rclcpp::Node("vehicle_control") {
 }
 
 void Control::path_callback(const nav_msgs::msg::Path::SharedPtr path_msg) {
+    this->pathValid = false;
     this->refPoses.clear();
-    Pose refPose;
+    Path refPose;
     for (size_t i = 0; i < path_msg->poses.size(); ++i) {
         refPose.x = path_msg->poses[i].pose.position.x;
         refPose.y = path_msg->poses[i].pose.position.y;
-        // refPose.heading = this->quat_to_yaw(path_msg->poses[i].pose.orientation);
+        refPose.yaw = this->quat_to_yaw(path_msg->poses[i].pose.orientation);
         this->refPoses.push_back(refPose);
     }
+    this->pathValid = true;
 }
 
-void Control::velocity_callback(const std_msgs::msg::Int8::SharedPtr velocity_msg) {
+void Control::velocity_callback(const example_interfaces::msg::Float64::SharedPtr velocity_msg) {
     this->velocityValid = false;
-    this->currVel = velocity_msg->data;
-    if (this->currVel >= 0) {this->currDirection = true;}
-    else {this->currDirection = false;}
+    this->target_velocity = velocity_msg->data;
     this->velocityValid = true;
 }
 
-void Control::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr pose_msg) {
+void Control::pose_callback(const nav_msgs::msg::Odometry::SharedPtr pose_msg) {
     this->poseValid = false;
-    this->currPose.x = pose_msg->pose.position.x;
-    this->currPose.y = pose_msg->pose.position.y;
-    this->currPose.heading = this->quat_to_yaw(pose_msg->pose.orientation);
+    this->currPose.x = pose_msg->pose.pose.position.x * 100;  // [cm]
+    this->currPose.y = pose_msg->pose.pose.position.y * 100;  // [cm]
+
+    tf2::Quaternion q(
+      pose_msg->pose.pose.orientation.x,
+      pose_msg->pose.pose.orientation.y,
+      pose_msg->pose.pose.orientation.z,
+      pose_msg->pose.pose.orientation.w);
+    tf2::Matrix3x3 m(q);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);  // [rad]
+    this->currPose.yaw = yaw;
+
+    this->currPose.v = sqrt(pow(pose_msg->twist.twist.linear.x, 2)
+                        + pow(pose_msg->twist.twist.linear.y, 2)) * 100;  // [cm/s]
     this->poseValid = true;
 }
 
 void Control::publisher_timer_callback() {
-    if (this->refPoses.empty() || !this->poseValid || !this->velocityValid) {return;}
-
-    path_callback();
-    velocity_callback();
-    pose_callback();
+    if (this->refPoses.empty() || !this->poseValid || !this->pathValid) {return;}
 
-    this->controller.purepursuit_control(this->currPose.x, this->currPose.y, this->currPose.heading, this->v);
+    this->controller.purepursuit_control(this->refPoses, this->target_velocity, this->currPose.x, this->currPose.y, this->currPose.yaw, this->currPose.v);
 
     this->speedCommand = this->controller.getThrottle();
     this->steerCommand = - this->controller.getDelta() * 180 / M_PI;
@@ -62,16 +72,11 @@ void Control::publisher_timer_callback() {
 }
 
 void Control::publish_drive(float speed, float steer) {
-    ackermann_msgs::msg::AckermannDriveStamped drive_msg;
-
-    drive_msg.header = std_msgs::msg::Header();
-    drive_msg.header.stamp = rclcpp::Clock().now();
-    drive_msg.header.frame_id = "map";
-
-    drive_msg.drive.speed = speed;
-    drive_msg.drive.steering_angle = steer;
-
-    this->drive_publisher_->publish(drive_msg);
+    auto drive_msg = std::make_unique<geometry_msgs::msg::Twist>();
+    drive_msg->linear.x = speed;
+    drive_msg->angular.z = steer;
+    std::cout << "Throttle : " << speed << " Steering : " << steer << std::endl;
+    this->drive_publisher_->publish(std::move(drive_msg));
 }
 
 float Control::quat_to_yaw(const geometry_msgs::msg::Quaternion quat) {
@@ -83,4 +88,4 @@ float Control::quat_to_yaw(const geometry_msgs::msg::Quaternion quat) {
     tf2::Matrix3x3 matrix(tf2_quat);
     matrix.getRPY(roll, pitch, yaw);
     return yaw;
-}
+}

control/src/controller/pure_pursuit.cpp

@@ -1,9 +1,11 @@
-PurePursuit::PurePursuit(std::vector<std::vector<int>> route, double speed, std::vector<double> cpoint, double WB, double Kdd, double Ldc) {
+#include "controller/pure_pursuit.hpp"
+
+PurePursuit::PurePursuit(double WB, double Kdd, double Ldc) {
     // Vehicle Status Init
-    this->x = cpoint[0];
-    this->y = cpoint[1];
-    this->yaw = cpoint[2];
-    this->v = 0.0;
+    // this->x = 0.0;
+    // this->y = 0.0;
+    // this->yaw = 0.0;
+    // this->v = 0.0;
     this->acceleration = 0.0;
 
     this->WB = WB;
@@ -27,16 +29,13 @@ PurePursuit::PurePursuit(std::vector<std::vector<int>> route, double speed, std:
     // PurePursuit Init
     this->delta = 0.0;
 
-    // Path Init
-    this->route = route;
-    this->route_size = route.size();
-
     // Target Status Init
-    this->target_speed = speed;
     this->target_node = 0;
 }
 
-void PurePursuit::purepursuit_control(double x, double y, double yaw, double v) {
+void PurePursuit::purepursuit_control(std::vector<Path> refPoses, double target_v, double x, double y, double yaw, double v) {
+    this->target_speed = target_v;
+    this->refPoses = refPoses;
     this->acceleration = this->pid_speed_control();
     this->delta = this->purepursuit_steer_calc();
     this->target_node = this->update_target_node();
@@ -56,8 +55,9 @@ double PurePursuit::pid_speed_control() {
 }
 
 double PurePursuit::purepursuit_steer_calc() {
-    double tx = route[this->target_node][0];
-    double ty = route[this->target_node][1];
+    double tx = refPoses[this->target_node].x;
+    double ty = refPoses[this->target_node].y;
+    // double tyaw = refPoses[this->target_node].yaw;
 
     double alpha = atan2(ty - this->rear_y, tx - this->rear_x) - this->yaw;
     double delta = atan2(2.0 * this->WB * sin(alpha)/this->Ld, 1.0);
@@ -67,13 +67,13 @@ double PurePursuit::purepursuit_steer_calc() {
 
 int PurePursuit::update_target_node() {
     int cnode = this->target_node;
-    if(cnode == this->route_size-1) { return cnode; }
+    if(cnode == this->refPoses.size()-1) { return cnode; }
 
-    double distance_cnode = this->calc_distance(this->route[cnode][0], this->route[cnode][1]);
+    double distance_cnode = this->calc_distance(this->refPoses[cnode].x, this->refPoses[cnode].y);
     double distance_nnode = 0.0;
 
-    while (cnode < this->route.size()-1) {
-        distance_nnode = this->calc_distance(this->route[cnode + 1][0], this->route[cnode + 1][1]);
+    while (cnode < this->refPoses.size()-1) {
+        distance_nnode = this->calc_distance(this->refPoses[cnode + 1].x, this->refPoses[cnode + 1].y);
         if (distance_cnode < distance_nnode) {
             break;
         }
@@ -83,8 +83,8 @@ int PurePursuit::update_target_node() {
 
     this->Ld = this->Kdd * this->v + this->Ldc;
 
-    if(cnode < this->route_size-1) {
-        while(this->Ld > this->calc_distance(this->route[cnode][0], this->route[cnode][1])) {
+    if(cnode < this->refPoses.size()-1) {
+        while(this->Ld > this->calc_distance(this->refPoses[cnode].x, this->refPoses[cnode].y)) {
             cnode += 1;
         }
     }
@@ -102,7 +102,7 @@ void PurePursuit::update_state(double x, double y, double yaw, double v) {
     rear_y = y - ((WB / 2) * sin(yaw));
 }
 
-double PurePursuit::calc_distance(int point_x, int point_y) {
+double PurePursuit::calc_distance(double point_x, double point_y) {
     double dx = this->rear_x - point_x;
     double dy = this->rear_y - point_y;