80 crptg1prioc stanley and pp implementation

.gitignore

@@ -2,4 +2,4 @@ logs/
 build/
 install/
 
-.vscode/
+.vscode/

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(ctrl_vehicle_control_lat_pure_p)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(autoware_control_msgs REQUIRED)
+find_package(autoware_planning_msgs REQUIRED)
+find_package(autoware_vehicle_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pacmod3_msgs REQUIRED)
+find_package(crp_msgs REQUIRED)
+
+
+include_directories(include)
+
+
+add_executable(ctrl_vehicle_control_lat_pure_p src/ctrl_vehicle_control_lat_pure_p.cpp)
+
+ament_target_dependencies(ctrl_vehicle_control_lat_pure_p  rclcpp geometry_msgs std_msgs autoware_control_msgs autoware_planning_msgs autoware_vehicle_msgs nav_msgs pacmod3_msgs crp_msgs)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS
+ctrl_vehicle_control_lat_pure_p
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/README.md

@@ -0,0 +1,27 @@
+# Ctrl Vehicle Control
+
+A package for trajectory following, Lateral and Longitudinal Control of the Vehicle in the APL layer. 
+
+## APL level Controller of the Vehicle
+
+The Controller uses a PID controller, with a Feedback and FeedForward layer, the error in the Feedback loop is based on lateral and angle errror. 
+
+### Usage:
+
+```
+ ros2 launch ctrl_vehicle_control ctrl_vehicle_control.launch.py
+```
+
+### Parameters
+| Name             | Type   | Default value                                   |
+|------------------|--------|-------------------------------------------------|
+| /ctrl/ffGainOffsetGround    | double | 1.0                                             |
+| /ctrl/ffGainSlope | double | 0.1 |
+| /ctrl/ffLookAheadTime | double | 1.5
+| /ctrl/steeringAngleLPFilter | double | 0.7
+| /ctrl/fbLookAheadTime | double | 0.5
+| /ctrl/fbPGain | double | 0.01
+| /ctrl/fbDGain | double | 0.0
+| /ctrl/fbIGain | double | 0.0
+| /ctrl/fbMinLookAheadDistance | double | 0.0
+| /ctrl/fbIntegralLimit | double | 3.0  

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/include/ctrl_vehicle_control_lat_pure_p/controller_inputs.hpp

@@ -0,0 +1,31 @@
+#ifndef CONTROLLER_INPUTS_HPP_
+#define CONTROLLER_INPUTS_HPP_
+
+
+#include <stdio.h>
+#include <string>
+#include <vector>
+
+
+namespace crp
+{
+    namespace apl
+    {
+        struct ControlInput{
+            std::vector<double> path_x;
+            std::vector<double> path_y;
+            std::vector<double> path_theta;
+            double target_speed{0.0f};
+            double egoPoseGlobal [3]{0.0f};
+            double vxEgo{0.0f};
+            double egoSteeringAngle{0.0f};
+        };
+
+        struct ControlParams{
+            double lookahead_time{1.0f};
+            double wheelbase{2.9f};
+        };
+    }
+}
+
+#endif

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/include/ctrl_vehicle_control_lat_pure_p/controller_outputs.hpp

@@ -0,0 +1,20 @@
+#ifndef CONTROLLER_OUTPUTS_HPP_
+#define CONTROLLER_OUTPUTS_HPP_
+
+#include <stdio.h>
+#include <string>
+#include <vector>
+
+
+namespace crp
+{
+    namespace apl
+    {
+        struct ControlOutput{
+            double accelerationTarget{0.0f};
+            double steeringAngleTarget{0.0f};
+        };
+    }
+}
+
+#endif

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/include/ctrl_vehicle_control_lat_pure_p/ctrl_vehicle_control_lat_pure_p.hpp

@@ -0,0 +1,52 @@
+#ifndef CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_HPP
+#define CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_HPP
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <math.h>
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "std_msgs/msg/float32_multi_array.hpp"
+#include "std_msgs/msg/float32.hpp"
+#include "crp_msgs/msg/ego.hpp"
+
+#include "autoware_control_msgs/msg/control.hpp"
+#include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "autoware_vehicle_msgs/msg/steering_report.hpp"
+
+#include "ctrl_vehicle_control_lat_pure_p/controller_inputs.hpp"
+#include "ctrl_vehicle_control_lat_pure_p/controller_outputs.hpp"
+
+namespace crp
+{
+    namespace apl
+    {
+        class CtrlVehicleControlLat : public rclcpp::Node
+        {
+            public:
+                CtrlVehicleControlLat();
+
+            private:
+                // VARIABLES
+                crp::apl::ControlInput m_input;
+                crp::apl::ControlOutput m_output;
+                crp::apl::ControlParams m_params;
+
+                void trajCallback(const autoware_planning_msgs::msg::Trajectory input_msg);
+                void egoVehicleCallback(const crp_msgs::msg::Ego input_msg);
+
+                void pure_p_control();
+                void loop();
+
+                rclcpp::TimerBase::SharedPtr timer_;
+                rclcpp::Publisher<autoware_control_msgs::msg::Lateral>::SharedPtr m_pub_cmd;
+
+                rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr m_traj_sub_;
+                rclcpp::Subscription<crp_msgs::msg::Ego>::SharedPtr m_egoVehicle_sub_;
+                autoware_control_msgs::msg::Lateral m_ctrlCmdMsg;
+        }; 
+    } // namespace apl
+}
+#endif // CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_HPP

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/launch/ctrl_vehicle_control_lat_pure_p.launch.py

@@ -0,0 +1,18 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    ctrl_vehicle_control_lat_pure_p = Node(
+        package="ctrl_vehicle_control_lat_pure_p",
+        executable="ctrl_vehicle_control_lat_pure_p",
+        parameters=[
+            {"/ctrl/lookahead_time": 1.0},
+            {"/ctrl/wheelbase": 2.7},
+        ]
+    )
+
+    ld.add_action(ctrl_vehicle_control_lat_pure_p)
+
+    return ld

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/package.xml

@@ -0,0 +1,29 @@
+<?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>ctrl_vehicle_control_lat_pure_p</name>
+  <version>0.0.0</version>
+  <description>Vehicle Control Package with Stanley lateral controller</description>
+  <maintainer email="[email protected]">mesmatyi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>pacmod3_msgs</depend>
+  <depend>crp_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

crp_APL/controllers/ctrl_vehicle_control_lat_pure_p/src/ctrl_vehicle_control_lat_pure_p.cpp

@@ -0,0 +1,106 @@
+#include <ctrl_vehicle_control_lat_pure_p/ctrl_vehicle_control_lat_pure_p.hpp>
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+
+crp::apl::CtrlVehicleControlLat::CtrlVehicleControlLat() : Node("CtrlVehicleControlLatPurePursuit")
+{
+    timer_ = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&CtrlVehicleControlLat::loop, this));  
+    m_pub_cmd = this->create_publisher<autoware_control_msgs::msg::Lateral>("/control/command/control_cmdLat", 30);
+
+    m_traj_sub_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>("/plan/trajectory", 10, std::bind(&CtrlVehicleControlLat::trajCallback, this, std::placeholders::_1));
+    m_egoVehicle_sub_ = this->create_subscription<crp_msgs::msg::Ego>("/ego", 10, std::bind(&CtrlVehicleControlLat::egoVehicleCallback, this, std::placeholders::_1));
+
+    this->declare_parameter("/ctrl/lookahead_time", 1.0f);
+    this->declare_parameter("/ctrl/wheelbase", 2.7f);
+
+    RCLCPP_INFO(this->get_logger(), "ctrl_vehicle_control has been started");
+}
+
+void crp::apl::CtrlVehicleControlLat::trajCallback(const autoware_planning_msgs::msg::Trajectory input_msg)
+{
+    m_input.path_x.clear();
+    m_input.path_y.clear();
+    m_input.path_theta.clear();
+    double quaternion[4];
+
+
+    // this callback maps the input trajectory to the internal interface
+    for (long unsigned int i=0; i<input_msg.points.size(); i++)
+    {
+        m_input.path_x.push_back(input_msg.points.at(i).pose.position.x);
+        m_input.path_y.push_back(input_msg.points.at(i).pose.position.y);
+    }
+
+    if (input_msg.points.size() > 0)
+        m_input.target_speed = input_msg.points.at(0).longitudinal_velocity_mps;
+    else
+        m_input.target_speed = 0.0f;
+
+}
+
+void crp::apl::CtrlVehicleControlLat::egoVehicleCallback(const crp_msgs::msg::Ego input_msg)
+{
+    m_input.vxEgo = input_msg.twist.twist.linear.x;
+    m_input.egoSteeringAngle = input_msg.tire_angle_front;
+
+    m_input.egoPoseGlobal[0] = input_msg.pose.pose.position.x;
+    m_input.egoPoseGlobal[1] = input_msg.pose.pose.position.y;
+}
+
+void crp::apl::CtrlVehicleControlLat::pure_p_control()
+{
+
+    float distance_to_index = m_input.vxEgo * m_params.lookahead_time;
+
+    RCLCPP_INFO(this->get_logger(), "distance to index: %f", distance_to_index);
+
+    float current_distance = 0.0f;
+    int target_index = 0;
+
+    for (int i = 0; i < m_input.path_x.size()-1; i++)
+    {
+        current_distance += sqrt(pow(m_input.path_x.at(i+1) - m_input.path_x.at(i), 2) + pow(m_input.path_y.at(i+1) - m_input.path_y.at(i), 2));
+        if (current_distance > distance_to_index)
+        {
+            target_index = i;
+            break;
+        }
+    }
+
+    // calculate the steering angle
+    m_output.steeringAngleTarget = 0.0f;
+
+    float alpha = atan2(m_input.path_y[target_index], m_input.path_x[target_index]);
+
+    m_output.steeringAngleTarget = atan2(2.0f * m_params.wheelbase * sin(alpha) / distance_to_index, 1);
+
+}
+
+void crp::apl::CtrlVehicleControlLat::loop()
+{
+    // parameter assignments
+    m_params.lookahead_time = this->get_parameter("/ctrl/lookahead_time").as_double();
+    m_params.wheelbase = this->get_parameter("/ctrl/wheelbase").as_double();
+
+    // control algorithm
+    if(m_input.path_x.size() > 0 && m_input.path_y.size() > 0)
+        pure_p_control();
+
+    // steering angle and steering angle gradiant
+    m_ctrlCmdMsg.stamp = this->now();
+    m_ctrlCmdMsg.steering_tire_angle = m_output.steeringAngleTarget;
+    m_ctrlCmdMsg.steering_tire_rotation_rate = 0.0f;
+
+    m_pub_cmd->publish(m_ctrlCmdMsg);
+}
+
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<crp::apl::CtrlVehicleControlLat>());
+    rclcpp::shutdown();
+    return 0;
+}

crp_APL/controllers/ctrl_vehicle_control_lat_stanley/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(ctrl_vehicle_control_lat_stanley)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(autoware_control_msgs REQUIRED)
+find_package(autoware_planning_msgs REQUIRED)
+find_package(autoware_vehicle_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pacmod3_msgs REQUIRED)
+find_package(crp_msgs REQUIRED)
+
+
+include_directories(include)
+
+
+add_executable(ctrl_vehicle_control_lat_stanley src/ctrl_vehicle_control_lat_stanley.cpp)
+
+ament_target_dependencies(ctrl_vehicle_control_lat_stanley  rclcpp geometry_msgs std_msgs autoware_control_msgs autoware_planning_msgs autoware_vehicle_msgs nav_msgs pacmod3_msgs crp_msgs)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS
+ctrl_vehicle_control_lat_stanley
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_package()