Feat (control): separate control modes

aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml

@@ -10,22 +10,22 @@
     <let name="steering_tire_angle_gain_var" value="1.0" if="$(var simulation)" />
     <let name="steering_tire_angle_gain_var" value="1.639" unless="$(var simulation)" />
 
-    <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node"
+    <!-- <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node"
       output="screen" unless="$(var use_stanley)">
-      <param name="use_external_target_vel" value="false" />
+      <param name="use_external_target_vel" value="true" />
       <param name="external_target_vel" value="8.0" />
       <param name="lookahead_gain" value="0.24" />
       <param name="lookahead_min_distance" value="2.0" />
       <param name="speed_proportional_gain" value="2.0" />
       <param name="steering_tire_angle_gain" value="$(var steering_tire_angle_gain_var)"/>
 
       <remap from="input/kinematics" to="/localization/kinematic_state" />
-      <!-- <remap from="input/trajectory" to="/planning/scenario_planning/trajectory" /> -->
+      <remap from="input/trajectory" to="/planning/scenario_planning/trajectory" />
       <remap from="input/trajectory" to="/planning/output/mppi_planned_path" />
       <remap from="output/control_cmd" to="/control/command/control_cmd" />
-    </node>
+    </node> -->
 
-    <node pkg="stanley_control" exec="stanley_control" name="stanley_control_node" output="screen"
+    <!-- <node pkg="stanley_control" exec="stanley_control" name="stanley_control_node" output="screen"
       if="$(var use_stanley)">
       <param name="speed_proportional_gain" value="2.14" />
       <param name="external_target_vel" value="8.0" />
@@ -35,10 +35,39 @@
       <remap from="input/kinematics" to="/localization/kinematic_state" />
       <remap from="input/trajectory" to="/planning/scenario_planning/trajectory" />
       <remap from="output/control_cmd" to="/control/command/control_cmd" />
+    </node> -->
+
+    <!-- Control component -->
+    <!-- Longitudinal (speed) control -->
+    <node pkg="simple_pd_speed_control" exec="simple_pd_speed_control_node" name="simple_pd_speed_control" output="screen">
+      <param name="speed_proportional_gain" value="2.0" />
+      <remap from="input/kinematics" to="/localization/kinematic_state" />
+      <remap from="input/trajectory" to="/planning/output/mppi_planned_path" />
+    </node>
+
+    <!-- Lateral (speed) control -->
+    <node pkg="lateral_pure_pursuit" exec="lateral_pure_pursuit_node" name="lateral_pure_pursuit" 
+      output="screen" unless="$(var use_stanley)">
+      <param name="wheel_base" value="1.09" />
+      <param name="lookahead_gain" value="0.24" />
+      <param name="lookahead_min_distance" value="2.0" />
+      <remap from="input/kinematics" to="/localization/kinematic_state" />
+      <remap from="input/trajectory" to="/planning/output/mppi_planned_path" />
+    </node>
+
+    <node pkg="lateral_stanley" exec="lateral_stanley_node" name="lateral_stanley" 
+      output="screen" if="$(var use_stanley)">
+      <param name="k_gain" value="2.0" />
+      <param name="k_gain_slow" value="1.0" />
+      <remap from="input/kinematics" to="/localization/kinematic_state" />
+      <remap from="input/trajectory" to="/planning/scenario_planning/trajectory" />
+    </node>
+
+    <!-- control unifier -->
+    <node pkg="ackermann_control_publisher" exec="ackermann_control_publisher_node" name="ackermann_control_publisher" output="screen">
+      <remap from="input/longitudinal" to="output/target_acc" />
+      <remap from="input/lateral" to="output/steer_angle" />
+      <remap from="output/ackermann_command" to="/control/command/control_cmd" />
     </node>
-    <!-- speed_proportional_gain: P gain for speed control -->
-    <!-- external_target_vel: Target speed -->
-    <!-- k_gain: Gain to determine additional steering based on speed -->
-    <!-- k_gain_slow: Constant to prevent zero division -->
   </group>
 </launch>

aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(ackermann_control_publisher)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+ament_auto_add_executable(ackermann_control_publisher_node
+  src/ackermann_control_publisher.cpp
+)
+
+ament_auto_package()

aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/include/ackermann_control_publisher/ackermann_control_publisher.hpp

@@ -0,0 +1,33 @@
+#include <rclcpp/rclcpp.hpp>
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <std_msgs/msg/float64.hpp>
+
+namespace ackermann_control_publisher {
+    using namespace std::literals::chrono_literals;
+    using std_msgs::msg::Float64;
+    using autoware_auto_control_msgs::msg::AckermannControlCommand;
+
+    class AckermannControlPublisher : public rclcpp::Node {
+        public:
+            explicit AckermannControlPublisher();
+
+            // subscribers
+            rclcpp::Subscription<Float64>::SharedPtr sub_longitudinal_;
+            rclcpp::Subscription<Float64>::SharedPtr sub_lateral_;
+
+            // publishers
+            rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_cmd_;
+
+            // timer for control
+            rclcpp::TimerBase::SharedPtr timer_;
+
+        private:
+
+            // control command
+            double longitudinal_acc_; // 目標加速度 [m/s^2]
+            double lateral_tire_angle_; // タイヤ舵角 [rad]
+
+            void onTimer();
+            bool subscribeMessageAvailable();
+    };
+} //namespace ackermann_control_publisher

aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/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>ackermann_control_publisher</name>
+  <version>0.0.1</version>
+  <description>Bundles individual lateral/longitudinal commands and publishes them as `AckermannControlCommand` message</description>
+  <maintainer email="[email protected]">Shotaro Itahara</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

aichallenge/workspace/src/aichallenge_submit/control/ackermann_control_publisher/src/ackermann_control_publisher.cpp

@@ -0,0 +1,55 @@
+#include "ackermann_control_publisher/ackermann_control_publisher.hpp"
+
+namespace ackermann_control_publisher{
+    AckermannControlPublisher::AckermannControlPublisher(): Node("ackermann_control_publisher"){
+
+        pub_cmd_ = create_publisher<AckermannControlCommand>("output/ackermann_command", 1);
+
+        sub_longitudinal_ = create_subscription<Float64>(
+            "input/longitudinal", 1, [this](const Float64::SharedPtr msg) { longitudinal_acc_ = msg->data; });
+        sub_lateral_ = create_subscription<Float64>(
+            "input/lateral", 1, [this](const Float64::SharedPtr msg) { lateral_tire_angle_ = msg->data; });
+
+        using namespace std::literals::chrono_literals;
+        timer_ =
+            rclcpp::create_timer(this, get_clock(), 5ms, std::bind(&AckermannControlPublisher::onTimer, this)); // 200Hz
+
+    }
+
+    void AckermannControlPublisher::onTimer(){
+        // check data
+        if (!subscribeMessageAvailable()) {
+            return;
+        }
+        rclcpp::Time stamp = get_clock()->now();
+
+        AckermannControlCommand cmd = AckermannControlCommand();
+        cmd.stamp = stamp;
+        cmd.longitudinal.stamp = stamp;
+        cmd.lateral.stamp = stamp;
+
+        cmd.longitudinal.acceleration = longitudinal_acc_;
+        cmd.lateral.steering_tire_angle = lateral_tire_angle_;
+
+        pub_cmd_->publish(cmd);
+    }
+
+    bool AckermannControlPublisher::subscribeMessageAvailable(){
+        if (!longitudinal_acc_) {
+            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available");
+            return false;
+        }
+        if (!lateral_tire_angle_) {
+            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available");
+            return false;
+        }
+        return true;
+    }
+}
+
+int main(int argc, char const * argv[]){
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ackermann_control_publisher::AckermannControlPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}

aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(lateral_pure_pursuit)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+ament_auto_add_executable(lateral_pure_pursuit_node
+  src/lateral_pure_pursuit.cpp
+)
+
+ament_auto_package()

aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/include/lateral_pure_pursuit/lateral_pure_pursuit.hpp

@@ -0,0 +1,81 @@
+// Copyright 2024 Booars
+//
+// 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 LATERAL_PURE_PURSUIT_HPP_
+#define LATERAL_PURE_PURSUIT_HPP_
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+
+#include <cmath>
+
+namespace lateral_pure_pursuit{
+    using autoware_auto_planning_msgs::msg::Trajectory;
+    using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+    using geometry_msgs::msg::Pose;
+    using geometry_msgs::msg::Twist;
+    using geometry_msgs::msg::Vector3Stamped;
+    using visualization_msgs::msg::Marker;
+    using std_msgs::msg::Float64;
+    using nav_msgs::msg::Odometry;
+
+    class LateralPurePursuit : public rclcpp::Node {
+        public:
+            explicit LateralPurePursuit();
+
+            // subscribers
+            rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
+            rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+
+            // publishers
+            rclcpp::Publisher<Float64>::SharedPtr pub_cmd_;
+            rclcpp::Publisher<Marker>::SharedPtr pub_marker_;
+
+            // timer for control
+            rclcpp::TimerBase::SharedPtr timer_;
+
+            // updated by subscribers
+            Trajectory::SharedPtr trajectory_;
+            Odometry::SharedPtr odometry_;
+
+
+
+
+        private:
+            void onTimer();
+            bool subscribeMessageAvailable();
+
+            //parameters
+            double wheel_base_;              // Distance between front and rear axle, needed for geometric control
+            double lookahead_gain_;          // Determines how the lookahead distance grows in relationship with speed
+            double lookahead_min_distance_;  // Minimum lookahead distance, any lookahead distance will be built on top of this
+            double extra_steering_gain_;     // Duct tape fix constant to account for lack of steering motor power
+
+            OnSetParametersCallbackHandle::SharedPtr reset_param_handler_;
+            rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector<rclcpp::Parameter> &parameters);
+    };
+} // namespace lateral_pure_pursuit
+
+
+#endif //LATERAL_PURE_PURSUIT_HPP_

aichallenge/workspace/src/aichallenge_submit/control/lateral/lateral_pure_pursuit/package.xml

@@ -0,0 +1,23 @@
+<?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>lateral_pure_pursuit</name>
+  <version>0.0.1</version>
+  <description>Pure pursuit control, takes desired trajectory and odometry, returns steering angle only</description>
+  <maintainer email="[email protected]">Shotaro Itahara</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>motion_utils</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>