From 7851622b11ebc1f9ae74af86098416ece79049c6 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Sat, 25 Oct 2025 16:01:58 +0200 Subject: [PATCH 01/25] feat: create can --- src/arussim/launch/arussim_launch.py | 41 ++++++++++++++++++++++++++-- 1 file changed, 39 insertions(+), 2 deletions(-) diff --git a/src/arussim/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index c90f8615..dfdae30c 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -1,7 +1,7 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess +from launch.actions import ExecuteProcess, TimerAction from launch_ros.actions import Node @@ -12,8 +12,45 @@ def generate_launch_description(): 'config', 'arussim_rviz_config.rviz' ) + modprobe_vcan = ExecuteProcess( + cmd=['sudo','modprobe', 'vcan'], + shell=False + ) + create_can0 = TimerAction( + period=1.0, + actions=[ + ExecuteProcess( + cmd=['sudo','ip', 'link', 'add', 'dev', 'can0', 'type', 'vcan'], + shell=False + ) + ] + ) + up_can0 = TimerAction( + period=1.5, + actions=[ + ExecuteProcess( + cmd=['sudo','ip', 'link', 'set', 'up', 'can0'], + shell=False + ) + ] + ) + create_can1 = ExecuteProcess( + cmd=['sudo','ip', 'link', 'add', 'dev', 'can1', 'type', 'vcan'], + shell=False + ) + up_can1 = ExecuteProcess( + cmd=['sudo','ip', 'link', 'set', 'up', 'can1'], + shell=False + ) + + return LaunchDescription([ + modprobe_vcan, + create_can0, + up_can0, + create_can1, + up_can1, Node( package='rviz2', executable='rviz2', @@ -30,7 +67,7 @@ def generate_launch_description(): config='sensors_config.yaml'), create_node(pkg='arussim', exec='supervisor_exec', - config='supervisor_config.yaml') + config='supervisor_config.yaml'), ]) From 243d332d5c463df3d946cd81224cfe12710cfb7d Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Sat, 25 Oct 2025 17:43:17 +0200 Subject: [PATCH 02/25] feat: send sensors by can --- src/arussim/include/arussim/sensors.hpp | 44 ++++-- src/arussim/src/sensors.cpp | 197 +++++++++++++++++------- 2 files changed, 175 insertions(+), 66 deletions(-) diff --git a/src/arussim/include/arussim/sensors.hpp b/src/arussim/include/arussim/sensors.hpp index 4e9eff0f..9083db74 100644 --- a/src/arussim/include/arussim/sensors.hpp +++ b/src/arussim/include/arussim/sensors.hpp @@ -14,6 +14,12 @@ #include "arussim_msgs/msg/four_wheel_drive.hpp" #include "std_msgs/msg/float32.hpp" #include +#include +#include +#include +#include +#include + /** * @class Sensors @@ -28,6 +34,21 @@ class Sensors : public rclcpp::Node */ Sensors(); // Constructor + //Define CAN frame and signal structures + struct CanSignal { + int bit_in; + int bit_fin; + bool is_signed; + double scale; + double offset; + }; + struct CanFrame { + uint32_t id; + int size; + std::map signals; + }; + static std::vector frames; + private: // Variables double x_ = 0; @@ -91,12 +112,6 @@ class Sensors : public rclcpp::Node */ void extensometer_timer(); - /** - * @brief Timer function for the wheel speed sensors - * - */ - void wheel_speed_timer(); - /** * @brief Timer function for the IMU * @@ -107,9 +122,11 @@ class Sensors : public rclcpp::Node * @brief Timer function for the 4WD torque * */ - void torque_cmd_timer(); - + void inverter_timer(); + static uint64_t encode_signal(double value, double scale, double offset, int bit_len, bool is_signed); + void send_can_frame(const CanFrame &frame, const std::map &values); + // ROS Communication rclcpp::Subscription::SharedPtr state_sub_; // State subscriber @@ -122,11 +139,18 @@ class Sensors : public rclcpp::Node rclcpp::Publisher::SharedPtr ws_fl_pub_; // Wheel speed publisher rclcpp::Publisher::SharedPtr ws_rr_pub_; // Wheel speed publisher rclcpp::Publisher::SharedPtr ws_rl_pub_; // Wheel speed publisher - rclcpp::TimerBase::SharedPtr ws_timer_; // Wheel speed timer rclcpp::Publisher::SharedPtr ext_pub_; // Extensometer publisher rclcpp::TimerBase::SharedPtr ext_timer_; // Extensometer timer rclcpp::Publisher::SharedPtr torque_pub_; // Torque publisher - rclcpp::TimerBase::SharedPtr torque_timer_; // Torque timer + rclcpp::TimerBase::SharedPtr inverter_timer_; // Inverter timer + + //CAN Communication + int can_socket_; + struct ifreq ifr_{}; + struct sockaddr_can addr_{}; + struct can_frame canMsg_{}; + + }; \ No newline at end of file diff --git a/src/arussim/src/sensors.cpp b/src/arussim/src/sensors.cpp index b882de32..46efb79d 100644 --- a/src/arussim/src/sensors.cpp +++ b/src/arussim/src/sensors.cpp @@ -7,7 +7,7 @@ * */ #include "arussim/sensors.hpp" - +std::vector Sensors::frames; /** * @class Sensors * @brief Sensors class for ARUSsim @@ -61,6 +61,13 @@ Sensors::Sensors() : Node("sensors") this->get_parameter("extensometer.extensometer_frequency", kExtensometerFrequency); this->get_parameter("extensometer.noise_extensometer", kNoiseExtensometer); + //CAN Socket setup + can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + std::strcpy(ifr_.ifr_name, "can0"); + ioctl(can_socket_, SIOCGIFINDEX, &ifr_); + addr_.can_family = AF_CAN; + addr_.can_ifindex = ifr_.ifr_ifindex; + bind(can_socket_, (struct sockaddr *)&addr_, sizeof(addr_)); // State subscriber state_sub_ = this->create_subscription( @@ -78,21 +85,6 @@ Sensors::Sensors() : Node("sensors") std::bind(&Sensors::imu_timer, this) ); - // Wheel speed - ws_fr_pub_ = this->create_publisher( - "/arussim/fr_wheel_speed", 10); - ws_fl_pub_ = this->create_publisher( - "/arussim/fl_wheel_speed", 10); - ws_rr_pub_ = this->create_publisher( - "/arussim/rr_wheel_speed", 10); - ws_rl_pub_ = this->create_publisher( - "/arussim/rl_wheel_speed", 10); - - ws_timer_ = this->create_wall_timer( - std::chrono::milliseconds((int)(1000/kWheelSpeedFrequency)), - std::bind(&Sensors::wheel_speed_timer, this) - ); - // Extensometer ext_pub_ = this->create_publisher("/arussim/extensometer", 10); @@ -101,13 +93,53 @@ Sensors::Sensors() : Node("sensors") std::bind(&Sensors::extensometer_timer, this) ); - // Torque cmd + // Inverter torque_pub_ = this->create_publisher("/arussim/torque4WD", 10); - torque_timer_ = this->create_wall_timer( + inverter_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1000/kTorqueFrequency)), - std::bind(&Sensors::torque_cmd_timer, this) + std::bind(&Sensors::inverter_timer, this) ); + ws_fr_pub_ = this->create_publisher( + "/arussim/fr_wheel_speed", 10); + ws_fl_pub_ = this->create_publisher( + "/arussim/fl_wheel_speed", 10); + ws_rr_pub_ = this->create_publisher( + "/arussim/rr_wheel_speed", 10); + ws_rl_pub_ = this->create_publisher( + "/arussim/rl_wheel_speed", 10); + + frames = { + {0x1A3, 4, { // IMU ax, ay + {"IMU/ax", {0, 15, true, 0.02, 0.0}}, + {"IMU/ay", {16, 31, true, 0.02, 0.0}} + }}, + {0x1A4, 2, { // IMU yaw_rate + {"IMU/yaw_rate", {0, 15, true, 0.000349065, 0.0}} + }}, + {0x134, 2, { // Extensometer + {"extensometer", {0, 15, true, -0.000031688042484, 0.476959989071}} + }}, + {0x102, 5, { // Front Left inverter + {"fl_inv_speed", {0, 23, true, 0.00000018879763543, 0.0}}, + {"fl_inv_torque", {24, 39, true, 0.0098, 0.0}} + }}, + {0x106, 5, { // Front Right inverter + {"fr_inv_speed", {0, 23, true, 0.00000018879763543, 0.0}}, + {"fr_inv_torque", {24, 39, true, 0.0098, 0.0}} + }}, + {0x110, 5, { // Rear Left inverter + {"rl_inv_speed", {0, 23, true, 0.00000018879763543, 0.0}}, + {"rl_inv_torque", {24, 39, true, 0.0098, 0.0}} + }}, + {0x114, 5, { // Rear Right inverter + {"rr_inv_speed", {0, 23, true, 0.00000018879763543, 0.0}}, + {"rr_inv_torque", {24, 39, true, 0.0098, 0.0}} + }}, + {0x161, 2, { + {"as_status", {8, 15, false, 1.0, 0.0}} + }} + }; } /** @@ -131,11 +163,21 @@ void Sensors::state_callback(const arussim_msgs::msg::State::SharedPtr msg) torque_cmd = msg->torque; } +//Function to encode a signal into a CAN frame +uint64_t Sensors::encode_signal(double value, double scale, double offset, int bit_len, bool /*is_signed*/) +{ + double raw_f_ = (value - offset) / scale; + int64_t raw_i_ = (int64_t) std::round(raw_f_); + uint64_t mask_ = (1ULL << (bit_len)) - 1; + raw_i_ &= mask_; + return (uint64_t)raw_i_; +} + /** * @brief Timer function for the 4WD torque * */ -void Sensors::torque_cmd_timer(){ +void Sensors::inverter_timer(){ // Random noise generation with different noise for each wheel speed std::random_device rd; std::mt19937 gen(rd()); @@ -162,6 +204,47 @@ void Sensors::torque_cmd_timer(){ // Publish the torque message torque_pub_->publish(message); + + // Random noise generation with different noise for each wheel speed + std::normal_distribution<> dist_front_right(0.0, kNoiseWheelSpeedFrontRight); + std::normal_distribution<> dist_front_left(0.0, kNoiseWheelSpeedFrontLeft); + std::normal_distribution<> dist_rear_right(0.0, kNoiseWheelSpeedRearRight); + std::normal_distribution<> dist_rear_left(0.0, kNoiseWheelSpeedRearLeft); + + // Apply noise to the state variables + wheel_speed_.fr_ = wheel_speed.front_right * 0.225 + dist_front_right(gen); + wheel_speed_.fl_ = wheel_speed.front_left * 0.225 + dist_front_left(gen); + wheel_speed_.rr_ = wheel_speed.rear_right * 0.225 + dist_rear_right(gen); + wheel_speed_.rl_ = wheel_speed.rear_left * 0.225 + dist_rear_left(gen); + + // Create the wheel speed message + auto msg_fr = std_msgs::msg::Float32(); + auto msg_fl = std_msgs::msg::Float32(); + auto msg_rr = std_msgs::msg::Float32(); + auto msg_rl = std_msgs::msg::Float32(); + + msg_fr.data = wheel_speed_.fr_; + msg_fl.data = wheel_speed_.fl_; + msg_rr.data = wheel_speed_.rr_; + msg_rl.data = wheel_speed_.rl_; + + // Publish the torque message + ws_fr_pub_->publish(msg_fr); + ws_fl_pub_->publish(msg_fl); + ws_rr_pub_->publish(msg_rr); + ws_rl_pub_->publish(msg_rl); + + //Send Inverter CAN frames + std::map values = { {"fl_inv_speed", wheel_speed_.fl_}, {"fl_inv_torque", torque_cmd_.fl_ }, + {"fr_inv_speed", wheel_speed_.fr_}, {"fr_inv_torque", torque_cmd_.fr_}, + {"rl_inv_speed", wheel_speed_.rl_}, {"rl_inv_torque", torque_cmd_.rl_}, + {"rr_inv_speed", wheel_speed_.rr_}, {"rr_inv_torque", torque_cmd_.rr_} + }; + + for (auto &frame : frames) { if (frame.id == 0x102 || frame.id == 0x106 || frame.id == 0x110 || frame.id == 0x114) { + send_can_frame(frame, values); + } +} } /** @@ -194,46 +277,20 @@ void Sensors::imu_timer() ax_pub_->publish(msg_ax); ay_pub_->publish(msg_ay); r_pub_->publish(msg_r); + + //Send CAN frames for IMU (GSS ids) + std::map values = { {"IMU/ax", ax_ + dist_ax(gen)}, + {"IMU/ay", ay_ + dist_ay(gen)}, {"IMU/yaw_rate", r_ + dist_r(gen)} }; + + for (auto &frame : frames) { + if (frame.id == 0x1A3 || frame.id == 0x1A4) { + send_can_frame(frame, values); + } } -/** - * @brief Timer function for the wheel speed sensors - * - */ -void Sensors::wheel_speed_timer() -{ - // Random noise generation with different noise for each wheel speed - std::random_device rd; - std::mt19937 gen(rd()); - std::normal_distribution<> dist_front_right(0.0, kNoiseWheelSpeedFrontRight); - std::normal_distribution<> dist_front_left(0.0, kNoiseWheelSpeedFrontLeft); - std::normal_distribution<> dist_rear_right(0.0, kNoiseWheelSpeedRearRight); - std::normal_distribution<> dist_rear_left(0.0, kNoiseWheelSpeedRearLeft); - - // Apply noise to the state variables - wheel_speed_.fr_ = wheel_speed.front_right * 0.225 + dist_front_right(gen); - wheel_speed_.fl_ = wheel_speed.front_left * 0.225 + dist_front_left(gen); - wheel_speed_.rr_ = wheel_speed.rear_right * 0.225 + dist_rear_right(gen); - wheel_speed_.rl_ = wheel_speed.rear_left * 0.225 + dist_rear_left(gen); - - // Create the wheel speed message - auto msg_fr = std_msgs::msg::Float32(); - auto msg_fl = std_msgs::msg::Float32(); - auto msg_rr = std_msgs::msg::Float32(); - auto msg_rl = std_msgs::msg::Float32(); - - msg_fr.data = wheel_speed_.fr_; - msg_fl.data = wheel_speed_.fl_; - msg_rr.data = wheel_speed_.rr_; - msg_rl.data = wheel_speed_.rl_; - - // Publish the torque message - ws_fr_pub_->publish(msg_fr); - ws_fl_pub_->publish(msg_fl); - ws_rr_pub_->publish(msg_rr); - ws_rl_pub_->publish(msg_rl); } + /** * @brief Timer function for the extensometer * @@ -249,6 +306,34 @@ void Sensors::extensometer_timer() auto message = std_msgs::msg::Float32(); message.data = delta_ + dist(gen); ext_pub_->publish(message); + + //Send CAN frame for extensometer + std::map values = { {"extensometer", delta_ + dist(gen)} }; + + for (auto &frame : frames) { + if (frame.id == 0x134) { + send_can_frame(frame, values); + } + } +} + +void Sensors::send_can_frame(const CanFrame &frame, const std::map &values) { + canMsg_.can_id = frame.id; + canMsg_.can_dlc = frame.size; + std::memset(canMsg_.data, 0, sizeof(canMsg_.data)); + for (auto &sig_pair : frame.signals) { + const auto &topic = sig_pair.first; + const auto &sig = sig_pair.second; + int bit_len = sig.bit_fin - sig.bit_in + 1; + uint64_t raw = encode_signal(values.at(topic), sig.scale, sig.offset, bit_len, sig.is_signed); + int byte_idx = sig.bit_in / 8; + int bit_offset = sig.bit_in % 8; + canMsg_.data[byte_idx] |= (raw << bit_offset) & 0xFF; + if (bit_len + bit_offset > 8 && byte_idx + 1 < frame.size) { + canMsg_.data[byte_idx + 1] |= (raw >> (8 - bit_offset)) & 0xFF; + } + } + write(can_socket_, &canMsg_, sizeof(canMsg_)); } /** From d2459f76afca7422e28841f735867dcfeacfa01c Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Mon, 27 Oct 2025 20:15:45 +0100 Subject: [PATCH 03/25] feat: recieve cmd by can --- src/arussim/include/arussim/arussim_node.hpp | 45 +++++++------- src/arussim/src/arussim_node.cpp | 65 ++++++++++---------- 2 files changed, 53 insertions(+), 57 deletions(-) diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index bd146cc9..4fdf49c2 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -47,6 +47,13 @@ #include "controller_sim/traction_control.hpp" #include "controller_sim/torque_vectoring.hpp" +#include +#include +#include +#include +#include + + /** * @class Simulator * @brief The Simulator class is responsible for simulating the vehicle dynamics, @@ -98,13 +105,16 @@ class Simulator : public rclcpp::Node double kCOGBackDist; double kCarWidth; + //Ros2 rclcpp::Clock::SharedPtr clock_; rclcpp::Time time_last_cmd_; - double input_acc_; - double input_delta_; - double target_r_; std::vector torque_cmd_; + //Can + float can_acc_; + float can_target_r_; + float can_delta_; + visualization_msgs::msg::Marker marker_; pcl::PointCloud track_; arussim_msgs::msg::Trajectory fixed_trajectory_msg_; @@ -156,15 +166,6 @@ class Simulator : public rclcpp::Node */ void on_fast_timer(); - /** - * @brief Callback for receiving control commands. - * - * This method processes incoming control commands (acceleration and steering angle) - * to update the vehicle's dynamics. - * - * @param msg The control command message. - */ - void cmd_callback(const arussim_msgs::msg::Cmd::SharedPtr msg); /** * @brief Callback for receiving teleportation commands from RViz. @@ -175,14 +176,6 @@ class Simulator : public rclcpp::Node */ void rviz_telep_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - /** - * @brief Callback for receiving EBS (Emergency Brake System) commands. - * - * This method processes EBS commands to apply emergency braking if necessary. - * - * @param msg The EBS command message. - */ - void ebs_callback(const std_msgs::msg::Bool::SharedPtr msg); /** * @brief Callback for receiving reset commands. @@ -225,11 +218,11 @@ class Simulator : public rclcpp::Node */ void cone_visualization(); + void receive_can(); + rclcpp::TimerBase::SharedPtr slow_timer_; rclcpp::TimerBase::SharedPtr fast_timer_; rclcpp::TimerBase::SharedPtr controller_sim_timer_; - rclcpp::Subscription::SharedPtr cmd_sub_; - rclcpp::Subscription::SharedPtr ebs_sub_; rclcpp::Subscription::SharedPtr rviz_telep_sub_; rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Publisher::SharedPtr control_vx_pub_; @@ -246,5 +239,11 @@ class Simulator : public rclcpp::Node std::shared_ptr tf_broadcaster_; rclcpp::Subscription::SharedPtr reset_sub_; rclcpp::Subscription::SharedPtr circuit_sub_; - + rclcpp::TimerBase::SharedPtr receive_can_timer_; + + //CAN Communication + int can_socket_; + struct ifreq ifr_{}; + struct sockaddr_can addr_{}; + struct can_frame frame_; }; \ No newline at end of file diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index 708930b8..73dd8823 100755 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -98,15 +98,14 @@ Simulator::Simulator() : Node("simulator") controller_sim_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1000/kControllerRate)), std::bind(&Simulator::on_controller_sim_timer, this)); + receive_can_timer_ = this->create_wall_timer( + std::chrono::milliseconds((int)(1000/kStateUpdateRate)), + std::bind(&Simulator::receive_can, this)); - cmd_sub_ = this->create_subscription("/arussim/cmd", 1, - std::bind(&Simulator::cmd_callback, this, std::placeholders::_1)); circuit_sub_ = this->create_subscription("/arussim/circuit", 1, std::bind(&Simulator::load_track, this, std::placeholders::_1)); rviz_telep_sub_ = this->create_subscription( "/initialpose", 1, std::bind(&Simulator::rviz_telep_callback, this, std::placeholders::_1)); - ebs_sub_ = this->create_subscription( - "/arussim_interface/EBS", 1, std::bind(&Simulator::ebs_callback, this, std::placeholders::_1)); reset_sub_ = this->create_subscription("/arussim/reset", 1, std::bind(&Simulator::reset_callback, this, std::placeholders::_1)); @@ -127,13 +126,21 @@ Simulator::Simulator() : Node("simulator") marker_.color.b = 70.0/255.0; marker_.color.a = 1.0; marker_.lifetime = rclcpp::Duration::from_seconds(0.0); + + //CAN Socket setup + can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + std::strcpy(ifr_.ifr_name, "can0"); + ioctl(can_socket_, SIOCGIFINDEX, &ifr_); + addr_.can_family = AF_CAN; + addr_.can_ifindex = ifr_.ifr_ifindex; + bind(can_socket_, (struct sockaddr *)&addr_, sizeof(addr_)); // Set controller_sim period and GSS usage controller_sim_.init(1/kControllerRate, kUseGSS); // Initialize torque variable torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; - + // Set CSV file if (kCSVState) { csv_generator_state_ = std::make_shared("state"); @@ -339,7 +346,7 @@ void Simulator::on_controller_sim_timer() { ); // Torque command calculation - torque_cmd_ = controller_sim_.get_torque_cmd(input_acc_, target_r_); + torque_cmd_ = controller_sim_.get_torque_cmd(can_acc_, can_target_r_); // Publish control estimation std_msgs::msg::Float32 control_vx_msg; @@ -367,12 +374,12 @@ void Simulator::on_fast_timer() rclcpp::Time current_time = clock_->now(); if((current_time - time_last_cmd_).seconds() > 0.2 && vehicle_dynamics_.vx_ != 0) { - input_acc_ = 0; + can_acc_ = 0; } double dt = 1.0 / kStateUpdateRate; - vehicle_dynamics_.update_simulation(input_delta_, torque_cmd_, dt); + vehicle_dynamics_.update_simulation(can_delta_, torque_cmd_, dt); if(use_tpl_){ check_lap(); @@ -434,38 +441,28 @@ void Simulator::on_fast_timer() marker_pub_->publish(marker_); } -/** - * @brief Callback for receiving control commands. - * - * This method updates the vehicle's acceleration and steering angle based on - * incoming commands. - * - * @param msg Incoming control command message. - */ -void Simulator::cmd_callback(const arussim_msgs::msg::Cmd::SharedPtr msg) + void Simulator::receive_can() { - input_acc_ = msg->acc; - input_delta_ = msg->delta; - target_r_ = msg->target_r; - time_last_cmd_ = clock_->now(); -} + + read(can_socket_, &frame_, sizeof(struct can_frame)); -void Simulator::ebs_callback(const std_msgs::msg::Bool::SharedPtr msg) -{ - if (msg->data) { - input_acc_ = 0.0; - input_delta_ = 0.0; - torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; - vehicle_dynamics_.vx_ = 0.0; - vehicle_dynamics_.vy_ = 0.0; - vehicle_dynamics_.r_ = 0.0; - } + if (frame_.can_id == 0x222) { + int16_t acc_scaled = static_cast((frame_.data[1] << 8) | frame_.data[0]); + int16_t yaw_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); + int16_t delta_scaled = static_cast((frame_.data[5] << 8) | frame_.data[4]); + + can_acc_ = static_cast(acc_scaled) / 100.0f; + can_target_r_ = static_cast(yaw_scaled) / 1000.0f; + can_delta_ = static_cast(delta_scaled) / 100.0f; + time_last_cmd_ = clock_->now(); + } } + void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) { - input_acc_ = 0.0; - input_delta_ = 0.0; + can_acc_ = 0.0; + can_delta_ = 0.0; torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; vehicle_dynamics_ = VehicleDynamics(); From c2a7dc0af204f24cd79436f5110e94915b149b2e Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Wed, 29 Oct 2025 17:13:43 +0100 Subject: [PATCH 04/25] feat: control dummy --- src/arussim/CMakeLists.txt | 11 ++ src/arussim/config/control_sim_config.yaml | 5 + src/arussim/include/arussim/arussim_node.hpp | 2 + .../arussim/control_sim/control_sim.hpp | 62 ++++++++ src/arussim/launch/arussim_launch.py | 4 + src/arussim/src/arussim_node.cpp | 33 ++++- src/arussim/src/control_sim/control_sim.cpp | 137 ++++++++++++++++++ 7 files changed, 246 insertions(+), 8 deletions(-) create mode 100644 src/arussim/config/control_sim_config.yaml create mode 100644 src/arussim/include/arussim/control_sim/control_sim.hpp create mode 100644 src/arussim/src/control_sim/control_sim.cpp diff --git a/src/arussim/CMakeLists.txt b/src/arussim/CMakeLists.txt index 2c2a2b4e..f2008ffe 100755 --- a/src/arussim/CMakeLists.txt +++ b/src/arussim/CMakeLists.txt @@ -117,11 +117,22 @@ ament_target_dependencies(supervisor_exec rclcpp tf2_ros tf2) +# generate executable for control_sim.cpp +add_executable(control_sim_exec src/control_sim/control_sim.cpp) +ament_target_dependencies(control_sim_exec rclcpp + std_msgs + geometry_msgs + sensor_msgs + arussim_msgs + tf2_ros + tf2) + # Install both executables install(TARGETS arussim_exec sensors_exec supervisor_exec + control_sim_exec DESTINATION lib/${PROJECT_NAME}) # share folders location diff --git a/src/arussim/config/control_sim_config.yaml b/src/arussim/config/control_sim_config.yaml new file mode 100644 index 00000000..5557507e --- /dev/null +++ b/src/arussim/config/control_sim_config.yaml @@ -0,0 +1,5 @@ +arussim: + ros__parameters: + mass: 250.0 + rdyn: 0.225 + gear_ratio: 12.48 \ No newline at end of file diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index 4fdf49c2..3a37b9a9 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -52,6 +52,7 @@ #include #include #include +#include /** @@ -114,6 +115,7 @@ class Simulator : public rclcpp::Node float can_acc_; float can_target_r_; float can_delta_; + std::vector can_torque_cmd_; visualization_msgs::msg::Marker marker_; pcl::PointCloud track_; diff --git a/src/arussim/include/arussim/control_sim/control_sim.hpp b/src/arussim/include/arussim/control_sim/control_sim.hpp new file mode 100644 index 00000000..e3aab78e --- /dev/null +++ b/src/arussim/include/arussim/control_sim/control_sim.hpp @@ -0,0 +1,62 @@ +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "std_msgs/msg/header.hpp" +#include "arussim_msgs/msg/cmd.hpp" +#include "arussim_msgs/msg/cmd4_wd.hpp" + +#include "std_msgs/msg/float32.hpp" + +class ControlSim : public rclcpp::Node +{ + public: + ControlSim(); + + + private: + void receive_can(); + void send_torque(); + void send_state(); + int can_socket_; + struct ifreq ifr_; + struct sockaddr_can addr_; + struct can_frame frame; + rclcpp::TimerBase::SharedPtr timer_receive_; + rclcpp::TimerBase::SharedPtr timer_send_; + rclcpp::TimerBase::SharedPtr timer_state_; + + + + double kMass; + float kRdyn; + float kGearRatio; + + int16_t torque_i_; + int16_t vx_i_; + int16_t vy_i_; + int16_t r_i_; + float acc_; + float target_r_; + float vx_; + float vy_; + float r_; + int16_t acc_scaled_; + int16_t yaw_scaled_; + int16_t vx_scaled_; + int16_t vy_scaled_; + int16_t r_scaled_; + + rclcpp::Publisher::SharedPtr torque_pub_; + rclcpp::Publisher::SharedPtr cmd_pub_; + rclcpp::Publisher::SharedPtr control_vx_pub_; + rclcpp::Publisher::SharedPtr control_vy_pub_; + rclcpp::Publisher::SharedPtr control_r_pub_; +}; \ No newline at end of file diff --git a/src/arussim/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index dfdae30c..8daa1761 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -68,6 +68,10 @@ def generate_launch_description(): create_node(pkg='arussim', exec='supervisor_exec', config='supervisor_config.yaml'), + create_node(pkg='arussim', + exec='control_sim_exec', + config='control_sim_config.yaml'), + ]) diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index 73dd8823..ec6b6043 100755 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -99,7 +99,7 @@ Simulator::Simulator() : Node("simulator") std::chrono::milliseconds((int)(1000/kControllerRate)), std::bind(&Simulator::on_controller_sim_timer, this)); receive_can_timer_ = this->create_wall_timer( - std::chrono::milliseconds((int)(1000/kStateUpdateRate)), + std::chrono::milliseconds((int)(1)), std::bind(&Simulator::receive_can, this)); circuit_sub_ = this->create_subscription("/arussim/circuit", 1, @@ -139,7 +139,7 @@ Simulator::Simulator() : Node("simulator") controller_sim_.init(1/kControllerRate, kUseGSS); // Initialize torque variable - torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; + can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; // Set CSV file if (kCSVState) { @@ -379,7 +379,7 @@ void Simulator::on_fast_timer() double dt = 1.0 / kStateUpdateRate; - vehicle_dynamics_.update_simulation(can_delta_, torque_cmd_, dt); + vehicle_dynamics_.update_simulation(can_delta_, can_torque_cmd_, dt); if(use_tpl_){ check_lap(); @@ -451,11 +451,28 @@ void Simulator::on_fast_timer() int16_t yaw_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); int16_t delta_scaled = static_cast((frame_.data[5] << 8) | frame_.data[4]); - can_acc_ = static_cast(acc_scaled) / 100.0f; - can_target_r_ = static_cast(yaw_scaled) / 1000.0f; - can_delta_ = static_cast(delta_scaled) / 100.0f; + can_acc_ = static_cast(acc_scaled) / 100.0; + can_target_r_ = static_cast(yaw_scaled) / 1000.0; + can_delta_ = static_cast(delta_scaled) / 100.0; time_last_cmd_ = clock_->now(); } + + else if (frame_.can_id == 0x200){ + int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); + can_torque_cmd_.at(0) = torque_scaled * 9.8 / 1000.0; + } + else if (frame_.can_id == 0x203){ + int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); + can_torque_cmd_.at(1) = torque_scaled * 9.8 / 1000.0; + } + else if (frame_.can_id == 0x206){ + int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); + can_torque_cmd_.at(2) = torque_scaled * 9.8 / 1000.0; + } + else if (frame_.can_id == 0x209){ + int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); + can_torque_cmd_.at(3) = torque_scaled * 9.8 / 1000.0; + } } @@ -463,7 +480,7 @@ void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::Share { can_acc_ = 0.0; can_delta_ = 0.0; - torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; + can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; vehicle_dynamics_ = VehicleDynamics(); @@ -727,4 +744,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp new file mode 100644 index 00000000..78b8ce11 --- /dev/null +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -0,0 +1,137 @@ +/** + * @file controller_sin.cpp + * @author Carmen Menenendez (carmenmenendezda@gmail.com) + * @brief contol_sim node for the ARUSSIM package. This node simulates the Control PCB of the vehicle. + * @version 0.1 + * @date 2024-10-16 + * + */ +#include "arussim/control_sim/control_sim.hpp" + +/** + * @class ControlSim + * @brief ControlSim class for ARUSsim + * This class simulates the Control PCB of the vehicle. + * + */ + +ControlSim::ControlSim() : Node("control_sim") { + + this->declare_parameter("mass", 348.0); + this->declare_parameter("rdyn", 0.225); + this->declare_parameter("gear_ratio", 12.48); + this->get_parameter("mass", kMass); + this->get_parameter("rdyn", kRdyn); + this->get_parameter("gear_ratio", kGearRatio); + + // Timers + timer_receive_ = this->create_wall_timer(std::chrono::milliseconds(1), + std::bind(&ControlSim::receive_can, this)); + timer_send_ = this->create_wall_timer(std::chrono::milliseconds(5), + std::bind(&ControlSim::send_torque, this)); + timer_state_ = this->create_wall_timer(std::chrono::milliseconds(100), + std::bind(&ControlSim::send_state, this)); + + // CAN socket + can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + std::strcpy(ifr_.ifr_name, "can0"); + ioctl(can_socket_, SIOCGIFINDEX, &ifr_); + addr_.can_family = AF_CAN; + addr_.can_ifindex = ifr_.ifr_ifindex; + bind(can_socket_, (struct sockaddr *)&addr_, sizeof(addr_)); + + +} + + void ControlSim::receive_can() +{ + + read(can_socket_, &frame, sizeof(struct can_frame)); + + if (frame.can_id == 0x222) { + acc_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + yaw_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); + + acc_ = acc_scaled_ / 100.0; + target_r_ = yaw_scaled_ / 1000.0; + float max_torque = 21 * 1000 / 9.8; + + float F = acc_ * kMass; + torque_i_ = (int)(F * 0.225 / 4.0 * 1000.0 / 9.8); + RCLCPP_INFO(this->get_logger(), "F = %.2f, torque_i = %d", F, torque_i_); + } + + else if (frame.can_id == 0x1A3) { + vx_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + vy_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); + + vx_ = vx_scaled_ * 0.2f; + vy_ = vy_scaled_ * 0.2f; + + } + else if (frame.can_id == 0x1A4) { + r_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + + r_ = r_scaled_ * 0.00034906585; + + } + + +} + +void ControlSim::send_torque() +{ + std::array ids = {0x200, 0x203, 0x206, 0x209}; + + for (auto id : ids) { + frame.can_id = id; + frame.can_dlc = 8; + frame.data[0] = 0x00; + frame.data[1] = 0x07; + + frame.data[2] = torque_i_ & 0xFF; + frame.data[3] = (torque_i_ >> 8) & 0xFF; + + uint16_t torque_max = 1000; + uint16_t torque_min = -1000; + frame.data[4] = torque_max & 0xFF; + frame.data[5] = (torque_max >> 8) & 0xFF; + frame.data[6] = torque_min & 0xFF; + frame.data[7] = (torque_min >> 8) & 0xFF; + + write(can_socket_, &frame, sizeof(struct can_frame)); + + + } + +} + +void ControlSim::send_state(){ + + frame.can_id = 0x122; + frame.can_dlc = 6; + + vx_i_ = (int)(vx_ * 100.0f); + frame.data[0] = vx_i_ & 0xFF; + frame.data[1] = (vx_i_ >> 8) & 0xFF; + + vy_i_ = (int)(vy_ * 100.0f); + frame.data[2] = vy_i_ & 0xFF; + frame.data[3] = (vy_i_ >> 8) & 0xFF; + + r_i_ = (int)(r_ * 1000.0f); + frame.data[4] = r_i_ & 0xFF; + frame.data[5] = (r_i_ >> 8) & 0xFF; + + write(can_socket_, &frame, sizeof(struct can_frame)); + + +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 6c2b4dfbe584f96b2c2a1ed44d98ef9e4b3912ee Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Wed, 29 Oct 2025 21:39:09 +0100 Subject: [PATCH 05/25] fix: use vx and vy at control sim --- src/arussim/include/arussim/arussim_node.hpp | 23 +----- src/arussim/include/arussim/sensors.hpp | 11 --- .../include/arussim/vehicle_dynamics.hpp | 2 +- src/arussim/src/arussim_node.cpp | 44 ------------ src/arussim/src/control_sim/control_sim.cpp | 10 +-- src/arussim/src/sensors.cpp | 70 ++++--------------- 6 files changed, 20 insertions(+), 140 deletions(-) diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index 3a37b9a9..c2c35180 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -41,11 +41,6 @@ #include #include "std_msgs/msg/string.hpp" -#include "controller_sim/controller_sim.hpp" -#include "controller_sim/estimation.hpp" -#include "controller_sim/power_limitation.hpp" -#include "controller_sim/traction_control.hpp" -#include "controller_sim/torque_vectoring.hpp" #include #include @@ -53,7 +48,7 @@ #include #include #include - +#include /** * @class Simulator @@ -78,7 +73,6 @@ class Simulator : public rclcpp::Node private: VehicleDynamics vehicle_dynamics_; - ControllerSim controller_sim_; std::string kTrackName; double kStateUpdateRate; @@ -106,17 +100,15 @@ class Simulator : public rclcpp::Node double kCOGBackDist; double kCarWidth; - //Ros2 rclcpp::Clock::SharedPtr clock_; rclcpp::Time time_last_cmd_; - std::vector torque_cmd_; //Can float can_acc_; float can_target_r_; float can_delta_; std::vector can_torque_cmd_; - + visualization_msgs::msg::Marker marker_; pcl::PointCloud track_; arussim_msgs::msg::Trajectory fixed_trajectory_msg_; @@ -152,13 +144,6 @@ class Simulator : public rclcpp::Node */ void on_slow_timer(); - /** - * @brief Callback function for the controller timer. - * - * This method is called at regular intervals to update and publish low level controller - * action, simulating vehicle control unit from real car. - */ - void on_controller_sim_timer(); /** * @brief Callback function for the fast timer. @@ -224,12 +209,8 @@ class Simulator : public rclcpp::Node rclcpp::TimerBase::SharedPtr slow_timer_; rclcpp::TimerBase::SharedPtr fast_timer_; - rclcpp::TimerBase::SharedPtr controller_sim_timer_; rclcpp::Subscription::SharedPtr rviz_telep_sub_; rclcpp::Publisher::SharedPtr state_pub_; - rclcpp::Publisher::SharedPtr control_vx_pub_; - rclcpp::Publisher::SharedPtr control_vy_pub_; - rclcpp::Publisher::SharedPtr control_r_pub_; rclcpp::Publisher::SharedPtr track_pub_; rclcpp::Publisher::SharedPtr lidar_perception_pub_; rclcpp::Publisher::SharedPtr camera_perception_pub_; diff --git a/src/arussim/include/arussim/sensors.hpp b/src/arussim/include/arussim/sensors.hpp index 9083db74..6351fedf 100644 --- a/src/arussim/include/arussim/sensors.hpp +++ b/src/arussim/include/arussim/sensors.hpp @@ -130,19 +130,8 @@ class Sensors : public rclcpp::Node // ROS Communication rclcpp::Subscription::SharedPtr state_sub_; // State subscriber - rclcpp::Publisher::SharedPtr ax_pub_; // ax publisher - rclcpp::Publisher::SharedPtr ay_pub_; // ay publisher - rclcpp::Publisher::SharedPtr r_pub_; // r publisher rclcpp::TimerBase::SharedPtr imu_timer_; // IMU timer - - rclcpp::Publisher::SharedPtr ws_fr_pub_; // Wheel speed publisher - rclcpp::Publisher::SharedPtr ws_fl_pub_; // Wheel speed publisher - rclcpp::Publisher::SharedPtr ws_rr_pub_; // Wheel speed publisher - rclcpp::Publisher::SharedPtr ws_rl_pub_; // Wheel speed publisher - - rclcpp::Publisher::SharedPtr ext_pub_; // Extensometer publisher rclcpp::TimerBase::SharedPtr ext_timer_; // Extensometer timer - rclcpp::Publisher::SharedPtr torque_pub_; // Torque publisher rclcpp::TimerBase::SharedPtr inverter_timer_; // Inverter timer diff --git a/src/arussim/include/arussim/vehicle_dynamics.hpp b/src/arussim/include/arussim/vehicle_dynamics.hpp index 54f8eeb5..4e7e0de6 100644 --- a/src/arussim/include/arussim/vehicle_dynamics.hpp +++ b/src/arussim/include/arussim/vehicle_dynamics.hpp @@ -3,7 +3,7 @@ #include #include "arussim/csv_generator.hpp" #include -#include "controller_sim/controller_sim.hpp" + class VehicleDynamics { public: diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index ec6b6043..51fe300f 100755 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -66,12 +66,6 @@ Simulator::Simulator() : Node("simulator") state_pub_ = this->create_publisher( "/arussim/state", 10); - control_vx_pub_ = this->create_publisher( - "/arussim/control_vx", 10); - control_vy_pub_ = this->create_publisher( - "/arussim/control_vy", 10); - control_r_pub_ = this->create_publisher( - "/arussim/control_r", 10); track_pub_ = this->create_publisher( "/arussim/track", 10); lidar_perception_pub_ = this->create_publisher( @@ -95,9 +89,6 @@ Simulator::Simulator() : Node("simulator") fast_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1000/kStateUpdateRate)), std::bind(&Simulator::on_fast_timer, this)); - controller_sim_timer_ = this->create_wall_timer( - std::chrono::milliseconds((int)(1000/kControllerRate)), - std::bind(&Simulator::on_controller_sim_timer, this)); receive_can_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1)), std::bind(&Simulator::receive_can, this)); @@ -135,9 +126,6 @@ Simulator::Simulator() : Node("simulator") addr_.can_ifindex = ifr_.ifr_ifindex; bind(can_socket_, (struct sockaddr *)&addr_, sizeof(addr_)); - // Set controller_sim period and GSS usage - controller_sim_.init(1/kControllerRate, kUseGSS); - // Initialize torque variable can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; @@ -329,38 +317,6 @@ void Simulator::on_slow_timer() cone_visualization(); } -void Simulator::on_controller_sim_timer() { - // Update sensor data in ControllerSim - // TO DO: use sensors with noise instead of ground truth - controller_sim_.set_sensors( - vehicle_dynamics_.ax_, - vehicle_dynamics_.ay_, - vehicle_dynamics_.r_, - vehicle_dynamics_.delta_, - vehicle_dynamics_.wheel_speed_.fl_, - vehicle_dynamics_.wheel_speed_.fr_, - vehicle_dynamics_.wheel_speed_.rl_, - vehicle_dynamics_.wheel_speed_.rr_, - vehicle_dynamics_.vx_, - vehicle_dynamics_.vy_ - ); - - // Torque command calculation - torque_cmd_ = controller_sim_.get_torque_cmd(can_acc_, can_target_r_); - - // Publish control estimation - std_msgs::msg::Float32 control_vx_msg; - control_vx_msg.data = controller_sim_.vx_; - control_vx_pub_->publish(control_vx_msg); - - std_msgs::msg::Float32 control_vy_msg; - control_vy_msg.data = controller_sim_.vy_; - control_vy_pub_->publish(control_vy_msg); - - std_msgs::msg::Float32 control_r_msg; - control_r_msg.data = controller_sim_.r_; - control_r_pub_->publish(control_r_msg); -} /** * @brief Fast timer callback for state updates. diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index 78b8ce11..07a56c4e 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -58,15 +58,15 @@ ControlSim::ControlSim() : Node("control_sim") { float F = acc_ * kMass; torque_i_ = (int)(F * 0.225 / 4.0 * 1000.0 / 9.8); - RCLCPP_INFO(this->get_logger(), "F = %.2f, torque_i = %d", F, torque_i_); + } - else if (frame.can_id == 0x1A3) { + else if (frame.can_id == 0x1A0) { vx_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); vy_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); - vx_ = vx_scaled_ * 0.2f; - vy_ = vy_scaled_ * 0.2f; + vx_ = vx_scaled_ * 0.2 / 3.6; + vy_ = vy_scaled_ * 0.2 / 3.6; } else if (frame.can_id == 0x1A4) { @@ -122,7 +122,7 @@ void ControlSim::send_state(){ r_i_ = (int)(r_ * 1000.0f); frame.data[4] = r_i_ & 0xFF; frame.data[5] = (r_i_ >> 8) & 0xFF; - + RCLCPP_INFO(this->get_logger(), "can vx = %.2f", vx_); write(can_socket_, &frame, sizeof(struct can_frame)); diff --git a/src/arussim/src/sensors.cpp b/src/arussim/src/sensors.cpp index 46efb79d..f4492339 100644 --- a/src/arussim/src/sensors.cpp +++ b/src/arussim/src/sensors.cpp @@ -76,17 +76,11 @@ Sensors::Sensors() : Node("sensors") ); // IMU - ax_pub_ = this->create_publisher("/arussim/IMU/ax", 10); - ay_pub_ = this->create_publisher("/arussim/IMU/ay", 10); - r_pub_ = this->create_publisher("/arussim/IMU/yaw_rate", 10); - imu_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1000/kImuFrequency)), std::bind(&Sensors::imu_timer, this) ); - // Extensometer - ext_pub_ = this->create_publisher("/arussim/extensometer", 10); ext_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1000/kExtensometerFrequency)), @@ -100,19 +94,16 @@ Sensors::Sensors() : Node("sensors") std::chrono::milliseconds((int)(1000/kTorqueFrequency)), std::bind(&Sensors::inverter_timer, this) ); - ws_fr_pub_ = this->create_publisher( - "/arussim/fr_wheel_speed", 10); - ws_fl_pub_ = this->create_publisher( - "/arussim/fl_wheel_speed", 10); - ws_rr_pub_ = this->create_publisher( - "/arussim/rr_wheel_speed", 10); - ws_rl_pub_ = this->create_publisher( - "/arussim/rl_wheel_speed", 10); frames = { - {0x1A3, 4, { // IMU ax, ay + {0x1A0, 4, { + {"GSS/vx", {0, 15, true, 0.0555556, 0.0}}, + {"GSS/vy", {16, 31, true, 0.0555556, 0.0}} + }}, + {0x1A3, 4, { // GSS {"IMU/ax", {0, 15, true, 0.02, 0.0}}, - {"IMU/ay", {16, 31, true, 0.02, 0.0}} + {"IMU/ay", {16, 31, true, 0.02, 0.0}}, + }}, {0x1A4, 2, { // IMU yaw_rate {"IMU/yaw_rate", {0, 15, true, 0.000349065, 0.0}} @@ -215,24 +206,7 @@ void Sensors::inverter_timer(){ wheel_speed_.fr_ = wheel_speed.front_right * 0.225 + dist_front_right(gen); wheel_speed_.fl_ = wheel_speed.front_left * 0.225 + dist_front_left(gen); wheel_speed_.rr_ = wheel_speed.rear_right * 0.225 + dist_rear_right(gen); - wheel_speed_.rl_ = wheel_speed.rear_left * 0.225 + dist_rear_left(gen); - - // Create the wheel speed message - auto msg_fr = std_msgs::msg::Float32(); - auto msg_fl = std_msgs::msg::Float32(); - auto msg_rr = std_msgs::msg::Float32(); - auto msg_rl = std_msgs::msg::Float32(); - - msg_fr.data = wheel_speed_.fr_; - msg_fl.data = wheel_speed_.fl_; - msg_rr.data = wheel_speed_.rr_; - msg_rl.data = wheel_speed_.rl_; - - // Publish the torque message - ws_fr_pub_->publish(msg_fr); - ws_fl_pub_->publish(msg_fl); - ws_rr_pub_->publish(msg_rr); - ws_rl_pub_->publish(msg_rl); + wheel_speed_.rl_ = wheel_speed.rear_left * 0.225 + dist_rear_left(gen); //Send Inverter CAN frames std::map values = { {"fl_inv_speed", wheel_speed_.fl_}, {"fl_inv_torque", torque_cmd_.fl_ }, @@ -260,30 +234,15 @@ void Sensors::imu_timer() std::normal_distribution<> dist_ax(0.0, kNoiseImuAx); std::normal_distribution<> dist_ay(0.0, kNoiseImuAy); std::normal_distribution<> dist_r(0.0, kNoiseImuR); - - // Create IMU data messages - auto msg_ax = std_msgs::msg::Float32(); - auto msg_ay = std_msgs::msg::Float32(); - auto msg_r = std_msgs::msg::Float32(); - - // Yaw rate - msg_r.data = r_ + dist_r(gen); - - // Linear acceleration - msg_ax.data = ax_ + dist_ax(gen); - msg_ay.data = ay_ + dist_ay(gen); - - // Publish the IMU message - ax_pub_->publish(msg_ax); - ay_pub_->publish(msg_ay); - r_pub_->publish(msg_r); + //TODO: Add noise to vx and vy //Send CAN frames for IMU (GSS ids) std::map values = { {"IMU/ax", ax_ + dist_ax(gen)}, - {"IMU/ay", ay_ + dist_ay(gen)}, {"IMU/yaw_rate", r_ + dist_r(gen)} }; + {"IMU/ay", ay_ + dist_ay(gen)}, {"IMU/yaw_rate", r_ + dist_r(gen)}, + {"GSS/vx", vx_}, {"GSS/vy", vy_} }; for (auto &frame : frames) { - if (frame.id == 0x1A3 || frame.id == 0x1A4) { + if (frame.id == 0x1A3 || frame.id == 0x1A4 || frame.id == 0x1A0) { send_can_frame(frame, values); } } @@ -302,11 +261,6 @@ void Sensors::extensometer_timer() std::mt19937 gen(rd()); std::normal_distribution<> dist(0.0, kNoiseExtensometer); - - auto message = std_msgs::msg::Float32(); - message.data = delta_ + dist(gen); - ext_pub_->publish(message); - //Send CAN frame for extensometer std::map values = { {"extensometer", delta_ + dist(gen)} }; From c6ae6e4966f8c744d4c7de3a01c83c9a0c15cd98 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Thu, 30 Oct 2025 12:43:51 +0100 Subject: [PATCH 06/25] feat: thread for can receive --- src/arussim/include/arussim/arussim_node.hpp | 1 + src/arussim/include/controller_sim | 1 - src/arussim/src/arussim_node.cpp | 49 ++++++++++---------- src/arussim/src/control_sim/control_sim.cpp | 1 - 4 files changed, 25 insertions(+), 27 deletions(-) delete mode 160000 src/arussim/include/controller_sim diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index c2c35180..e8cc320e 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -229,4 +229,5 @@ class Simulator : public rclcpp::Node struct ifreq ifr_{}; struct sockaddr_can addr_{}; struct can_frame frame_; + std::thread thread_; }; \ No newline at end of file diff --git a/src/arussim/include/controller_sim b/src/arussim/include/controller_sim deleted file mode 160000 index 0db9513d..00000000 --- a/src/arussim/include/controller_sim +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0db9513d105e6f0f7b383606ffa312c3c8897fc5 diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index 51fe300f..06c1cd5e 100755 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -89,9 +89,10 @@ Simulator::Simulator() : Node("simulator") fast_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1000/kStateUpdateRate)), std::bind(&Simulator::on_fast_timer, this)); - receive_can_timer_ = this->create_wall_timer( - std::chrono::milliseconds((int)(1)), - std::bind(&Simulator::receive_can, this)); + + //Thread for CAN reception + std::thread thread_(&Simulator::receive_can, this); + thread_.detach(); circuit_sub_ = this->create_subscription("/arussim/circuit", 1, std::bind(&Simulator::load_track, this, std::placeholders::_1)); @@ -397,41 +398,39 @@ void Simulator::on_fast_timer() marker_pub_->publish(marker_); } - void Simulator::receive_can() +void Simulator::receive_can() { - - read(can_socket_, &frame_, sizeof(struct can_frame)); + int flags = fcntl(can_socket_, F_GETFL, 0); + fcntl(can_socket_, F_SETFL, flags | O_NONBLOCK); + while (rclcpp::ok()) { + int nbytes = read(can_socket_, &frame_, sizeof(struct can_frame)); + if (nbytes < 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } if (frame_.can_id == 0x222) { int16_t acc_scaled = static_cast((frame_.data[1] << 8) | frame_.data[0]); int16_t yaw_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); int16_t delta_scaled = static_cast((frame_.data[5] << 8) | frame_.data[4]); - can_acc_ = static_cast(acc_scaled) / 100.0; - can_target_r_ = static_cast(yaw_scaled) / 1000.0; - can_delta_ = static_cast(delta_scaled) / 100.0; - time_last_cmd_ = clock_->now(); - } - - else if (frame_.can_id == 0x200){ - int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); - can_torque_cmd_.at(0) = torque_scaled * 9.8 / 1000.0; + can_acc_ = static_cast(acc_scaled) / 100.0f; + can_target_r_ = static_cast(yaw_scaled) / 1000.0f; + can_delta_ = static_cast(delta_scaled) / 100.0f; + time_last_cmd_ = clock_->now(); } - else if (frame_.can_id == 0x203){ + + else if (frame_.can_id == 0x200 || frame_.can_id == 0x203 || + frame_.can_id == 0x206 || frame_.can_id == 0x209) { + int idx = (frame_.can_id - 0x200) / 3; // 0,1,2,3 int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); - can_torque_cmd_.at(1) = torque_scaled * 9.8 / 1000.0; - } - else if (frame_.can_id == 0x206){ - int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); - can_torque_cmd_.at(2) = torque_scaled * 9.8 / 1000.0; - } - else if (frame_.can_id == 0x209){ - int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); - can_torque_cmd_.at(3) = torque_scaled * 9.8 / 1000.0; + can_torque_cmd_.at(idx) = torque_scaled * 9.8f / 1000.0f; } + } } + void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) { can_acc_ = 0.0; diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index 07a56c4e..e2ce9dfb 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -122,7 +122,6 @@ void ControlSim::send_state(){ r_i_ = (int)(r_ * 1000.0f); frame.data[4] = r_i_ & 0xFF; frame.data[5] = (r_i_ >> 8) & 0xFF; - RCLCPP_INFO(this->get_logger(), "can vx = %.2f", vx_); write(can_socket_, &frame, sizeof(struct can_frame)); From 948b1180a81752ca8469245d8fa8183d47665c4d Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Tue, 4 Nov 2025 21:24:02 +0100 Subject: [PATCH 07/25] feat: control 4wd fail --- src/arussim/CMakeLists.txt | 7 +- src/arussim/include/arussim/arussim_node.hpp | 3 + .../include/arussim/control_sim/Parameters.h | 180 ++++++++++++++ .../include/arussim/control_sim/SensorData.h | 27 +++ .../arussim/control_sim/aux_functions.h | 47 ++++ .../arussim/control_sim/control_sim.hpp | 33 ++- .../include/arussim/control_sim/estimation.h | 26 ++ .../arussim/control_sim/power_control.h | 28 +++ .../arussim/control_sim/torque_vectoring.h | 28 +++ .../arussim/control_sim/traction_control.h | 28 +++ src/arussim/src/control_sim/aux_functions.c | 228 ++++++++++++++++++ src/arussim/src/control_sim/control_sim.cpp | 194 ++++++++++++--- src/arussim/src/control_sim/estimation.c | 22 ++ src/arussim/src/control_sim/power_control.c | 31 +++ .../src/control_sim/torque_vectoring.c | 117 +++++++++ .../src/control_sim/traction_control.c | 144 +++++++++++ 16 files changed, 1106 insertions(+), 37 deletions(-) create mode 100644 src/arussim/include/arussim/control_sim/Parameters.h create mode 100644 src/arussim/include/arussim/control_sim/SensorData.h create mode 100644 src/arussim/include/arussim/control_sim/aux_functions.h create mode 100644 src/arussim/include/arussim/control_sim/estimation.h create mode 100644 src/arussim/include/arussim/control_sim/power_control.h create mode 100644 src/arussim/include/arussim/control_sim/torque_vectoring.h create mode 100644 src/arussim/include/arussim/control_sim/traction_control.h create mode 100644 src/arussim/src/control_sim/aux_functions.c create mode 100644 src/arussim/src/control_sim/estimation.c create mode 100644 src/arussim/src/control_sim/power_control.c create mode 100644 src/arussim/src/control_sim/torque_vectoring.c create mode 100644 src/arussim/src/control_sim/traction_control.c diff --git a/src/arussim/CMakeLists.txt b/src/arussim/CMakeLists.txt index f2008ffe..275d2c05 100755 --- a/src/arussim/CMakeLists.txt +++ b/src/arussim/CMakeLists.txt @@ -118,7 +118,12 @@ ament_target_dependencies(supervisor_exec rclcpp tf2) # generate executable for control_sim.cpp -add_executable(control_sim_exec src/control_sim/control_sim.cpp) +add_executable(control_sim_exec src/control_sim/control_sim.cpp + src/control_sim/estimation.c + src/control_sim/aux_functions.c + src/control_sim/torque_vectoring.c + src/control_sim/traction_control.c + src/control_sim/power_control.c) ament_target_dependencies(control_sim_exec rclcpp std_msgs geometry_msgs diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index e8cc320e..5a69420b 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -108,6 +108,7 @@ class Simulator : public rclcpp::Node float can_target_r_; float can_delta_; std::vector can_torque_cmd_; + uint16_t as_status_; visualization_msgs::msg::Marker marker_; pcl::PointCloud track_; @@ -230,4 +231,6 @@ class Simulator : public rclcpp::Node struct sockaddr_can addr_{}; struct can_frame frame_; std::thread thread_; + bool can_blocked_; + }; \ No newline at end of file diff --git a/src/arussim/include/arussim/control_sim/Parameters.h b/src/arussim/include/arussim/control_sim/Parameters.h new file mode 100644 index 00000000..aedf40ce --- /dev/null +++ b/src/arussim/include/arussim/control_sim/Parameters.h @@ -0,0 +1,180 @@ +/* + * Parameters.h + * + * Created on: Feb 22, 2025 + * Author: carme + */ +#include + + + +#ifndef INC_PARAMETERS_H_ +#define INC_PARAMETERS_H_ +#ifdef __cplusplus +extern "C" { +#endif + +//VDC MODELS ACTIVATION +#define TV_ACTIVE 0 +#define TC_ACTIVE 0 +#define EST_ACTIVE 0 + + +#define pi 3.141592 +#define eps 0.0000001 +#define GRAVITY 9.81 +#define DEFAULT_MAX_POWER 20000 +#define DEFAULT_MIN_POWER -20000 + +#define DEFAULT_MAX_VOLTAGE 595 +#define DEFAULT_MIN_VOLTAGE 430 + +//CAR DATA +//distance +#define DEFAULT_TW 1.22 +#define DEFAULT_WB 1.535 +#define DEFAULT_R_CDG 0.5 + + +//Mass and inertia +#define DEFAULT_MASS 340. +#define DEFAULT_NSM 25 +#define DEFAULT_H_CDG 0.273 +#define DEFAULT_H_CDG_NSM 0.225 +#define DEFAULT_H_CDG_SM 0.3 +#define DEFAULT_H_RC_F 0.033 +#define DEFAULT_H_RC_R 0.097 + +//Stiffness +#define K_s_f 87563 +#define K_s_r 87563 +#define K_ARB_f 3786 +#define K_ARB_r 11 + +//Motion Ratios +#define MR_s 1.1003 +#define MR_ARB_f_DIRK 2.05 +#define MR_ARB_r_DIRK 2.745 +#define r_ARB_f 0.0628 +#define psi_ARB_f 0.07079055 //4.056 * pi /180 +#define r_ARB_r 0.07 +#define psi_ARB_r 0.1282817 //7.35 * pi /180 +#define DEFAULT_GEAR_RATIO 12.48 +#define DEFAULT_RDYN 0.225 +#define DEFAULT_WHEEL_INERTIA 0.4 +#define DEFAULT_TL_POS 21.0 //Pruebas borriqueta +#define DEFAULT_TL_NEG -21.0 //Pruebas borriqueta + +//AERO +#define DEFAULT_RHO 1.225 +#define DEFAULT_CDA 1.971 +#define DEFAULT_CLA 4.747 +#define DEFAULT_R_CDP 0.4604 +#define DEFAULT_H_CDP 0.517 + +//YAW RATE PID +#define TV_KP 400 //Pruebas mesa +#define TV_KI 0 +#define TV_KD 0 +#define DEFAULT_TV_N 0 +#define DEFAULT_MAX_MZ 600 + + +//TC PID +#define TC_K_ 30 +#define TC_TI 0.1 +#define TC_TD 0 + + +//TC Paramtets +#define DEFAULT_TC_V0 5 +#define DEFAULT_TC_V_GAIN 3 + +//PACEJKA TIRE MODEL +// PAC parameters +#define PAC_KALPHAP 0.1809f +#define PAC_KLAMBDA_P 0.1397f +#define PAC_BLAT 12.f +#define PAC_BLON 17.f +#define PAC_DLAT -1.33f +#define PAC_CLAT 2.f +#define PAC_DLON 1.198f +#define PAC_CLON 1.3f + +//TODO: eliminar de la estructura y del init lo que no se use +typedef struct { + float g; + float maximum_power; + float minimum_power; + + float V_max; + float V_min; + + //CAR DATA + //Distances + float trackwidthR, trackwidthF; + float wheelbase; + float r_cdg; + float lf, lr; + + //Mass and inertia + uint16_t mass; + uint8_t nsm_f, nsm_r; + uint8_t sm; + float sm_f, sm_r; + float h_cdg; + float h_cdg_nsm_f, h_cdg_nsm_r; + float h_cdg_sm; + float h_RC_f, h_RC_r, h_RA; + + //Motion Ratios + float RS_f, RS_r, RS; + float gear_ratio; + float rdyn, wheel_inertia; + float torque_limit_negative [4]; + float torque_limit_positive [4]; + + + //AERO + float rho, CDA, CLA; + float r_cdp, h_cdp; + + float fz_params[13]; + + //TC + int v0; + int v_gain; +} Parameters; + +typedef struct{ + int autonomous, driving, inspection; + float acc, target_r; + float inspection_torque[4]; +} DV; + +typedef struct{ + //TV + float TV_Kp, TV_Ki, TV_Kd, TV_N; + float integral; + float err_prev; + float deriv_filt; + uint16_t max_Mz; + + //TC + float TC_K, TC_Ti, TC_Td; + + //general + float TS; + uint32_t last_timestamp; + int init; + +} PID; + + + +#ifdef __cplusplus +} +#endif + + +#endif /* INC_PARAMETERS_H_ */ \ No newline at end of file diff --git a/src/arussim/include/arussim/control_sim/SensorData.h b/src/arussim/include/arussim/control_sim/SensorData.h new file mode 100644 index 00000000..02888c48 --- /dev/null +++ b/src/arussim/include/arussim/control_sim/SensorData.h @@ -0,0 +1,27 @@ +#ifndef CONTROL_SIM_SENSORDATA_H +#define CONTROL_SIM_SENSORDATA_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + float acceleration_x, acceleration_y, acceleration_z; + float angular_x, angular_y, angular_z; + float speed_x, speed_y; + float steering_angle; + float apps; + float load_cell; + float motor_speed[4]; + float current, vehicle_side_voltage, battery_voltage; + float power; + float V_soc; +} SensorData; + +#ifdef __cplusplus +} +#endif + +#endif // CONTROL_SIM_SENSORDATA_H diff --git a/src/arussim/include/arussim/control_sim/aux_functions.h b/src/arussim/include/arussim/control_sim/aux_functions.h new file mode 100644 index 00000000..e5fd6702 --- /dev/null +++ b/src/arussim/include/arussim/control_sim/aux_functions.h @@ -0,0 +1,47 @@ +/* + * aux_functions.h + * + * Created on: May 14, 2025 + * Author: carme + */ + +#ifndef INC_AUX_FUNCTIONS_H_ +#define INC_AUX_FUNCTIONS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "arussim/control_sim/SensorData.h" +#include "arussim/control_sim/Parameters.h" +#include +typedef struct { + float kAlphaP; + float kLambdaP; + float Blat; + float Blon; + float Dlat; + float Clat; + float Dlon; + float Clon; +} PAC; + +typedef struct{ + float force_fy[4]; + float force_fx[4]; + float Mz; + float force_fx_rolling; + float tire_load[4]; +} TIRE; + +float driver_request(SensorData *sensors, Parameters *parameters); +void Calculate_Tire_Loads(SensorData *sensors, Parameters *parameters, float *state, TIRE *tire); +void Calculate_Tire_Forces(TIRE *tire, const float slip_angle[4], const float slip_ratio[4]); +float pc_request(DV *dv, Parameters *parameters); + + +#ifdef __cplusplus +} +#endif + +#endif /* INC_AUX_FUNCTIONS_H_ */ \ No newline at end of file diff --git a/src/arussim/include/arussim/control_sim/control_sim.hpp b/src/arussim/include/arussim/control_sim/control_sim.hpp index e3aab78e..9cb372d0 100644 --- a/src/arussim/include/arussim/control_sim/control_sim.hpp +++ b/src/arussim/include/arussim/control_sim/control_sim.hpp @@ -15,6 +15,15 @@ #include "std_msgs/msg/float32.hpp" +#include "arussim/control_sim/SensorData.h" +#include "arussim/control_sim/Parameters.h" +#include "arussim/control_sim/aux_functions.h" +#include "arussim/control_sim/estimation.h" +#include "arussim/control_sim/torque_vectoring.h" +#include "arussim/control_sim/traction_control.h" +#include "arussim/control_sim/power_control.h" + + class ControlSim : public rclcpp::Node { public: @@ -22,16 +31,24 @@ class ControlSim : public rclcpp::Node private: + SensorData sensors; + Parameters parameters; + DV dv; + TIRE tire; + PID pid; + + void Parameters_Init(Parameters *); void receive_can(); - void send_torque(); + void send_torque(float torque_out[4]); void send_state(); + void default_task(); int can_socket_; struct ifreq ifr_; struct sockaddr_can addr_; struct can_frame frame; rclcpp::TimerBase::SharedPtr timer_receive_; - rclcpp::TimerBase::SharedPtr timer_send_; - rclcpp::TimerBase::SharedPtr timer_state_; + rclcpp::TimerBase::SharedPtr timer_defaultTask_; + rclcpp::Clock::SharedPtr clock_; @@ -43,16 +60,16 @@ class ControlSim : public rclcpp::Node int16_t vx_i_; int16_t vy_i_; int16_t r_i_; - float acc_; - float target_r_; - float vx_; - float vy_; - float r_; int16_t acc_scaled_; int16_t yaw_scaled_; int16_t vx_scaled_; int16_t vy_scaled_; int16_t r_scaled_; + float state[3]; + float SR[4]; + float TC_out[4]; + float TV_out[4]; + float fx_request; rclcpp::Publisher::SharedPtr torque_pub_; rclcpp::Publisher::SharedPtr cmd_pub_; diff --git a/src/arussim/include/arussim/control_sim/estimation.h b/src/arussim/include/arussim/control_sim/estimation.h new file mode 100644 index 00000000..6f87aed8 --- /dev/null +++ b/src/arussim/include/arussim/control_sim/estimation.h @@ -0,0 +1,26 @@ +/* + * estimation.h + * + * Created on: Jan 29, 2025 + * Author: carme + */ + + +#ifndef ESTIMATION_H_ +#define ESTIMATION_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "arussim/control_sim/Parameters.h" +#include +#include "arussim/control_sim/SensorData.h" + +void Estimation_Init(void); +void Estimation_Update(SensorData *sensors, Parameters *params, float estimation_out[3]); + +#ifdef __cplusplus +} +#endif +#endif /* SRC_ESTIMATION_H_ */ \ No newline at end of file diff --git a/src/arussim/include/arussim/control_sim/power_control.h b/src/arussim/include/arussim/control_sim/power_control.h new file mode 100644 index 00000000..1266aa3c --- /dev/null +++ b/src/arussim/include/arussim/control_sim/power_control.h @@ -0,0 +1,28 @@ +/* + * power_control.h + * + * Created on: Jan 29, 2025 + * Author: carme + */ + +#ifndef SRC_POWER_CONTROL_H_ +#define SRC_POWER_CONTROL_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + +#include "arussim/control_sim/SensorData.h" +#include "arussim/control_sim/Parameters.h" +#include + + +void PowerControl_Init(Parameters *parameters); +void PowerControl_Update(SensorData *sensors, Parameters *parameters, float * torque_out); + +#ifdef __cplusplus +} +#endif + +#endif /* SRC_POWER_CONTROL_H_ */ \ No newline at end of file diff --git a/src/arussim/include/arussim/control_sim/torque_vectoring.h b/src/arussim/include/arussim/control_sim/torque_vectoring.h new file mode 100644 index 00000000..855d2923 --- /dev/null +++ b/src/arussim/include/arussim/control_sim/torque_vectoring.h @@ -0,0 +1,28 @@ +/* + * torque_vectoring.h + * + * Created on: Apr 19, 2025 + * Author: carme + */ + +#ifndef INC_TORQUE_VECTORING_H_ +#define INC_TORQUE_VECTORING_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + +#include "arussim/control_sim/SensorData.h" +#include "arussim/control_sim/Parameters.h" +#include "arussim/control_sim/aux_functions.h" +#include + +void TorqueVectoring_Init(PID *pid); +void TorqueVectoring_Update(SensorData *sensors, Parameters *parameters, PID *pid, TIRE *tire, DV *dv, float fx_request, float *state, float *torque_out) ; +float Target_Generation (SensorData *sensors, Parameters *parameters, PID *pid, DV *dv, float *state); + +#ifdef __cplusplus +} +#endif +#endif /* INC_TORQUE_VECTORING_H_ */ \ No newline at end of file diff --git a/src/arussim/include/arussim/control_sim/traction_control.h b/src/arussim/include/arussim/control_sim/traction_control.h new file mode 100644 index 00000000..1d2cf919 --- /dev/null +++ b/src/arussim/include/arussim/control_sim/traction_control.h @@ -0,0 +1,28 @@ +/* + * traction_control.h + * + * Created on: Jan 29, 2025 + * Author: carme + */ + +#ifndef SRC_TRACTION_CONTROL_H_ +#define SRC_TRACTION_CONTROL_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "arussim/control_sim/SensorData.h" +#include "arussim/control_sim/Parameters.h" +#include "arussim/control_sim/aux_functions.h" +#include + + + +void TractionControl_Init(PID *pid, Parameters *parameters); +void TractionControl_Update(SensorData *sensors, Parameters *parameters, PID *pid, TIRE *tire, float *Tin, float *TC, float *SR, DV *dv); + +#ifdef __cplusplus +} +#endif +#endif /* INC_TORQUE_VECTORING_H_ */ \ No newline at end of file diff --git a/src/arussim/src/control_sim/aux_functions.c b/src/arussim/src/control_sim/aux_functions.c new file mode 100644 index 00000000..7f0ed47c --- /dev/null +++ b/src/arussim/src/control_sim/aux_functions.c @@ -0,0 +1,228 @@ + /* + * aux_functions.c + * + * Created on: May 14, 2025 + * Author: carme + */ + + +#include "arussim/control_sim/aux_functions.h" +float tire_loadtx[4]; +float tire_loadty[4]; +float tire_loadneg[4]; +float tire_loadneg2[4]; + +float driver_request(SensorData *sensors, Parameters *parameters) { + float fx_request = sensors->apps * 1.25f - 100*sensors->load_cell; + if (fx_request <= -95) { + fx_request = -99; + } else if (fx_request >= 95) { + fx_request = 99; + } + + if(fx_request < 0){ + fx_request = -fx_request*2*parameters->torque_limit_negative[0]*parameters->gear_ratio/parameters->rdyn*0.01; + } else { + fx_request = fx_request*2*parameters->torque_limit_positive[0]*parameters->gear_ratio/parameters->rdyn*0.01; + } + + return fx_request; +} +float pc_request (DV *dv, Parameters *parameters){ + return dv->acc * parameters->mass; +} +void Calculate_Tire_Loads(SensorData *sensors, Parameters *parameters, float *state, TIRE *tire) { + + float FL = parameters->fz_params[0] * state[0] * state[0]; + float FD = parameters->fz_params[1] * state[0] * state[0]; + + // LATERAL LOAD TRANSFER + + //nonsuspended weight transfer (N) + float y_WT_ns_f = parameters->fz_params[2] * sensors->acceleration_y; + float y_WT_ns_r = parameters->fz_params[2] * sensors->acceleration_y; + + //suspended geometric weight transfer (N) + float y_WT_s_g_f = parameters->fz_params[3] * sensors->acceleration_y; + float y_WT_s_g_r = parameters->fz_params[4] * sensors->acceleration_y; + + //susp elastic WT (N) + float y_WT_s_e_f = parameters->fz_params[5] * sensors->acceleration_y; + float y_WT_s_e_r = parameters->fz_params[6] * sensors->acceleration_y; + + // LONG LOAD TRANSFER + + //long nonsuspended weight transfer (N) + float x_WT_ns = parameters->fz_params[7] * sensors->acceleration_x; + + //long suspended weight transfer (N) + float x_WT_s = parameters->fz_params[8] * sensors->acceleration_x; + //long suspended elastic weight transfer (N) +// float x_WT_s_e_f = -x_WT_s * ((sensors->acceleration_x < 0) ? 1 : 0) - x_WT_s * ((sensors->acceleration_x > 0) ? 1 : 0); //HA +// float x_WT_s_e_r = x_WT_s * ((sensors->acceleration_x < 0) ? 1 : 0) + x_WT_s * ((sensors->acceleration_x > 0) ? 1 : 0); //VA + + + //WHEEL LOADS + tire->tire_load[0] = parameters->fz_params[9] + parameters->fz_params[10] * FL + - parameters->fz_params[11] * FD - y_WT_ns_f - y_WT_s_g_f + - y_WT_s_e_f - 0.5f * (x_WT_ns + x_WT_s); + + tire->tire_load[1] = parameters->fz_params[9] + parameters->fz_params[10] * FL + - parameters->fz_params[11] * FD + y_WT_ns_f + y_WT_s_g_f + + y_WT_s_e_f - 0.5f * (x_WT_ns + x_WT_s); + + tire->tire_load[2] = parameters->fz_params[12] + (0.5f - parameters->fz_params[10]) * FL + + parameters->fz_params[11] * FD - y_WT_ns_r - y_WT_s_g_r + - y_WT_s_e_r + 0.5f * (x_WT_ns + x_WT_s); + + tire->tire_load[3] = parameters->fz_params[12] + (0.5f - parameters->fz_params[10]) * FL + + parameters->fz_params[11] * FD + y_WT_ns_r + + y_WT_s_g_r + y_WT_s_e_r + 0.5f * (x_WT_ns + x_WT_s); + + + // NEGATIVE TIRE LOAD RECALCULATION + + for (int i = 0; i < 4; i++) { + tire_loadneg[i] = (tire->tire_load[i] < 0.f) ? tire->tire_load[i] : 0.f; + tire->tire_load[i] = (tire->tire_load[i] > 0.f) ? tire->tire_load[i] : 0.f; + } + + if ((tire_loadneg[0] < 0 && tire_loadneg[1] < 0) || (tire_loadneg[2] < 0 && tire_loadneg[3] < 0)) { + + tire_loadty[0] = 0.5f * (tire_loadneg[2] - tire_loadneg[3]); + tire_loadty[1] = -0.5f * (tire_loadneg[2] - tire_loadneg[3]); + tire_loadty[2] = 0.5f * (tire_loadneg[0] - tire_loadneg[1]); + tire_loadty[3] = -0.5f * (tire_loadneg[0] - tire_loadneg[1]); + + tire_loadtx[0] = 0.5f * (tire_loadneg[2] + tire_loadneg[3]); + tire_loadtx[1] = 0.5f * (tire_loadneg[2] + tire_loadneg[3]); + tire_loadtx[2] = 0.5f * (tire_loadneg[0] + tire_loadneg[1]); + tire_loadtx[3] = 0.5f * (tire_loadneg[0] + tire_loadneg[1]); + + for (int i = 0; i < 4; i++) { + tire->tire_load[i] += tire_loadty[i] + tire_loadtx[i]; + } + } + + else if ((tire_loadneg[0] < 0 && tire_loadneg[2] < 0) || (tire_loadneg[1] < 0 && tire_loadneg[3] < 0)) { + tire_loadty[0] = 0.5f * (tire_loadneg[1] + tire_loadneg[3]); + tire_loadty[1] = 0.5f * (tire_loadneg[0] + tire_loadneg[2]); + tire_loadty[2] = 0.5f * (tire_loadneg[1] + tire_loadneg[3]); + tire_loadty[3] = 0.5f * (tire_loadneg[0] + tire_loadneg[2]); + + tire_loadtx[0] = 0.5f * (tire_loadneg[1] - tire_loadneg[3]); + tire_loadtx[1] = 0.5f * (tire_loadneg[0] - tire_loadneg[2]); + tire_loadtx[2] = -0.5f * (tire_loadneg[1] - tire_loadneg[3]); + tire_loadtx[3] = -0.5f * (tire_loadneg[0] - tire_loadneg[2]); + + for (int i = 0; i < 4; i++) { + tire->tire_load[i] += tire_loadty[i] + tire_loadtx[i]; + } + } + + + else if (tire_loadneg[0] < 0 || tire_loadneg[1] < 0) { + float WT_x = 0.5f * (x_WT_ns + x_WT_s); + float WT_y = (y_WT_ns_f + y_WT_s_g_f + y_WT_s_e_f); + + tire_loadtx[0] = WT_x / (WT_x + fabs(WT_y) + eps) * tire_loadneg[0]; + tire_loadtx[1] = WT_x / (WT_x + fabs(WT_y) + eps) * tire_loadneg[1]; + tire_loadty[0] = WT_y / (WT_x + fabs(WT_y) + eps) * tire_loadneg[0] * ((sensors->acceleration_y > 0) - (sensors->acceleration_y < 0)); + tire_loadty[1] = WT_y / (WT_x + fabs(WT_y) + eps) * tire_loadneg[1] * ((sensors->acceleration_y > 0) - (sensors->acceleration_y < 0)); + + tire->tire_load[0] += tire_loadtx[1]; + tire->tire_load[1] += tire_loadtx[0]; + tire->tire_load[2] += tire_loadty[0]; + tire->tire_load[3] += tire_loadty[1]; + + for (int i = 0; i < 4; i++) { + tire_loadneg2[i] = (tire->tire_load[i] < 0.f) ? tire->tire_load[i] : 0.f; + } + + for (int i = 0; i < 4; i++) { + tire->tire_load[i] = (tire->tire_load[i] > 0.f) ? tire->tire_load[i] : 0.f; + } + + if (tire_loadneg2[2] < 0 || tire_loadneg2[3] < 0) { + tire->tire_load[0] += tire_loadneg2[3]; + tire->tire_load[1] += tire_loadneg2[2]; + } + if (tire_loadneg2[0] < 0 || tire_loadneg2[1] < 0) { + tire->tire_load[0] += 0.5f * (tire_loadneg2[0] + tire_loadneg2[1]); + tire->tire_load[1] += 0.5f * (tire_loadneg2[0] + tire_loadneg2[1]); + } + + + } else if (tire_loadneg[2] < 0 || tire_loadneg[3] < 0) { + float WT_x = 0.5f * (x_WT_ns + x_WT_s); + float WT_y = (y_WT_ns_f + y_WT_s_g_f + y_WT_s_e_f); + + + tire_loadtx[2] = -WT_x / (-WT_x + fabs(WT_y)) * tire_loadneg[2]; + tire_loadtx[3] = -WT_x / (-WT_x + fabs(WT_y)) * tire_loadneg[3]; + tire_loadty[2] = WT_y / (-WT_x + fabs(WT_y)) * tire_loadneg[2] * ((sensors->acceleration_y > 0) - (sensors->acceleration_y < 0)); + tire_loadty[3] = WT_y / (-WT_x + fabs(WT_y)) * tire_loadneg[3] * ((sensors->acceleration_y > 0) - (sensors->acceleration_y < 0)); + + tire->tire_load[0] += tire_loadty[2]; + tire->tire_load[1] += tire_loadty[3]; + tire->tire_load[2] += tire_loadtx[3]; + tire->tire_load[3] += tire_loadtx[2]; + + for (int i = 0; i < 4; i++) { + tire_loadneg2[i] = (tire->tire_load[i] < 0.f) ? tire->tire_load[i] : 0.f; + } + + for (int i = 0; i < 4; i++) { + tire->tire_load[i] = (tire->tire_load[i] > 0.f) ? tire->tire_load[i] : 0.f; + } + + if (tire_loadneg2[0] < 0 || tire_loadneg2[1] < 0) { + tire->tire_load[2] += tire_loadneg2[1]; + tire->tire_load[3] += tire_loadneg2[0]; + } + if (tire_loadneg2[2] < 0 || tire_loadneg2[3] < 0) { + tire->tire_load[0] += 0.5f * (tire_loadneg2[2] + tire_loadneg2[3]); + tire->tire_load[1] += 0.5f * (tire_loadneg2[2] + tire_loadneg2[3]); + } + } +} + +void Calculate_Tire_Forces(TIRE *tire, const float slip_angle[4], const float slip_ratio[4]) { + PAC pac = { + .kAlphaP = PAC_KALPHAP, + .kLambdaP = PAC_KLAMBDA_P, + .Blat = PAC_BLAT, + .Blon = PAC_BLON, + .Dlat = PAC_DLAT, + .Clat = PAC_CLAT, + .Dlon = PAC_DLON, + .Clon = PAC_CLON + }; + + float alpha_star[4]; + float lambda_star[4]; + float s_star[4]; + float slip_angle_local[4]; + float slip_ratio_local[4]; + float min_s_star = 0.01f; + + //Combined slip + for (int i = 0; i < 4; i++) { + alpha_star[i] = slip_angle[i] / pac.kAlphaP; + lambda_star[i] = slip_ratio[i] / pac.kLambdaP; + float val_s_star = sqrtf(alpha_star[i] * alpha_star[i] + lambda_star[i] * lambda_star[i]); + s_star[i] = (val_s_star > min_s_star) ? val_s_star : min_s_star; + + slip_angle_local[i] = s_star[i] * pac.kAlphaP; + slip_ratio_local[i] = s_star[i] * pac.kLambdaP; + + float fy_pure = tire->tire_load[i] * pac.Dlat * sinf(pac.Clat * atanf(pac.Blat * slip_angle_local[i])); + float fx_pure = tire->tire_load[i] * pac.Dlon * sinf(pac.Clon * atanf(pac.Blon * slip_ratio_local[i])); + + tire->force_fy[i] = fy_pure * alpha_star[i] / s_star[i]; + tire->force_fx[i] = fx_pure * lambda_star[i] / s_star[i]; + } + + tire->Mz = 0.0f; + tire->force_fx_rolling = 0.0f; +} \ No newline at end of file diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index e2ce9dfb..e1061f5e 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -24,13 +24,13 @@ ControlSim::ControlSim() : Node("control_sim") { this->get_parameter("rdyn", kRdyn); this->get_parameter("gear_ratio", kGearRatio); + clock_ = std::make_shared(RCL_SYSTEM_TIME); // Timers timer_receive_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&ControlSim::receive_can, this)); - timer_send_ = this->create_wall_timer(std::chrono::milliseconds(5), - std::bind(&ControlSim::send_torque, this)); - timer_state_ = this->create_wall_timer(std::chrono::milliseconds(100), - std::bind(&ControlSim::send_state, this)); + timer_defaultTask_ = this->create_wall_timer(std::chrono::milliseconds(5), + std::bind(&ControlSim::default_task, this)); + // CAN socket can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); @@ -40,6 +40,15 @@ ControlSim::ControlSim() : Node("control_sim") { addr_.can_ifindex = ifr_.ifr_ifindex; bind(can_socket_, (struct sockaddr *)&addr_, sizeof(addr_)); + //Initialize state + std::vector state = {0.0f, 0.0f, 0.0f}; + pid.init = 0; + Parameters_Init(¶meters); + Estimation_Init(); + PowerControl_Init(¶meters); + TractionControl_Init(&pid, ¶meters); + TorqueVectoring_Init(&pid); + } @@ -52,74 +61,169 @@ ControlSim::ControlSim() : Node("control_sim") { acc_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); yaw_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); - acc_ = acc_scaled_ / 100.0; - target_r_ = yaw_scaled_ / 1000.0; - float max_torque = 21 * 1000 / 9.8; - - float F = acc_ * kMass; - torque_i_ = (int)(F * 0.225 / 4.0 * 1000.0 / 9.8); + dv.acc = acc_scaled_ / 100.0; + dv.target_r = yaw_scaled_ / 1000.0; } + else if (frame.can_id == 0x134){ + int16_t steering_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + sensors.steering_angle = steering_scaled_* -0.000031688042484 + 0.476959989071; + } + else if (frame.can_id == 0x1A0) { vx_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); vy_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); - vx_ = vx_scaled_ * 0.2 / 3.6; - vy_ = vy_scaled_ * 0.2 / 3.6; + sensors.speed_x = vx_scaled_ * 0.2 / 3.6; + sensors.speed_y = vy_scaled_ * 0.2 / 3.6; + + } + else if (frame.can_id == 0x1A3) { + int16_t acc_x_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + int16_t acc_y_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); + int16_t acc_z_scaled_ = static_cast((frame.data[5] << 8) | frame.data[4]); + sensors.acceleration_x = acc_x_scaled_ * 0.02; + sensors.acceleration_y = acc_y_scaled_ * 0.02; + sensors.acceleration_z = acc_z_scaled_ * 0.02; } - else if (frame.can_id == 0x1A4) { - r_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); - r_ = r_scaled_ * 0.00034906585; + else if (frame.can_id == 0x1A4) { + int16_t pitch_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + int16_t roll_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); + r_scaled_ = static_cast((frame.data[5] << 8) | frame.data[4]); + + sensors.angular_x = pitch_scaled_ * 0.02f * 0.0174532; + sensors.angular_y = roll_scaled_ * 0.02f * 0.0174532; + sensors.angular_z = r_scaled_ * 0.02f * 0.0174532; } + else if (frame.can_id == 0x102){ + int32_t ws_scaled_0 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[0] = ws_scaled_0 * 0.00000018879763543; + } + else if (frame.can_id == 0x106){ + int32_t ws_scaled_1 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[1] = ws_scaled_1 * 0.00000018879763543; + } + else if (frame.can_id == 0x110){ + int32_t ws_scaled_2 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[2] = ws_scaled_2 * 0.00000018879763543; + } + else if (frame.can_id == 0x114){ + int32_t ws_scaled_3 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[3] = ws_scaled_3 * 0.00000018879763543; + } + } -void ControlSim::send_torque() +void ControlSim::Parameters_Init(Parameters *p) { + p->gear_ratio = DEFAULT_GEAR_RATIO; + p->maximum_power = DEFAULT_MAX_POWER; + p->minimum_power = DEFAULT_MIN_POWER; + p->V_max = DEFAULT_MAX_VOLTAGE; + p->V_min = DEFAULT_MIN_VOLTAGE; + p->wheelbase = DEFAULT_WB; + p->rdyn = DEFAULT_RDYN; + p->trackwidthF = DEFAULT_TW; + p->trackwidthR = DEFAULT_TW; + p->wheel_inertia = DEFAULT_WHEEL_INERTIA; + for (int i = 0; i < 4; i++) { + p->torque_limit_positive[i] = DEFAULT_TL_POS; + p->torque_limit_negative[i] = DEFAULT_TL_NEG; + } + p->mass = DEFAULT_MASS; + p->g = GRAVITY; + p->r_cdg =DEFAULT_R_CDG; + p->lf = p->wheelbase * p->r_cdg; + p->lr = p->wheelbase * (1 - p->r_cdg); + p->nsm_f = DEFAULT_NSM; + p->nsm_r = DEFAULT_NSM; + p->sm= p->mass - p->nsm_f - p->nsm_r; + p->sm_f = p->sm * p->r_cdg; + p->sm_r = p->sm * (1- p->r_cdg); + p->h_cdg = DEFAULT_H_CDG; + p->h_cdg_nsm_f = DEFAULT_H_CDG_NSM; + p->h_cdg_nsm_r= DEFAULT_H_CDG_NSM; + p->h_cdg_sm = DEFAULT_H_CDG_SM; + p->h_RC_f = DEFAULT_H_RC_F; + p->h_RC_r = DEFAULT_H_RC_R; + p->h_RA = p->h_RC_f + (p->h_RC_r - p->h_RC_f) * p->lf / p->wheelbase; + + float MR_ARB_f = MR_ARB_f_DIRK * 180 /pi * 1 / (r_ARB_f * 1000 * cos(psi_ARB_f)); + p->RS_f = 0.5f * p->trackwidthF * p->trackwidthF * tan(pi/180) * (K_s_f / (MR_s * MR_s) + K_ARB_f / (MR_ARB_f * MR_ARB_f)); + float MR_ARB_r = MR_ARB_f_DIRK * 180 /pi * 1 / (r_ARB_r * 1000 * cos(psi_ARB_r)); + p->RS_r = 0.5f * p->trackwidthF * p->trackwidthF * tan(pi/180) * (K_s_r / (MR_s * MR_s) + K_ARB_r / (MR_ARB_r * MR_ARB_r)); + p->RS = p->RS_f + p->RS_r; + + p->rho = DEFAULT_RHO; + p->CDA = DEFAULT_CDA; + p->CLA = DEFAULT_CLA; + p->r_cdp = DEFAULT_R_CDP; + p->h_cdp = DEFAULT_H_CDP; + + p->fz_params[0] = 0.5f * DEFAULT_RHO * DEFAULT_CLA; + p->fz_params[1] = 0.5f * DEFAULT_RHO * DEFAULT_CDA; + p->fz_params[2] = DEFAULT_NSM *DEFAULT_H_CDG_NSM/ DEFAULT_TW; + p->fz_params[3] = p->sm_f * DEFAULT_H_RC_F / DEFAULT_TW; + p->fz_params[4] = p->sm_r * DEFAULT_H_RC_R / DEFAULT_TW; + p->fz_params[5] = p->sm * (DEFAULT_H_CDG_SM - p->h_RA) * p->RS_f / p->RS / DEFAULT_TW; + p->fz_params[6] = p->sm * (DEFAULT_H_CDG_SM - p->h_RA) * p->RS_r / p->RS / DEFAULT_TW; + p->fz_params[7] = (DEFAULT_NSM * DEFAULT_H_CDG_NSM * 2)/ DEFAULT_WB; + p->fz_params[8] = p->sm * DEFAULT_H_CDG_SM / DEFAULT_WB; + p->fz_params[9] = 0.5 * DEFAULT_MASS * GRAVITY * p->lf / DEFAULT_WB; + p->fz_params[10] = 0.5 * DEFAULT_R_CDP; + p->fz_params[11] = 0.5 * DEFAULT_H_CDP / DEFAULT_WB; + p->fz_params[12] = 0.5 * DEFAULT_MASS* GRAVITY * p->lr / DEFAULT_WB; + +} + +void ControlSim::send_torque(float torque_out[4]) { std::array ids = {0x200, 0x203, 0x206, 0x209}; - for (auto id : ids) { - frame.can_id = id; + for (size_t i = 0; i < ids.size(); ++i) { + torque_i_ = static_cast(torque_out[i] * 1000 / 9.8); + + frame.can_id = ids[i]; frame.can_dlc = 8; - frame.data[0] = 0x00; - frame.data[1] = 0x07; + + frame.data[0] = 0x00; + frame.data[1] = 0x07; frame.data[2] = torque_i_ & 0xFF; frame.data[3] = (torque_i_ >> 8) & 0xFF; - uint16_t torque_max = 1000; - uint16_t torque_min = -1000; + int16_t torque_max = 1000; + int16_t torque_min = -1000; frame.data[4] = torque_max & 0xFF; frame.data[5] = (torque_max >> 8) & 0xFF; frame.data[6] = torque_min & 0xFF; frame.data[7] = (torque_min >> 8) & 0xFF; write(can_socket_, &frame, sizeof(struct can_frame)); - - - } + } } + void ControlSim::send_state(){ frame.can_id = 0x122; frame.can_dlc = 6; - vx_i_ = (int)(vx_ * 100.0f); + vx_i_ = (int)(state[0]* 100.0f); frame.data[0] = vx_i_ & 0xFF; frame.data[1] = (vx_i_ >> 8) & 0xFF; - vy_i_ = (int)(vy_ * 100.0f); + vy_i_ = (int)(state[1] * 100.0f); frame.data[2] = vy_i_ & 0xFF; frame.data[3] = (vy_i_ >> 8) & 0xFF; - r_i_ = (int)(r_ * 1000.0f); + r_i_ = (int)(state[2] * 1000.0f); frame.data[4] = r_i_ & 0xFF; frame.data[5] = (r_i_ >> 8) & 0xFF; write(can_socket_, &frame, sizeof(struct can_frame)); @@ -127,6 +231,40 @@ void ControlSim::send_state(){ } +void ControlSim::default_task() +{ + rclcpp::Time current_time = clock_->now(); + if (pid.init == 0){ + pid.last_timestamp = current_time.seconds(); + } + else{ + pid.TS = current_time.seconds() - pid.last_timestamp; + pid.last_timestamp = current_time.seconds(); + } + + Estimation_Update(&sensors, ¶meters, state); + send_state(); + fx_request = pc_request(&dv, ¶meters); + std::cout << "f " << fx_request + << std::endl; + Calculate_Tire_Loads(&sensors, ¶meters, state, &tire); + TorqueVectoring_Update(&sensors, ¶meters, &pid, &tire, &dv, fx_request, state, TV_out); + TractionControl_Update(&sensors, ¶meters, &pid, &tire, TV_out, TC_out, SR, &dv); + PowerControl_Update(&sensors, ¶meters, TC_out); + std::cout << "TC_out = [" + << TV_out[0] << ", " + << TV_out[1] << ", " + << TV_out[2] << ", " + << TV_out[3] << "]" + << std::endl; + + if (pid.init == 0){ + pid.init = 1; + } + + send_torque(TC_out); +} + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); diff --git a/src/arussim/src/control_sim/estimation.c b/src/arussim/src/control_sim/estimation.c new file mode 100644 index 00000000..12a966ca --- /dev/null +++ b/src/arussim/src/control_sim/estimation.c @@ -0,0 +1,22 @@ +/* + * estimation.c + * + * Created on: Jan 29, 2025 + * Author: carme + */ + + +#include "arussim/control_sim/estimation.h" + + +void Estimation_Init() +{ + +} +void Estimation_Update(SensorData *sensors, Parameters *params, float estimation_out[3]){ + + // Use wheel speeds' mean and kinematic bicycle model + estimation_out[0] = sensors->speed_x;//0.25*(params->rdyn/params->gear_ratio)*(sensors->motor_speed[0]+sensors->motor_speed[1]+sensors->motor_speed[2]+sensors->motor_speed[3]); + estimation_out[1] = sensors->speed_y;//params->lr/params->wheelbase*tanf(sensors->steering_angle)*estimation_out[0]; + estimation_out[2] = sensors->angular_z; +} \ No newline at end of file diff --git a/src/arussim/src/control_sim/power_control.c b/src/arussim/src/control_sim/power_control.c new file mode 100644 index 00000000..b31b0f0e --- /dev/null +++ b/src/arussim/src/control_sim/power_control.c @@ -0,0 +1,31 @@ +/* + * power_control.c + * + * Created on: Jan 29, 2025 + * Author: carme + */ + +#include "arussim/control_sim/power_control.h" + + + +void PowerControl_Init(Parameters *parameters) +{ + +} +void PowerControl_Update(SensorData *sensors, Parameters *parameters, float * torque_out) { + + float PW_total = torque_out[0] * sensors->motor_speed[0] + torque_out[1]*sensors->motor_speed[1] + + torque_out[2]*sensors->motor_speed[2]+ torque_out[3]* sensors->motor_speed[3]; + + if (PW_total > parameters->maximum_power) { + + for (int i = 0; i < 4; i++) { + if (sensors->motor_speed[i] <= 0) { + torque_out[i] = 0; + } else { + torque_out[i] = parameters->maximum_power / (4 * sensors->motor_speed[i]); + } + } + } +} \ No newline at end of file diff --git a/src/arussim/src/control_sim/torque_vectoring.c b/src/arussim/src/control_sim/torque_vectoring.c new file mode 100644 index 00000000..b9c91c6d --- /dev/null +++ b/src/arussim/src/control_sim/torque_vectoring.c @@ -0,0 +1,117 @@ +/* + * torque_vectoring.c + * + * Created on: Apr 19, 2025 + * Author: carme + */ + +#include "arussim/control_sim/torque_vectoring.h" + +float tire_load[4]; +uint8_t TV_active; +float target_r; + +void TorqueVectoring_Init(PID *pid) { + + pid->TV_Kd= TV_KD; + pid->TV_Ki= TV_KI; + pid->TV_Kp= TV_KP; + pid->TV_N= DEFAULT_TV_N; + pid->max_Mz = DEFAULT_MAX_MZ; +} + +void TorqueVectoring_Update(SensorData *sensors, Parameters *parameters, PID *pid, TIRE *tire, DV *dv, float fx_request, float *state, float *torque_out) { + if (TV_active == 1){ + float Mz_request = Target_Generation(sensors, parameters, pid, dv, state); + +// float fz_front_mean = 0.5f * (tire->tire_load[0] + tire->tire_load[1]); +// float fz_rear_mean = 0.5f * (tire->tire_load[2] + tire->tire_load[3]); +// +// float fx_dist[4] = { fx_request * fz_front_mean, fx_request * fz_front_mean, fx_request * fz_rear_mean, fx_request * fz_rear_mean }; +// +// float mz_dist[4] = { 2.0f * Mz_request * tire->tire_load[0] / (-parameters->trackwidthF), +// 2.0f * Mz_request * tire->tire_load[1] / (parameters->trackwidthF), +// 2.0f * Mz_request * tire->tire_load[2] / (-parameters->trackwidthR), +// 2.0f * Mz_request * tire->tire_load[3] / (parameters->trackwidthR) }; +// +// float sum_fz = tire->tire_load[0] + tire->tire_load[1] + tire->tire_load[2] + tire->tire_load[3]; +// +// for (int i = 0; i < 4; ++i) { +// torque_out[i] = parameters->rdyn * (fx_dist[i] + mz_dist[i]) / sum_fz / parameters->gear_ratio; +// +// if (torque_out[i] > parameters->torque_limit_positive[i]) { +// torque_out[i] = parameters->torque_limit_positive[i]; +// } else if (torque_out[i] < parameters->torque_limit_negative[i]) { +// torque_out[i] = parameters->torque_limit_negative[i]; +// } +// } + + torque_out[0] = parameters->rdyn*(0.25*fx_request - Mz_request/parameters->trackwidthR); + torque_out[1] = parameters->rdyn*(0.25*fx_request - Mz_request/parameters->trackwidthR); + torque_out[2] = parameters->rdyn*(0.25*fx_request - Mz_request/parameters->trackwidthR); + torque_out[3] = parameters->rdyn*(0.25*fx_request + Mz_request/parameters->trackwidthR); + + for(int i=0; i<4; i++){ + if (torque_out[i] > parameters->torque_limit_positive[i]) { + torque_out[i] = parameters->torque_limit_positive[i]; + } else if (torque_out[i] < parameters->torque_limit_negative[i]) { + torque_out[i] = parameters->torque_limit_negative[i]; + } + } + + } else { + for (int i = 0; i < 4; ++i) { + torque_out[i] = parameters->rdyn * fx_request / 4; + + if (torque_out[i] > parameters->torque_limit_positive[i]) { + torque_out[i] = parameters->torque_limit_positive[i]; + } else if (torque_out[i] < parameters->torque_limit_negative[i]) { + torque_out[i] = parameters->torque_limit_negative[i]; + } + } + } + + if (fx_request <= 0){ + for(int i = 0; i<4; ++i){ + if(torque_out[i] > 0.0){ + torque_out[i] = 0.0; + } + } + } + +} + +float Target_Generation(SensorData *sensors, Parameters *parameters, PID *pid, DV *dv, float *state) { + + //Calculate Error + if (dv->autonomous == 1){ + target_r = dv->target_r; + } + else{ + target_r = tan(sensors->steering_angle) * state[0] / parameters->wheelbase; + } + float error = target_r - state[2]; + + //PID Init + if (pid->init == 0){ + pid->err_prev = error; + pid->integral = 0.f; + pid->deriv_filt = 0.f; + return 0.f; + } + + //PID + pid-> integral += error * pid->TS; + float deriv = (error - pid->err_prev) / pid->TS; + pid->deriv_filt = (pid->TV_N * deriv + pid->deriv_filt) / (1 + pid->TV_N * pid->TS); + float Mz_request = pid->TV_Kp * error + pid->TV_Ki * pid->integral + pid->TV_Kd * pid->deriv_filt; + pid->err_prev = error; + + //Saturation + Mz_request = (Mz_request > pid->max_Mz) ? pid->max_Mz : Mz_request; + Mz_request = (Mz_request < -pid->max_Mz) ? -pid->max_Mz : Mz_request; + + if (state[0] < 3.0) {Mz_request = 0.0;} + return Mz_request; + +} \ No newline at end of file diff --git a/src/arussim/src/control_sim/traction_control.c b/src/arussim/src/control_sim/traction_control.c new file mode 100644 index 00000000..4dd9fdd7 --- /dev/null +++ b/src/arussim/src/control_sim/traction_control.c @@ -0,0 +1,144 @@ +/* + * traction_control.c + * + * Created on: Jan 29, 2025 + * Author: carme + */ + +#include "arussim/control_sim/traction_control.h" + + + +float vx_wheel_tire[4]; +//float SR[4]; +float TC_calc[4]; +float T_obj[4]; +float int_SRe[4]; +float int_SRep[4]; +float SR_e[4]; +float SR_e1[4]; +float SR_e1p[4]; +float slip_angle[4]; + + +void TractionControl_Init(PID *pid, Parameters *parameters) { + pid->TC_K = TC_K_; + pid->TC_Ti = TC_TI; + pid->TC_Td = TC_TD; + pid->init = 0; + for (int i = 0; i < 4; i ++){ + int_SRe[i] = 0.f; + int_SRep [i] = 0.f; + SR_e [i] = 0.f; + SR_e1 [i] = 0.f; + SR_e1p [i] = 0.f; + slip_angle[i] = 0.f; + + } + parameters->v0 = DEFAULT_TC_V0; + parameters->v_gain = DEFAULT_TC_V_GAIN; +} +void TractionControl_Update(SensorData *sensors, Parameters *parameters, PID *pid, TIRE *tire, float *Tin, float *TC, float *SR, DV *dv) { + if (TC_ACTIVE == 1 && pid->init == 1 && !dv->inspection) { + float state[3] = {sensors->speed_x, sensors->speed_y, sensors->angular_z}; + int bool_min_speed = 0; + + //Calculate all necessary variables + float vx_wheel[4] = {state[0] + state[2] * 0.5f * (- parameters->trackwidthF), + state[0] + state[2] * 0.5f * parameters->trackwidthF, + state[0] + state[2] * 0.5f * (- parameters->trackwidthR), + state[0] + state[2] * 0.5f * parameters->trackwidthR, + }; + + float vy_wheel[4] = { + state[1] + state[2] * parameters->lf, + state[1] + state[2] * parameters->lf, + state[1] + state[2] * (- parameters->lr), + state[1] + state[2] * (- parameters->lr), + }; + + vx_wheel_tire[0] = vx_wheel[0] * cosf(sensors->steering_angle) + vy_wheel[0] * sinf(sensors->steering_angle); + vx_wheel_tire[1] = vx_wheel[1] * cosf(sensors->steering_angle) + vy_wheel[1] * sinf(sensors->steering_angle); + vx_wheel_tire[2] = vx_wheel[2]; + vx_wheel_tire[3] = vx_wheel[3]; + + for (int i = 0; i < 4; i++) { + if (vx_wheel_tire[i] < 1.f){ + bool_min_speed = 1; + break; + } + } + + for (int i = 0; i < 4; ++i) { + if (bool_min_speed == 1){ + SR[i] = parameters->rdyn * sensors->motor_speed[i] / parameters->gear_ratio - vx_wheel_tire[i]; + } + else{ + SR[i] = parameters->rdyn * sensors->motor_speed[i] / parameters->gear_ratio / (vx_wheel_tire[i] + eps) -1; + } + + } + + //Define SR target + float SR_t[4] = { 0.1f, 0.1f, 0.1f, 0.1f }; + + //Calculate feedfoward torque + Calculate_Tire_Forces(tire, slip_angle, SR_t); + + for (int i = 0; i < 4; i++) { + T_obj[i] = tire->force_fx[i] * parameters->rdyn / parameters->gear_ratio + sensors->acceleration_x / parameters->rdyn * parameters->wheel_inertia / parameters->gear_ratio; + + //Control the value + SR_e[i] = SR_t[i] - fabsf(SR[i]); + int_SRep[i] = int_SRe[i] + SR_e[i]; + SR_e1p [i] = SR_e[i]; + TC_calc[i] = T_obj[i] + pid->TC_K * SR_e[i] - (pid->TC_Td / pid->TS) * (SR_e[i] - SR_e1[i]); + TC[i] = fminf(TC_calc[i], fmaxf(Tin[i], -TC_calc[i])); + } + + + //Traction control activation logic + for (int i = 0; i < 4; i++) { + if (TC_calc[i] > TC[i]) { + int_SRep[i] = 0; + } + } + + //Low Speed limitation + if (state[0] < 4.f) { + float T_limit = parameters->v0 + parameters->v_gain * state[0]; + if (Tin[0] > T_limit) TC[0] = T_limit; + if (Tin[1] > T_limit) TC[1] = T_limit; + } + + //Saturation + for (int i = 0; i < 4; i++) { + TC[i] = (TC[i] > parameters->torque_limit_positive[i]) ? parameters->torque_limit_positive[i] : TC[i]; + TC[i] = (TC[i] < parameters->torque_limit_negative[i]) ? parameters->torque_limit_negative[i] : TC[i]; + } + + //memory + for (int i = 0; i < 4; i++) { + int_SRe[i] = int_SRep[i]; + SR_e1[i] = SR_e1p[i]; + } + +// // Ignore TC at low speed +// if(bool_min_speed==1){ +// for(int i=0; i<4; i++){ +// TC[i] = Tin[i]; +// } +// } + + } else { + + for (int i = 0; i < 4; i ++){ + TC[i] = Tin[i]; + + //Saturation + TC[i] = (TC[i] > parameters->torque_limit_positive[i]) ? parameters->torque_limit_positive[i] : TC[i]; + TC[i] = (TC[i] < parameters->torque_limit_negative[i]) ? parameters->torque_limit_negative[i] : TC[i]; + } + + } +} \ No newline at end of file From 2ac567748c35e2e44b561dae6443efb1d4199a22 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Sat, 8 Nov 2025 19:53:34 +0100 Subject: [PATCH 08/25] fix: torque limit parameter & yaw rate can frmae --- src/arussim/include/arussim/arussim_node.hpp | 1 - .../include/arussim/control_sim/Parameters.h | 6 +- .../arussim/control_sim/control_sim.hpp | 7 +- src/arussim/src/control_sim/control_sim.cpp | 139 +++++++++--------- .../src/control_sim/torque_vectoring.c | 2 +- src/arussim/src/sensors.cpp | 4 +- 6 files changed, 76 insertions(+), 83 deletions(-) diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index 5a69420b..69e6e0ca 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -231,6 +231,5 @@ class Simulator : public rclcpp::Node struct sockaddr_can addr_{}; struct can_frame frame_; std::thread thread_; - bool can_blocked_; }; \ No newline at end of file diff --git a/src/arussim/include/arussim/control_sim/Parameters.h b/src/arussim/include/arussim/control_sim/Parameters.h index aedf40ce..6f62bca9 100644 --- a/src/arussim/include/arussim/control_sim/Parameters.h +++ b/src/arussim/include/arussim/control_sim/Parameters.h @@ -15,7 +15,7 @@ extern "C" { #endif //VDC MODELS ACTIVATION -#define TV_ACTIVE 0 +#define TV_ACTIVE 1 #define TC_ACTIVE 0 #define EST_ACTIVE 0 @@ -62,8 +62,8 @@ extern "C" { #define DEFAULT_GEAR_RATIO 12.48 #define DEFAULT_RDYN 0.225 #define DEFAULT_WHEEL_INERTIA 0.4 -#define DEFAULT_TL_POS 21.0 //Pruebas borriqueta -#define DEFAULT_TL_NEG -21.0 //Pruebas borriqueta +#define DEFAULT_TL_POS 262.08 // TODO: with gr +#define DEFAULT_TL_NEG -262.08 // with gr //AERO #define DEFAULT_RHO 1.225 diff --git a/src/arussim/include/arussim/control_sim/control_sim.hpp b/src/arussim/include/arussim/control_sim/control_sim.hpp index 9cb372d0..20898996 100644 --- a/src/arussim/include/arussim/control_sim/control_sim.hpp +++ b/src/arussim/include/arussim/control_sim/control_sim.hpp @@ -49,6 +49,7 @@ class ControlSim : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_receive_; rclcpp::TimerBase::SharedPtr timer_defaultTask_; rclcpp::Clock::SharedPtr clock_; + std::thread thread_; @@ -71,9 +72,5 @@ class ControlSim : public rclcpp::Node float TV_out[4]; float fx_request; - rclcpp::Publisher::SharedPtr torque_pub_; - rclcpp::Publisher::SharedPtr cmd_pub_; - rclcpp::Publisher::SharedPtr control_vx_pub_; - rclcpp::Publisher::SharedPtr control_vy_pub_; - rclcpp::Publisher::SharedPtr control_r_pub_; + }; \ No newline at end of file diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index e1061f5e..4b2d77b8 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -17,17 +17,9 @@ ControlSim::ControlSim() : Node("control_sim") { - this->declare_parameter("mass", 348.0); - this->declare_parameter("rdyn", 0.225); - this->declare_parameter("gear_ratio", 12.48); - this->get_parameter("mass", kMass); - this->get_parameter("rdyn", kRdyn); - this->get_parameter("gear_ratio", kGearRatio); clock_ = std::make_shared(RCL_SYSTEM_TIME); // Timers - timer_receive_ = this->create_wall_timer(std::chrono::milliseconds(1), - std::bind(&ControlSim::receive_can, this)); timer_defaultTask_ = this->create_wall_timer(std::chrono::milliseconds(5), std::bind(&ControlSim::default_task, this)); @@ -40,6 +32,10 @@ ControlSim::ControlSim() : Node("control_sim") { addr_.can_ifindex = ifr_.ifr_ifindex; bind(can_socket_, (struct sockaddr *)&addr_, sizeof(addr_)); + + std::thread thread_(&ControlSim::receive_can, this); + thread_.detach(); + //Initialize state std::vector state = {0.0f, 0.0f, 0.0f}; pid.init = 0; @@ -54,69 +50,69 @@ ControlSim::ControlSim() : Node("control_sim") { void ControlSim::receive_can() { - - read(can_socket_, &frame, sizeof(struct can_frame)); - - if (frame.can_id == 0x222) { - acc_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); - yaw_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); - - dv.acc = acc_scaled_ / 100.0; - dv.target_r = yaw_scaled_ / 1000.0; + while (rclcpp::ok()) { + read(can_socket_, &frame, sizeof(struct can_frame)); + + if (frame.can_id == 0x222) { + acc_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + yaw_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); + + dv.acc = acc_scaled_ / 100.0; + dv.target_r = yaw_scaled_ / 1000.0; + + } + + else if (frame.can_id == 0x134){ + int16_t steering_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + sensors.steering_angle = steering_scaled_* -0.000031688042484 + 0.476959989071; + } + + else if (frame.can_id == 0x1A0) { + vx_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + vy_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); + + sensors.speed_x = vx_scaled_ * 0.2 / 3.6; + sensors.speed_y = vy_scaled_ * 0.2 / 3.6; + + } + else if (frame.can_id == 0x1A3) { + int16_t acc_x_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + int16_t acc_y_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); + int16_t acc_z_scaled_ = static_cast((frame.data[5] << 8) | frame.data[4]); + sensors.acceleration_x = acc_x_scaled_ * 0.02; + sensors.acceleration_y = acc_y_scaled_ * 0.02; + sensors.acceleration_z = acc_z_scaled_ * 0.02; + + } + + else if (frame.can_id == 0x1A4) { + int16_t pitch_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); + int16_t roll_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); + r_scaled_ = static_cast((frame.data[5] << 8) | frame.data[4]); + + sensors.angular_x = pitch_scaled_ * 0.02f * 0.0174532; + sensors.angular_y = roll_scaled_ * 0.02f * 0.0174532; + sensors.angular_z = r_scaled_ * 0.02f * 0.0174532; + + } + else if (frame.can_id == 0x102){ + int32_t ws_scaled_0 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[0] = ws_scaled_0 * 0.00000018879763543; + } + else if (frame.can_id == 0x106){ + int32_t ws_scaled_1 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[1] = ws_scaled_1 * 0.00000018879763543; + } + else if (frame.can_id == 0x110){ + int32_t ws_scaled_2 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[2] = ws_scaled_2 * 0.00000018879763543; + } + else if (frame.can_id == 0x114){ + int32_t ws_scaled_3 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[3] = ws_scaled_3 * 0.00000018879763543; + } } - - else if (frame.can_id == 0x134){ - int16_t steering_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); - sensors.steering_angle = steering_scaled_* -0.000031688042484 + 0.476959989071; - } - - else if (frame.can_id == 0x1A0) { - vx_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); - vy_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); - - sensors.speed_x = vx_scaled_ * 0.2 / 3.6; - sensors.speed_y = vy_scaled_ * 0.2 / 3.6; - - } - else if (frame.can_id == 0x1A3) { - int16_t acc_x_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); - int16_t acc_y_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); - int16_t acc_z_scaled_ = static_cast((frame.data[5] << 8) | frame.data[4]); - sensors.acceleration_x = acc_x_scaled_ * 0.02; - sensors.acceleration_y = acc_y_scaled_ * 0.02; - sensors.acceleration_z = acc_z_scaled_ * 0.02; - - } - - else if (frame.can_id == 0x1A4) { - int16_t pitch_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); - int16_t roll_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); - r_scaled_ = static_cast((frame.data[5] << 8) | frame.data[4]); - - sensors.angular_x = pitch_scaled_ * 0.02f * 0.0174532; - sensors.angular_y = roll_scaled_ * 0.02f * 0.0174532; - sensors.angular_z = r_scaled_ * 0.02f * 0.0174532; - - } - else if (frame.can_id == 0x102){ - int32_t ws_scaled_0 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); - sensors.motor_speed[0] = ws_scaled_0 * 0.00000018879763543; - } - else if (frame.can_id == 0x106){ - int32_t ws_scaled_1 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); - sensors.motor_speed[1] = ws_scaled_1 * 0.00000018879763543; - } - else if (frame.can_id == 0x110){ - int32_t ws_scaled_2 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); - sensors.motor_speed[2] = ws_scaled_2 * 0.00000018879763543; - } - else if (frame.can_id == 0x114){ - int32_t ws_scaled_3 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); - sensors.motor_speed[3] = ws_scaled_3 * 0.00000018879763543; - } - - } @@ -245,14 +241,15 @@ void ControlSim::default_task() Estimation_Update(&sensors, ¶meters, state); send_state(); fx_request = pc_request(&dv, ¶meters); - std::cout << "f " << fx_request + std::cout << "f " << fx_request << " acc " << dv.acc << std::endl; + Calculate_Tire_Loads(&sensors, ¶meters, state, &tire); TorqueVectoring_Update(&sensors, ¶meters, &pid, &tire, &dv, fx_request, state, TV_out); TractionControl_Update(&sensors, ¶meters, &pid, &tire, TV_out, TC_out, SR, &dv); PowerControl_Update(&sensors, ¶meters, TC_out); std::cout << "TC_out = [" - << TV_out[0] << ", " + << TV_out[0] / 12.48<< ", " << TV_out[1] << ", " << TV_out[2] << ", " << TV_out[3] << "]" diff --git a/src/arussim/src/control_sim/torque_vectoring.c b/src/arussim/src/control_sim/torque_vectoring.c index b9c91c6d..edcf4031 100644 --- a/src/arussim/src/control_sim/torque_vectoring.c +++ b/src/arussim/src/control_sim/torque_vectoring.c @@ -4,7 +4,7 @@ * Created on: Apr 19, 2025 * Author: carme */ - +//TODO: Add gear ratio here and in vehicle dynamics #include "arussim/control_sim/torque_vectoring.h" float tire_load[4]; diff --git a/src/arussim/src/sensors.cpp b/src/arussim/src/sensors.cpp index f4492339..74191ea5 100644 --- a/src/arussim/src/sensors.cpp +++ b/src/arussim/src/sensors.cpp @@ -105,8 +105,8 @@ Sensors::Sensors() : Node("sensors") {"IMU/ay", {16, 31, true, 0.02, 0.0}}, }}, - {0x1A4, 2, { // IMU yaw_rate - {"IMU/yaw_rate", {0, 15, true, 0.000349065, 0.0}} + {0x1A4, 6, { // IMU yaw_rate + {"IMU/yaw_rate", {32, 47, true, 0.000349065, 0.0}} }}, {0x134, 2, { // Extensometer {"extensometer", {0, 15, true, -0.000031688042484, 0.476959989071}} From d142181777e88a3261d310ed0cc4bf7375460f49 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Tue, 11 Nov 2025 12:12:33 +0100 Subject: [PATCH 09/25] fix: launch bug --- src/arussim/launch/arussim_launch.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/arussim/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index 8daa1761..e98c3582 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -20,7 +20,7 @@ def generate_launch_description(): period=1.0, actions=[ ExecuteProcess( - cmd=['sudo','ip', 'link', 'add', 'dev', 'can0', 'type', 'vcan'], + cmd=['bash', '-c', "ip link show can0 >/dev/null 2>&1 || sudo ip link add dev can0 type vcan"], shell=False ) ] @@ -35,13 +35,18 @@ def generate_launch_description(): ] ) create_can1 = ExecuteProcess( - cmd=['sudo','ip', 'link', 'add', 'dev', 'can1', 'type', 'vcan'], - shell=False + cmd=['bash', '-c', "ip link show can1 >/dev/null 2>&1 || sudo ip link add dev can1 type vcan"], + shell=False ) - up_can1 = ExecuteProcess( - cmd=['sudo','ip', 'link', 'set', 'up', 'can1'], - shell=False + up_can1 = TimerAction( + period=2.0, + actions=[ + ExecuteProcess( + cmd=['sudo','ip', 'link', 'set', 'up', 'can1'], + shell=False + ) + ] ) From 3b93c1ea27057a4cfa1ba4f246d7a02737bcf10f Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Tue, 11 Nov 2025 12:59:38 +0100 Subject: [PATCH 10/25] fix: reset bug --- src/arussim/src/main_interface.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/arussim/src/main_interface.cpp b/src/arussim/src/main_interface.cpp index c803f6d4..5dc9c399 100644 --- a/src/arussim/src/main_interface.cpp +++ b/src/arussim/src/main_interface.cpp @@ -199,6 +199,7 @@ void MainInterface::update_lap_time_labels(double lap_time_) */ void MainInterface::launch_button_clicked() { + if (simulation_process_ == nullptr) { simulation_process_ = new QProcess(this); // Merge standard output and error @@ -211,6 +212,10 @@ void MainInterface::launch_button_clicked() args << "launch" << "common_meta" << launch_file; simulation_process_->start("ros2", args); } + QProcess can_process; + can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "can0" << "up"); + can_process.waitForFinished(); + } /** @@ -234,6 +239,11 @@ void MainInterface::stop_button_clicked() */ void MainInterface::reset_button_clicked() { + + QProcess can_process; + can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "can0" << "down"); + can_process.waitForFinished(); + auto msg = std_msgs::msg::Bool(); msg.data = true; reset_pub_->publish(msg); From 2d48fcc001a9effc7ec89f601ec49ddb13904a02 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Tue, 11 Nov 2025 14:34:16 +0100 Subject: [PATCH 11/25] refactor: use gear ratio --- src/arussim/config/simulator_config.yaml | 1 + src/arussim/include/arussim/arussim_node.hpp | 1 + .../include/arussim/control_sim/Parameters.h | 6 +++--- src/arussim/src/arussim_node.cpp | 4 +++- src/arussim/src/control_sim/control_sim.cpp | 8 -------- src/arussim/src/control_sim/torque_vectoring.c | 15 +++++++++------ 6 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/arussim/config/simulator_config.yaml b/src/arussim/config/simulator_config.yaml index d8939ea5..e35d616f 100755 --- a/src/arussim/config/simulator_config.yaml +++ b/src/arussim/config/simulator_config.yaml @@ -11,6 +11,7 @@ arussim: COG_front_dist: 1.9 # m COG_back_dist: -1.0 # m car_width: 0.8 # m + gear_ratio: 12.48 sensor: lidar_fov: 25.0 # m Perception circular range diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index 69e6e0ca..82ae43bd 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -94,6 +94,7 @@ class Simulator : public rclcpp::Node double kSimulationSpeedMultiplier; bool kTorqueVectoring; bool kDebug; + double kGearRatio; //Car boundaries double kCOGFrontDist; diff --git a/src/arussim/include/arussim/control_sim/Parameters.h b/src/arussim/include/arussim/control_sim/Parameters.h index 6f62bca9..9602c2e8 100644 --- a/src/arussim/include/arussim/control_sim/Parameters.h +++ b/src/arussim/include/arussim/control_sim/Parameters.h @@ -15,7 +15,7 @@ extern "C" { #endif //VDC MODELS ACTIVATION -#define TV_ACTIVE 1 +#define TV_ACTIVE 0 #define TC_ACTIVE 0 #define EST_ACTIVE 0 @@ -62,8 +62,8 @@ extern "C" { #define DEFAULT_GEAR_RATIO 12.48 #define DEFAULT_RDYN 0.225 #define DEFAULT_WHEEL_INERTIA 0.4 -#define DEFAULT_TL_POS 262.08 // TODO: with gr -#define DEFAULT_TL_NEG -262.08 // with gr +#define DEFAULT_TL_POS 21. +#define DEFAULT_TL_NEG -21. //AERO #define DEFAULT_RHO 1.225 diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index 06c1cd5e..becc248a 100755 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -37,6 +37,7 @@ Simulator::Simulator() : Node("simulator") this->declare_parameter("csv_state", false); this->declare_parameter("csv_vehicle_dynamics", false); this->declare_parameter("debug", false); + this->declare_parameter("vehicle.gear_ratio", 12.48); this->get_parameter("track", kTrackName); this->get_parameter("state_update_rate", kStateUpdateRate); @@ -60,6 +61,7 @@ Simulator::Simulator() : Node("simulator") this->get_parameter("csv_state", kCSVState); this->get_parameter("csv_vehicle_dynamics", kCSVVehicleDynamics); this->get_parameter("debug", kDebug); + this->get_parameter("vehicle.gear_ratio", kGearRatio); clock_ = std::make_shared(RCL_SYSTEM_TIME); tf_broadcaster_ = std::make_shared(this); @@ -424,7 +426,7 @@ void Simulator::receive_can() frame_.can_id == 0x206 || frame_.can_id == 0x209) { int idx = (frame_.can_id - 0x200) / 3; // 0,1,2,3 int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); - can_torque_cmd_.at(idx) = torque_scaled * 9.8f / 1000.0f; + can_torque_cmd_.at(idx) = torque_scaled * 9.8 / 1000.0 * kGearRatio; } } } diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index 4b2d77b8..bd0e72a4 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -241,19 +241,11 @@ void ControlSim::default_task() Estimation_Update(&sensors, ¶meters, state); send_state(); fx_request = pc_request(&dv, ¶meters); - std::cout << "f " << fx_request << " acc " << dv.acc - << std::endl; Calculate_Tire_Loads(&sensors, ¶meters, state, &tire); TorqueVectoring_Update(&sensors, ¶meters, &pid, &tire, &dv, fx_request, state, TV_out); TractionControl_Update(&sensors, ¶meters, &pid, &tire, TV_out, TC_out, SR, &dv); PowerControl_Update(&sensors, ¶meters, TC_out); - std::cout << "TC_out = [" - << TV_out[0] / 12.48<< ", " - << TV_out[1] << ", " - << TV_out[2] << ", " - << TV_out[3] << "]" - << std::endl; if (pid.init == 0){ pid.init = 1; diff --git a/src/arussim/src/control_sim/torque_vectoring.c b/src/arussim/src/control_sim/torque_vectoring.c index edcf4031..1972dd56 100644 --- a/src/arussim/src/control_sim/torque_vectoring.c +++ b/src/arussim/src/control_sim/torque_vectoring.c @@ -11,6 +11,7 @@ float tire_load[4]; uint8_t TV_active; float target_r; + void TorqueVectoring_Init(PID *pid) { pid->TV_Kd= TV_KD; @@ -46,10 +47,10 @@ void TorqueVectoring_Update(SensorData *sensors, Parameters *parameters, PID *pi // } // } - torque_out[0] = parameters->rdyn*(0.25*fx_request - Mz_request/parameters->trackwidthR); - torque_out[1] = parameters->rdyn*(0.25*fx_request - Mz_request/parameters->trackwidthR); - torque_out[2] = parameters->rdyn*(0.25*fx_request - Mz_request/parameters->trackwidthR); - torque_out[3] = parameters->rdyn*(0.25*fx_request + Mz_request/parameters->trackwidthR); + torque_out[0] = 0.; + torque_out[1] = 0.; + torque_out[2] = parameters->rdyn/parameters->gear_ratio*(0.5*fx_request - Mz_request/parameters->trackwidthR); + torque_out[3] = parameters->rdyn/parameters->gear_ratio*(0.5*fx_request + Mz_request/parameters->trackwidthR); for(int i=0; i<4; i++){ if (torque_out[i] > parameters->torque_limit_positive[i]) { @@ -60,8 +61,10 @@ void TorqueVectoring_Update(SensorData *sensors, Parameters *parameters, PID *pi } } else { - for (int i = 0; i < 4; ++i) { - torque_out[i] = parameters->rdyn * fx_request / 4; + torque_out[0]=0.; + torque_out[1]=0.; + for (int i = 2; i < 4; ++i) { + torque_out[i] = parameters->rdyn * fx_request / parameters->gear_ratio / 2; if (torque_out[i] > parameters->torque_limit_positive[i]) { torque_out[i] = parameters->torque_limit_positive[i]; From 2e366797bfa2ad5fb17a9c5587735fe2cd64fdb0 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Wed, 12 Nov 2025 11:55:44 +0100 Subject: [PATCH 12/25] refactor: use chain instead period at launch --- src/arussim/config/control_sim_config.yaml | 5 -- src/arussim/launch/arussim_launch.py | 76 ++++++++++++---------- src/arussim/src/main_interface.cpp | 4 +- 3 files changed, 44 insertions(+), 41 deletions(-) delete mode 100644 src/arussim/config/control_sim_config.yaml diff --git a/src/arussim/config/control_sim_config.yaml b/src/arussim/config/control_sim_config.yaml deleted file mode 100644 index 5557507e..00000000 --- a/src/arussim/config/control_sim_config.yaml +++ /dev/null @@ -1,5 +0,0 @@ -arussim: - ros__parameters: - mass: 250.0 - rdyn: 0.225 - gear_ratio: 12.48 \ No newline at end of file diff --git a/src/arussim/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index e98c3582..55b5f7cf 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -1,12 +1,13 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess, TimerAction +from launch.actions import ExecuteProcess, RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch_ros.actions import Node -def generate_launch_description(): - +def generate_launch_description(): + rviz_config_file = os.path.join( get_package_share_directory('arussim'), 'config', @@ -16,46 +17,55 @@ def generate_launch_description(): cmd=['sudo','modprobe', 'vcan'], shell=False ) - create_can0 = TimerAction( - period=1.0, - actions=[ - ExecuteProcess( + create_can0 = ExecuteProcess( cmd=['bash', '-c', "ip link show can0 >/dev/null 2>&1 || sudo ip link add dev can0 type vcan"], shell=False - ) - ] ) - up_can0 = TimerAction( - period=1.5, - actions=[ - ExecuteProcess( + up_can0 = ExecuteProcess( cmd=['sudo','ip', 'link', 'set', 'up', 'can0'], shell=False - ) - ] ) create_can1 = ExecuteProcess( cmd=['bash', '-c', "ip link show can1 >/dev/null 2>&1 || sudo ip link add dev can1 type vcan"], shell=False ) - - up_can1 = TimerAction( - period=2.0, - actions=[ - ExecuteProcess( - cmd=['sudo','ip', 'link', 'set', 'up', 'can1'], - shell=False - ) - ] - ) + up_can1 = ExecuteProcess( + cmd=['sudo','ip', 'link', 'set', 'up', 'can1'], + shell=False + ) + + # Chain processes to run one after another + sequence = [ + RegisterEventHandler( + OnProcessExit( + target_action=modprobe_vcan, + on_exit=[create_can0] + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=create_can0, + on_exit=[up_can0] + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=up_can0, + on_exit=[create_can1] + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=create_can1, + on_exit=[up_can1] + ) + ), + ] return LaunchDescription([ modprobe_vcan, - create_can0, - up_can0, - create_can1, - up_can1, + *sequence, Node( package='rviz2', executable='rviz2', @@ -74,9 +84,7 @@ def generate_launch_description(): exec='supervisor_exec', config='supervisor_config.yaml'), create_node(pkg='arussim', - exec='control_sim_exec', - config='control_sim_config.yaml'), - + exec='control_sim_exec'), ]) @@ -95,5 +103,5 @@ def create_node(pkg, config=None, exec=None, params=[]): executable=exec, name=pkg, output="screen", - parameters=[config_file]+params - ) \ No newline at end of file + parameters=[config_file] + params + ) diff --git a/src/arussim/src/main_interface.cpp b/src/arussim/src/main_interface.cpp index 5dc9c399..cbcb0460 100644 --- a/src/arussim/src/main_interface.cpp +++ b/src/arussim/src/main_interface.cpp @@ -213,7 +213,7 @@ void MainInterface::launch_button_clicked() simulation_process_->start("ros2", args); } QProcess can_process; - can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "can0" << "up"); + can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "up" << "can0"); can_process.waitForFinished(); } @@ -241,7 +241,7 @@ void MainInterface::reset_button_clicked() { QProcess can_process; - can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "can0" << "down"); + can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "down" << "can0"); can_process.waitForFinished(); auto msg = std_msgs::msg::Bool(); From 8503230e90a30c2fe5a9414cb769cc8b015f749b Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Wed, 12 Nov 2025 19:12:23 +0100 Subject: [PATCH 13/25] fix: use correct limit torque --- src/arussim/src/control_sim/control_sim.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index bd0e72a4..7d075938 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -193,8 +193,8 @@ void ControlSim::send_torque(float torque_out[4]) frame.data[2] = torque_i_ & 0xFF; frame.data[3] = (torque_i_ >> 8) & 0xFF; - int16_t torque_max = 1000; - int16_t torque_min = -1000; + int16_t torque_max = parameters.torque_limit_positive[i] * 1000 / 9.8; + int16_t torque_min = parameters.torque_limit_negative[i] * 1000 / 9.8; frame.data[4] = torque_max & 0xFF; frame.data[5] = (torque_max >> 8) & 0xFF; frame.data[6] = torque_min & 0xFF; From a570bdf432688e24967255d326d0c26a8026f8a6 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Wed, 12 Nov 2025 20:20:55 +0100 Subject: [PATCH 14/25] refactor: up and down just in main interface --- src/arussim/launch/arussim_launch.py | 21 --------------------- src/arussim/src/main_interface.cpp | 4 ++++ 2 files changed, 4 insertions(+), 21 deletions(-) diff --git a/src/arussim/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index 55b5f7cf..279bee4f 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -21,20 +21,11 @@ def generate_launch_description(): cmd=['bash', '-c', "ip link show can0 >/dev/null 2>&1 || sudo ip link add dev can0 type vcan"], shell=False ) - up_can0 = ExecuteProcess( - cmd=['sudo','ip', 'link', 'set', 'up', 'can0'], - shell=False - ) create_can1 = ExecuteProcess( cmd=['bash', '-c', "ip link show can1 >/dev/null 2>&1 || sudo ip link add dev can1 type vcan"], shell=False ) - up_can1 = ExecuteProcess( - cmd=['sudo','ip', 'link', 'set', 'up', 'can1'], - shell=False - ) - # Chain processes to run one after another sequence = [ RegisterEventHandler( @@ -46,21 +37,9 @@ def generate_launch_description(): RegisterEventHandler( OnProcessExit( target_action=create_can0, - on_exit=[up_can0] - ) - ), - RegisterEventHandler( - OnProcessExit( - target_action=up_can0, on_exit=[create_can1] ) ), - RegisterEventHandler( - OnProcessExit( - target_action=create_can1, - on_exit=[up_can1] - ) - ), ] return LaunchDescription([ diff --git a/src/arussim/src/main_interface.cpp b/src/arussim/src/main_interface.cpp index cbcb0460..edfa0a58 100644 --- a/src/arussim/src/main_interface.cpp +++ b/src/arussim/src/main_interface.cpp @@ -215,6 +215,8 @@ void MainInterface::launch_button_clicked() QProcess can_process; can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "up" << "can0"); can_process.waitForFinished(); + can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "up" << "can1"); + can_process.waitForFinished(); } @@ -243,6 +245,8 @@ void MainInterface::reset_button_clicked() QProcess can_process; can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "down" << "can0"); can_process.waitForFinished(); + can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "down" << "can1"); + can_process.waitForFinished(); auto msg = std_msgs::msg::Bool(); msg.data = true; From ca5c9a301c5303a34e3c0ab67a7a105e148b5b33 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Wed, 12 Nov 2025 20:31:55 +0100 Subject: [PATCH 15/25] feat: update power control --- src/arussim/src/control_sim/power_control.c | 48 +++++++++++++++++---- 1 file changed, 39 insertions(+), 9 deletions(-) diff --git a/src/arussim/src/control_sim/power_control.c b/src/arussim/src/control_sim/power_control.c index b31b0f0e..a7d08d62 100644 --- a/src/arussim/src/control_sim/power_control.c +++ b/src/arussim/src/control_sim/power_control.c @@ -16,16 +16,46 @@ void PowerControl_Init(Parameters *parameters) void PowerControl_Update(SensorData *sensors, Parameters *parameters, float * torque_out) { float PW_total = torque_out[0] * sensors->motor_speed[0] + torque_out[1]*sensors->motor_speed[1] + - torque_out[2]*sensors->motor_speed[2]+ torque_out[3]* sensors->motor_speed[3]; - - if (PW_total > parameters->maximum_power) { + torque_out[2]*sensors->motor_speed[2] + torque_out[3]* sensors->motor_speed[3]; + + // float r_battery = 1.2f; + // float PW_max_bms = sensors->V_soc*(sensors->V_soc - parameters->V_min) / r_battery; + // float PW_min_bms = sensors->V_soc*(sensors->V_soc - parameters->V_max) / r_battery; + + // float PW_max = (PW_max_bms < parameters->maximum_power) ? PW_max_bms : parameters->maximum_power; + // float PW_min = (PW_min_bms > parameters->minimum_power) ? PW_min_bms : parameters->minimum_power; + float PW_max = parameters->maximum_power; + float PW_min = parameters->minimum_power; + + if(PW_min > 0) PW_min = 0; + + if (PW_total > PW_max) { + + float PW_extra = (PW_total - PW_max)/PW_total; + for (int i = 2; i < 4; i++) { +// if (sensors->motor_speed[i] <= 0) { +// torque_out[i] = 0; +// } else { + torque_out[i] = torque_out[i] * (1 - PW_extra); +// } + } - for (int i = 0; i < 4; i++) { - if (sensors->motor_speed[i] <= 0) { - torque_out[i] = 0; - } else { - torque_out[i] = parameters->maximum_power / (4 * sensors->motor_speed[i]); - } + } else if (PW_total < PW_min){ + if (sensors->V_soc > parameters->V_max){ + torque_out[2] = 0; + torque_out[3] = 0; + } else { + + float PW_extra = (PW_total - PW_min)/PW_total; + for (int i = 2; i < 4; i++) { +// if (sensors->motor_speed[i] <= 0) { +// torque_out[i] = 0; +// } else { + torque_out[i] = torque_out[i] * (1 - PW_extra); +// } + } } + } + } \ No newline at end of file From 1fe52cf3450540bdddf75b5104b146b5b30c0bdd Mon Sep 17 00:00:00 2001 From: jmlp05 Date: Thu, 13 Nov 2025 00:18:57 +0100 Subject: [PATCH 16/25] fix: change sensors naming and motor speed conversions --- src/arussim/config/sensors_config.yaml | 32 +-- src/arussim/config/simulator_config.yaml | 2 - .../include/arussim/control_sim/Parameters.h | 6 +- .../include/arussim/main_interface.hpp | 2 +- src/arussim/include/arussim/sensors.hpp | 23 +- .../include/arussim/vehicle_dynamics.hpp | 4 +- src/arussim/resources/tracks/FSG25.pcd | 205 ++++++++++++++++++ src/arussim/src/arussim_node.cpp | 2 - src/arussim/src/control_sim/control_sim.cpp | 16 +- src/arussim/src/control_sim/power_control.c | 21 +- src/arussim/src/sensors.cpp | 125 +++++------ 11 files changed, 311 insertions(+), 127 deletions(-) create mode 100644 src/arussim/resources/tracks/FSG25.pcd diff --git a/src/arussim/config/sensors_config.yaml b/src/arussim/config/sensors_config.yaml index 2bd39ac3..625bad4e 100755 --- a/src/arussim/config/sensors_config.yaml +++ b/src/arussim/config/sensors_config.yaml @@ -1,24 +1,24 @@ arussim: ros__parameters: - imu: - imu_frequency: 100.0 # Hz Publish rate + gss: + gss_frequency: 100.0 # Hz Publish rate + noise_gss_vx: 0.01 # kmh Noise standard deviation + noise_gss_vy: 0.01 # kmh Noise standard deviation noise_imu_ax: 0.005 # m/s² Noise standard deviation noise_imu_ay: 0.005 # m/s² Noise standard deviation - noise_imu_r: 0.005 # rad/s Noise standard deviation + noise_imu_r: 0.025 # º/s Noise standard deviation - wheel_speed: - wheel_speed_frequency: 100.0 # Hz Publish rate - noise_wheel_speed_front_right: 0.005 # m/s Noise standard deviation (front right wheel) - noise_wheel_speed_front_left: 0.005 # m/s Noise standard deviation (front left wheel) - noise_wheel_speed_rear_right: 0.005 # m/s Noise standard deviation (rear right wheel) - noise_wheel_speed_rear_left: 0.005 # m/s Noise standard deviation (rear left wheel) - - torque: - torque_frequency: 100.0 # Hz Publish rate - noise_torque_front_right: 0.01 # m/s Noise standard deviation (front right wheel) - noise_torque_front_left: 0.01 # m/s Noise standard deviation (front left wheel) - noise_torque_rear_right: 0.01 # m/s Noise standard deviation (rear right wheel) - noise_torque_rear_left: 0.01 # m/s Noise standard deviation (rear left wheel) + inverter: + inverter_frequency: 100.0 # Hz Publish rate + noise_motor_speed_front_right: 0.005 # rpm Noise standard deviation (front right wheel) + noise_motor_speed_front_left: 0.005 # rpm Noise standard deviation (front left wheel) + noise_motor_speed_rear_right: 0.005 # rpm Noise standard deviation (rear right wheel) + noise_motor_speed_rear_left: 0.005 # rpm Noise standard deviation (rear left wheel) + noise_torque_front_right: 0.01 # Nm Noise standard deviation (front right wheel) + noise_torque_front_left: 0.01 # Nm Noise standard deviation (front left wheel) + noise_torque_rear_right: 0.01 # Nm Noise standard deviation (rear right wheel) + noise_torque_rear_left: 0.01 # Nm Noise standard deviation (rear left wheel) + gear_ratio: 12.48 # Transmission gear ratio extensometer: diff --git a/src/arussim/config/simulator_config.yaml b/src/arussim/config/simulator_config.yaml index e35d616f..0e95be49 100755 --- a/src/arussim/config/simulator_config.yaml +++ b/src/arussim/config/simulator_config.yaml @@ -2,8 +2,6 @@ arussim: ros__parameters: track: "FSG" state_update_rate: 1000.0 # Hz - controller_rate: 400.0 # Hz - use_gss: true csv_state: false # bool Save data to CSV file csv_vehicle_dynamics: false # bool Save data to CSV file diff --git a/src/arussim/include/arussim/control_sim/Parameters.h b/src/arussim/include/arussim/control_sim/Parameters.h index 9602c2e8..d691935f 100644 --- a/src/arussim/include/arussim/control_sim/Parameters.h +++ b/src/arussim/include/arussim/control_sim/Parameters.h @@ -21,10 +21,10 @@ extern "C" { #define pi 3.141592 -#define eps 0.0000001 +#define eps 0.00001 #define GRAVITY 9.81 -#define DEFAULT_MAX_POWER 20000 -#define DEFAULT_MIN_POWER -20000 +#define DEFAULT_MAX_POWER 50000 +#define DEFAULT_MIN_POWER -30000 #define DEFAULT_MAX_VOLTAGE 595 #define DEFAULT_MIN_VOLTAGE 430 diff --git a/src/arussim/include/arussim/main_interface.hpp b/src/arussim/include/arussim/main_interface.hpp index 1c1f1cf2..27ffc24a 100644 --- a/src/arussim/include/arussim/main_interface.hpp +++ b/src/arussim/include/arussim/main_interface.hpp @@ -111,7 +111,7 @@ private Q_SLOTS: int lap_counter_ = 0; double bar_size_; - double max_torque_value_ = 90.0; + double max_torque_value_ = 21.0; double scale_factor_; double center_y_; diff --git a/src/arussim/include/arussim/sensors.hpp b/src/arussim/include/arussim/sensors.hpp index 6351fedf..9e1cba16 100644 --- a/src/arussim/include/arussim/sensors.hpp +++ b/src/arussim/include/arussim/sensors.hpp @@ -60,8 +60,8 @@ class Sensors : public rclcpp::Node double ax_ = 0; double ay_ = 0; double delta_ = 0; - arussim_msgs::msg::FourWheelDrive wheel_speed; - arussim_msgs::msg::FourWheelDrive torque_cmd; + arussim_msgs::msg::FourWheelDrive wheel_speed_msg; + arussim_msgs::msg::FourWheelDrive torque_cmd_msg; struct { double fl_ = 0; @@ -80,21 +80,20 @@ class Sensors : public rclcpp::Node double kExtensometerFrequency; double kNoiseExtensometer; - double kWheelSpeedFrequency; - double kNoiseWheelSpeedFrontRight; - double kNoiseWheelSpeedFrontLeft; - double kNoiseWheelSpeedRearRight; - double kNoiseWheelSpeedRearLeft; - - double kTorqueFrequency; + double kInverterFrequency; + double kNoiseMotorSpeedFrontRight; + double kNoiseMotorSpeedFrontLeft; + double kNoiseMotorSpeedRearRight; + double kNoiseMotorSpeedRearLeft; double kNoiseTorqueFrontRight; double kNoiseTorqueFrontLeft; double kNoiseTorqueRearRight; double kNoiseTorqueRearLeft; + double kGearRatio; - double kImuFrequency; - double kNoiseImuX; - double kNoiseImuY; + double kGssFrequency; + double kNoiseGssVx; + double kNoiseGssVy; double kNoiseImuAx; double kNoiseImuAy; double kNoiseImuR; diff --git a/src/arussim/include/arussim/vehicle_dynamics.hpp b/src/arussim/include/arussim/vehicle_dynamics.hpp index 4e7e0de6..f8864cd1 100644 --- a/src/arussim/include/arussim/vehicle_dynamics.hpp +++ b/src/arussim/include/arussim/vehicle_dynamics.hpp @@ -60,13 +60,13 @@ class VehicleDynamics private: - double kMass = 260.0; + double kMass = 275.0; double kNsMassF = 25; double kNsMassR = 25; double kSMass = kMass - kNsMassF - kNsMassR; double kIzz = 180; - double kMassDistributionRear = 0.54; + double kMassDistributionRear = 0.5; double kSMassF = kSMass * (1-kMassDistributionRear); double kSMassR = kSMass * kMassDistributionRear; diff --git a/src/arussim/resources/tracks/FSG25.pcd b/src/arussim/resources/tracks/FSG25.pcd new file mode 100644 index 00000000..ebcf976b --- /dev/null +++ b/src/arussim/resources/tracks/FSG25.pcd @@ -0,0 +1,205 @@ +# .PCD v0.7 - Point Cloud Data file format +VERSION 0.7 +FIELDS x y z color score +SIZE 4 4 4 4 4 +TYPE F F F I F +COUNT 1 1 1 1 1 +WIDTH 194 +HEIGHT 1 +VIEWPOINT 0 0 0 1 0 0 0 +POINTS 194 +DATA ascii +2.1589599 -1.5159398 0 1 1 +5.2523899 -1.6644515 0 1 1 +12.970853 -1.981712 0 1 1 +5.4082675 1.8744341 0 0 1 +1.8868686 2.0118668 0 0 1 +13.096785 1.5355339 0 0 1 +17.309311 1.5041407 0 0 1 +8.3595095 1.7093412 0 0 1 +8.3595095 -1.8093412 0 1 1 +17.554169 -1.9818925 0 1 1 +20.395315 -14.153285 0 1 1 +24.425568 -12.178007 0 1 1 +28.487173 7.7680869 0 0 1 +27.951359 -0.072341733 0 1 1 +21.502439 -2.1525009 0 1 1 +24.957273 -1.6601055 0 1 1 +21.330135 1.3389348 0 0 1 +29.305323 1.2799541 0 1 1 +31.312468 10.926512 0 1 1 +28.423872 -10.204979 0 1 1 +31.941868 7.2814984 0 1 1 +23.756584 1.6727858 0 0 1 +26.128874 -15.167103 0 0 1 +29.593966 13.771836 0 1 1 +25.632574 2.5656779 0 0 1 +31.247147 4.9921699 0 1 1 +30.349005 3.1373088 0 1 1 +32.376591 -8.2037649 0 1 1 +30.217863 -13.146136 0 0 1 +27.238642 4.7598014 0 0 1 +34.371964 -11.064 0 0 1 +36.327015 -6.0918632 0 1 1 +38.264263 -8.9655857 0 0 1 +39.171391 -4.5031796 0 1 1 +41.564438 -6.9993463 0 0 1 +27.932634 10.02771 0 0 1 +27.215181 18.497355 0 1 1 +26.846125 11.499019 0 0 1 +48.717361 5.7634478 0 1 1 +23.685265 18.370972 0 0 1 +24.020483 21.613604 0 0 1 +28.471539 23.758625 0 1 1 +24.677256 14.690401 0 0 1 +27.819052 16.623669 0 1 1 +25.427727 27.322226 0 0 1 +28.911512 26.162848 0 1 1 +27.371773 20.485113 0 1 1 +25.092525 24.664038 0 0 1 +28.773726 28.347523 0 1 1 +28.015774 30.350317 0 1 1 +24.154259 29.858299 0 0 1 +22.047022 31.609123 0 0 1 +26.704306 32.212269 0 1 1 +19.697674 32.477417 0 0 1 +23.987146 34.51339 0 1 1 +16.767994 32.885231 0 0 1 +10.122384 29.56823 0 0 1 +14.024212 32.832642 0 0 1 +20.779217 35.813641 0 1 1 +10.08298 34.576283 0 1 1 +17.228348 36.399658 0 1 1 +13.390154 36.271404 0 1 1 +36.466827 35.727467 0 1 1 +38.927589 33.586906 0 1 1 +35.093513 38.061722 0 1 1 +38.245949 39.489956 0 0 1 +33.942535 40.366646 0 1 1 +40.415146 36.751541 0 0 1 +39.261189 37.782532 0 0 1 +36.667057 42.563786 0 0 1 +28.057068 44.272289 0 1 1 +31.885965 42.070721 0 1 1 +24.639784 46.038311 0 1 1 +33.606773 45.111908 0 0 1 +29.748629 47.330418 0 0 1 +7.8401265 32.212467 0 1 1 +26.224525 49.189087 0 0 1 +20.070274 48.191273 0 1 1 +5.2104545 29.676928 0 1 1 +7.3637481 26.905174 0 0 1 +16.353851 49.840904 0 1 1 +1.8839178 28.025196 0 1 1 +3.6649084 24.992493 0 0 1 +-0.29601502 47.521641 0 1 1 +0.38352704 23.400204 0 0 1 +-1.1447715 26.585136 0 1 1 +-3.7412024 45.442879 0 1 1 +-4.1468172 25.544716 0 1 1 +-3.3683834 22.123203 0 0 1 +-7.4353981 24.708443 0 1 1 +-10.450767 24.750372 0 1 1 +-7.3506927 21.198839 0 0 1 +-14.020123 26.007612 0 1 1 +-11.188739 21.346958 0 0 1 +-12.493056 25.115034 0 1 1 +-13.795375 21.823853 0 0 1 +-15.967439 30.320305 0 1 1 +-15.8444 28.032511 0 1 1 +-19.08392 26.792597 0 0 1 +-19.628405 28.890985 0 0 1 +-17.562588 24.400806 0 0 1 +-15.836972 23.001776 0 0 1 +-19.326326 31.299692 0 0 1 +-15.312813 32.374008 0 1 1 +-18.498682 33.816475 0 0 1 +-14.039708 34.788517 0 1 1 +-16.841091 36.86554 0 0 1 +-14.831886 39.803337 0 0 1 +-12.180857 42.745884 0 0 1 +-9.8278284 40.188469 0 1 1 +-12.372585 37.346138 0 1 1 +-7.3633113 42.704609 0 1 1 +-9.4391127 45.578014 0 0 1 +-1.3683158 50.865749 0 0 1 +-5.6232824 48.390308 0 0 1 +3.3372099 52.807728 0 0 1 +3.8183255 49.37133 0 1 1 +8.238225 53.712944 0 0 1 +12.635665 50.784924 0 1 1 +8.674428 50.136311 0 1 1 +13.358783 54.261093 0 0 1 +16.163525 53.586479 0 0 1 +18.187803 52.811752 0 0 1 +21.644053 51.296932 0 0 1 +45.006176 34.677559 0 0 1 +43.196323 31.586779 0 1 1 +49.439999 32.841549 0 0 1 +47.564274 29.853052 0 1 1 +49.530258 27.954706 0 1 1 +51.864311 30.595959 0 0 1 +51.38203 25.864531 0 1 1 +52.882019 23.388868 0 1 1 +54.494125 27.520527 0 0 1 +51.854076 15.227683 0 1 1 +53.795998 21.706135 0 1 1 +55.781399 25.431953 0 0 1 +53.58078 19.0145 0 1 1 +53.955544 20.658148 0 1 1 +57.07243 22.927053 0 0 1 +52.761898 17.096081 0 1 1 +55.133038 14.059982 0 0 1 +57.005287 18.398561 0 0 1 +57.47665 20.847204 0 0 1 +56.086044 16.043846 0 0 1 +54.466793 11.804601 0 0 1 +51.119404 12.846237 0 1 1 +53.460648 7.9486408 0 0 1 +50.252132 9.350152 0 1 1 +51.786808 4.0510702 0 0 1 +21.56674375 -17.75358011 0 0 1 +17.42035344 -19.63441486 0 0 1 +13.33346066 -21.48441486 0 0 1 +9.11579134 -23.39455418 0 0 1 +5.21400057 -25.15920470 0 0 1 +1.06333968 -26.99059105 0 0 1 +-3.22448775 -28.52010597 0 0 1 +-8.30616846 -29.79749007 0 0 1 +-12.2204170 -30.18467020 0 0 1 +-14.10011231 -29.8347023 0 0 1 +-15.84294671 -29.18245293 0 0 1 +-19.13933051 -26.44344854 0 0 1 +-19.81793831 -24.28833111 0 0 1 +-20.15352789 -18.99322764 0 0 1 +-19.78116957 -15.12005580 0 0 1 +-18.49984642 -10.78432289 0 0 1 +-16.61782151 -7.18249996 0 0 1 +-13.75052977 -3.72303676 0 0 1 +-10.14579013 -0.62569497 0 0 1 +-6.18001953 0.97992781 0 0 1 +-2.06560985 1.84959784 0 0 1 +-1.62951073 -1.75724879 0 1 1 +-4.95593399 -2.31892952 0 1 1 +-7.99036727 -3.35436742 0 1 1 +-10.95782868 -5.80577310 0 1 1 +-13.58305137 -9.02579982 0 1 1 +-15.16382270 -12.13851981 0 1 1 +-16.31691086 -15.59291443 0 1 1 +-16.67127606 -19.30970273 0 1 1 +-16.55322099 -22.36835433 0 1 1 +-15.97761936 -24.91589173 0 1 1 +-14.25726381 -25.99818969 0 1 1 +-4.50722671 -25.42921852 0 1 1 +-0.42700702 -23.71054419 0 1 1 +3.34894202 -22.08970264 0 1 1 +7.46993663 -20.31044269 0 1 1 +11.70686421 -18.38014700 0 1 1 +15.72611686 -16.58783022 0 1 1 +-10.0 -26.0 0 1 1 +45.9 -3.04 0 0 1 +43.2 -0.769 0 1 1 +45.4 1.51 0 1 1 +48.6 -0.265 0 0 1 +6.0 2.5 0 4 1 +6.0 -2.5 0 4 1 \ No newline at end of file diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index becc248a..a5453237 100755 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -337,7 +337,6 @@ void Simulator::on_fast_timer() } double dt = 1.0 / kStateUpdateRate; - vehicle_dynamics_.update_simulation(can_delta_, can_torque_cmd_, dt); if(use_tpl_){ @@ -438,7 +437,6 @@ void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::Share can_acc_ = 0.0; can_delta_ = 0.0; can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; - vehicle_dynamics_ = VehicleDynamics(); started_acc_ = false; diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index 7d075938..0cf34eec 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -96,20 +96,20 @@ ControlSim::ControlSim() : Node("control_sim") { } else if (frame.can_id == 0x102){ - int32_t ws_scaled_0 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); - sensors.motor_speed[0] = ws_scaled_0 * 0.00000018879763543; + int32_t ws_scaled_0 = static_cast((frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[0] = ws_scaled_0 * 0.0001047197551196; } else if (frame.can_id == 0x106){ - int32_t ws_scaled_1 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); - sensors.motor_speed[1] = ws_scaled_1 * 0.00000018879763543; + int32_t ws_scaled_1 = static_cast((frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[1] = ws_scaled_1 * 0.0001047197551196; } else if (frame.can_id == 0x110){ - int32_t ws_scaled_2 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); - sensors.motor_speed[2] = ws_scaled_2 * 0.00000018879763543; + int32_t ws_scaled_2 = static_cast((frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[2] = ws_scaled_2 * 0.0001047197551196; } else if (frame.can_id == 0x114){ - int32_t ws_scaled_3 = static_cast((frame.data[3] << 24) | (frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); - sensors.motor_speed[3] = ws_scaled_3 * 0.00000018879763543; + int32_t ws_scaled_3 = static_cast((frame.data[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[3] = ws_scaled_3 * 0.0001047197551196; } } diff --git a/src/arussim/src/control_sim/power_control.c b/src/arussim/src/control_sim/power_control.c index a7d08d62..b87d0497 100644 --- a/src/arussim/src/control_sim/power_control.c +++ b/src/arussim/src/control_sim/power_control.c @@ -18,42 +18,23 @@ void PowerControl_Update(SensorData *sensors, Parameters *parameters, float * to float PW_total = torque_out[0] * sensors->motor_speed[0] + torque_out[1]*sensors->motor_speed[1] + torque_out[2]*sensors->motor_speed[2] + torque_out[3]* sensors->motor_speed[3]; - // float r_battery = 1.2f; - // float PW_max_bms = sensors->V_soc*(sensors->V_soc - parameters->V_min) / r_battery; - // float PW_min_bms = sensors->V_soc*(sensors->V_soc - parameters->V_max) / r_battery; - - // float PW_max = (PW_max_bms < parameters->maximum_power) ? PW_max_bms : parameters->maximum_power; - // float PW_min = (PW_min_bms > parameters->minimum_power) ? PW_min_bms : parameters->minimum_power; float PW_max = parameters->maximum_power; float PW_min = parameters->minimum_power; - + if(PW_min > 0) PW_min = 0; if (PW_total > PW_max) { float PW_extra = (PW_total - PW_max)/PW_total; for (int i = 2; i < 4; i++) { -// if (sensors->motor_speed[i] <= 0) { -// torque_out[i] = 0; -// } else { torque_out[i] = torque_out[i] * (1 - PW_extra); -// } } } else if (PW_total < PW_min){ - if (sensors->V_soc > parameters->V_max){ - torque_out[2] = 0; - torque_out[3] = 0; - } else { float PW_extra = (PW_total - PW_min)/PW_total; for (int i = 2; i < 4; i++) { -// if (sensors->motor_speed[i] <= 0) { -// torque_out[i] = 0; -// } else { torque_out[i] = torque_out[i] * (1 - PW_extra); -// } - } } } diff --git a/src/arussim/src/sensors.cpp b/src/arussim/src/sensors.cpp index 74191ea5..fbb139f0 100644 --- a/src/arussim/src/sensors.cpp +++ b/src/arussim/src/sensors.cpp @@ -15,48 +15,49 @@ std::vector Sensors::frames; */ Sensors::Sensors() : Node("sensors") { - // Declare and get noise parameters for each IMU variable - this->declare_parameter("imu.noise_imu_ax", 0.01); - this->declare_parameter("imu.noise_imu_ay", 0.01); - this->declare_parameter("imu.noise_imu_r", 0.01); - this->declare_parameter("imu.imu_frequency", 100.0); - - // Declare wheel speed parameters - this->declare_parameter("wheel_speed.noise_wheel_speed_front_right", 0.01); - this->declare_parameter("wheel_speed.noise_wheel_speed_front_left", 0.01); - this->declare_parameter("wheel_speed.noise_wheel_speed_rear_right", 0.01); - this->declare_parameter("wheel_speed.noise_wheel_speed_rear_left", 0.01); - this->declare_parameter("wheel_speed.wheel_speed_frequency", 100.0); - - // Declare 4WD torque parameters - this->declare_parameter("torque.noise_torque_front_right", 0.01); - this->declare_parameter("torque.noise_torque_front_left", 0.01); - this->declare_parameter("torque.noise_torque_rear_right", 0.01); - this->declare_parameter("torque.noise_torque_rear_left", 0.01); - this->declare_parameter("torque.torque_frequency", 100.0); - + // Declare and get noise parameters for each GSS variable + this->declare_parameter("gss.noise_gss_vx", 0.01); + this->declare_parameter("gss.noise_gss_vy", 0.01); + this->declare_parameter("gss.noise_imu_ax", 0.01); + this->declare_parameter("gss.noise_imu_ay", 0.01); + this->declare_parameter("gss.noise_imu_r", 0.025); + this->declare_parameter("gss.gss_frequency", 100.0); + + // Declare inverter parameters + this->declare_parameter("inverter.noise_motor_speed_front_right", 0.01); + this->declare_parameter("inverter.noise_motor_speed_front_left", 0.01); + this->declare_parameter("inverter.noise_motor_speed_rear_right", 0.01); + this->declare_parameter("inverter.noise_motor_speed_rear_left", 0.01); + this->declare_parameter("inverter.noise_torque_front_right", 0.01); + this->declare_parameter("inverter.noise_torque_front_left", 0.01); + this->declare_parameter("inverter.noise_torque_rear_right", 0.01); + this->declare_parameter("inverter.noise_torque_rear_left", 0.01); + this->declare_parameter("inverter.gear_ratio", 12.48); + this->declare_parameter("inverter.inverter_frequency", 100.0); + // Declare extensometer parameters this->declare_parameter("extensometer.extensometer_frequency", 100.0); this->declare_parameter("extensometer.noise_extensometer", 0.01); // Get parameters - this->get_parameter("imu.noise_imu_ax", kNoiseImuAx); - this->get_parameter("imu.noise_imu_ay", kNoiseImuAy); - this->get_parameter("imu.noise_imu_r", kNoiseImuR); - this->get_parameter("imu.imu_frequency", kImuFrequency); - - this->get_parameter("wheel_speed.noise_wheel_speed_front_right", kNoiseWheelSpeedFrontRight); - this->get_parameter("wheel_speed.noise_wheel_speed_front_left", kNoiseWheelSpeedFrontLeft); - this->get_parameter("wheel_speed.noise_wheel_speed_rear_right", kNoiseWheelSpeedRearRight); - this->get_parameter("wheel_speed.noise_wheel_speed_rear_left", kNoiseWheelSpeedRearLeft); - this->get_parameter("wheel_speed.wheel_speed_frequency", kWheelSpeedFrequency); - - this->get_parameter("torque.noise_torque_front_right", kNoiseTorqueFrontRight); - this->get_parameter("torque.noise_torque_front_left", kNoiseTorqueFrontLeft); - this->get_parameter("torque.noise_torque_rear_right", kNoiseTorqueRearRight); - this->get_parameter("torque.noise_torque_rear_left", kNoiseTorqueRearLeft); - this->get_parameter("torque.torque_frequency", kTorqueFrequency); + this->get_parameter("gss.noise_gss_vx", kNoiseGssVx); + this->get_parameter("gss.noise_imu_ax", kNoiseGssVy); + this->get_parameter("gss.noise_imu_ax", kNoiseImuAx); + this->get_parameter("gss.noise_imu_ay", kNoiseImuAy); + this->get_parameter("gss.noise_imu_r", kNoiseImuR); + this->get_parameter("gss.gss_frequency", kGssFrequency); + + this->get_parameter("inverter.noise_motor_speed_front_right", kNoiseMotorSpeedFrontRight); + this->get_parameter("inverter.noise_motor_speed_front_left", kNoiseMotorSpeedFrontLeft); + this->get_parameter("inverter.noise_motor_speed_rear_right", kNoiseMotorSpeedRearRight); + this->get_parameter("inverter.noise_motor_speed_rear_left", kNoiseMotorSpeedRearLeft); + this->get_parameter("inverter.noise_torque_front_right", kNoiseTorqueFrontRight); + this->get_parameter("inverter.noise_torque_front_left", kNoiseTorqueFrontLeft); + this->get_parameter("inverter.noise_torque_rear_right", kNoiseTorqueRearRight); + this->get_parameter("inverter.noise_torque_rear_left", kNoiseTorqueRearLeft); + this->get_parameter("inverter.gear_ratio", kGearRatio); + this->get_parameter("inverter.inverter_frequency", kInverterFrequency); this->get_parameter("extensometer.extensometer_frequency", kExtensometerFrequency); this->get_parameter("extensometer.noise_extensometer", kNoiseExtensometer); @@ -77,7 +78,7 @@ Sensors::Sensors() : Node("sensors") // IMU imu_timer_ = this->create_wall_timer( - std::chrono::milliseconds((int)(1000/kImuFrequency)), + std::chrono::milliseconds((int)(1000/kGssFrequency)), std::bind(&Sensors::imu_timer, this) ); @@ -91,7 +92,7 @@ Sensors::Sensors() : Node("sensors") torque_pub_ = this->create_publisher("/arussim/torque4WD", 10); inverter_timer_ = this->create_wall_timer( - std::chrono::milliseconds((int)(1000/kTorqueFrequency)), + std::chrono::milliseconds((int)(1000/kInverterFrequency)), std::bind(&Sensors::inverter_timer, this) ); @@ -112,19 +113,19 @@ Sensors::Sensors() : Node("sensors") {"extensometer", {0, 15, true, -0.000031688042484, 0.476959989071}} }}, {0x102, 5, { // Front Left inverter - {"fl_inv_speed", {0, 23, true, 0.00000018879763543, 0.0}}, + {"fl_inv_speed", {0, 23, true, 0.0001047197551196, 0.0}}, {"fl_inv_torque", {24, 39, true, 0.0098, 0.0}} }}, {0x106, 5, { // Front Right inverter - {"fr_inv_speed", {0, 23, true, 0.00000018879763543, 0.0}}, + {"fr_inv_speed", {0, 23, true, 0.0001047197551196, 0.0}}, {"fr_inv_torque", {24, 39, true, 0.0098, 0.0}} }}, {0x110, 5, { // Rear Left inverter - {"rl_inv_speed", {0, 23, true, 0.00000018879763543, 0.0}}, + {"rl_inv_speed", {0, 23, true, 0.0001047197551196, 0.0}}, {"rl_inv_torque", {24, 39, true, 0.0098, 0.0}} }}, {0x114, 5, { // Rear Right inverter - {"rr_inv_speed", {0, 23, true, 0.00000018879763543, 0.0}}, + {"rr_inv_speed", {0, 23, true, 0.0001047197551196, 0.0}}, {"rr_inv_torque", {24, 39, true, 0.0098, 0.0}} }}, {0x161, 2, { @@ -150,8 +151,8 @@ void Sensors::state_callback(const arussim_msgs::msg::State::SharedPtr msg) ax_ = msg->ax; ay_ = msg->ay; delta_ = msg->delta; - wheel_speed = msg->wheel_speeds; - torque_cmd = msg->torque; + wheel_speed_msg = msg->wheel_speeds; + torque_cmd_msg = msg->torque; } //Function to encode a signal into a CAN frame @@ -180,10 +181,10 @@ void Sensors::inverter_timer(){ std::normal_distribution<> dist_rl(0.0, kNoiseTorqueRearLeft); // Apply noise to the state variables - torque_cmd_.fr_ = torque_cmd.front_right + dist_fr(gen); - torque_cmd_.fl_ = torque_cmd.front_left + dist_fl(gen); - torque_cmd_.rr_ = torque_cmd.rear_right + dist_rr(gen); - torque_cmd_.rl_ = torque_cmd.rear_left + dist_rl(gen); + torque_cmd_.fr_ = torque_cmd_msg.front_right; // 2WD + torque_cmd_.fl_ = torque_cmd_msg.front_left; // 2WD + torque_cmd_.rr_ = torque_cmd_msg.rear_right + dist_rr(gen); + torque_cmd_.rl_ = torque_cmd_msg.rear_left + dist_rl(gen); // Create the torque message auto message = arussim_msgs::msg::FourWheelDrive(); @@ -197,22 +198,22 @@ void Sensors::inverter_timer(){ torque_pub_->publish(message); // Random noise generation with different noise for each wheel speed - std::normal_distribution<> dist_front_right(0.0, kNoiseWheelSpeedFrontRight); - std::normal_distribution<> dist_front_left(0.0, kNoiseWheelSpeedFrontLeft); - std::normal_distribution<> dist_rear_right(0.0, kNoiseWheelSpeedRearRight); - std::normal_distribution<> dist_rear_left(0.0, kNoiseWheelSpeedRearLeft); + std::normal_distribution<> dist_front_right(0.0, kNoiseMotorSpeedFrontLeft); + std::normal_distribution<> dist_front_left(0.0, kNoiseMotorSpeedFrontRight); + std::normal_distribution<> dist_rear_right(0.0, kNoiseMotorSpeedRearLeft); + std::normal_distribution<> dist_rear_left(0.0, kNoiseMotorSpeedRearRight); // Apply noise to the state variables - wheel_speed_.fr_ = wheel_speed.front_right * 0.225 + dist_front_right(gen); - wheel_speed_.fl_ = wheel_speed.front_left * 0.225 + dist_front_left(gen); - wheel_speed_.rr_ = wheel_speed.rear_right * 0.225 + dist_rear_right(gen); - wheel_speed_.rl_ = wheel_speed.rear_left * 0.225 + dist_rear_left(gen); + wheel_speed_.fr_ = wheel_speed_msg.front_right; // 2WD + wheel_speed_.fl_ = wheel_speed_msg.front_left; // 2WD + wheel_speed_.rr_ = wheel_speed_msg.rear_right + dist_rear_right(gen); + wheel_speed_.rl_ = wheel_speed_msg.rear_left + dist_rear_left(gen); //Send Inverter CAN frames - std::map values = { {"fl_inv_speed", wheel_speed_.fl_}, {"fl_inv_torque", torque_cmd_.fl_ }, - {"fr_inv_speed", wheel_speed_.fr_}, {"fr_inv_torque", torque_cmd_.fr_}, - {"rl_inv_speed", wheel_speed_.rl_}, {"rl_inv_torque", torque_cmd_.rl_}, - {"rr_inv_speed", wheel_speed_.rr_}, {"rr_inv_torque", torque_cmd_.rr_} + std::map values = { {"fl_inv_speed", wheel_speed_.fl_*kGearRatio}, {"fl_inv_torque", torque_cmd_.fl_/kGearRatio}, + {"fr_inv_speed", wheel_speed_.fr_*kGearRatio}, {"fr_inv_torque", torque_cmd_.fr_/kGearRatio}, + {"rl_inv_speed", wheel_speed_.rl_*kGearRatio}, {"rl_inv_torque", torque_cmd_.rl_/kGearRatio}, + {"rr_inv_speed", wheel_speed_.rr_*kGearRatio}, {"rr_inv_torque", torque_cmd_.rr_/kGearRatio} }; for (auto &frame : frames) { if (frame.id == 0x102 || frame.id == 0x106 || frame.id == 0x110 || frame.id == 0x114) { @@ -231,6 +232,8 @@ void Sensors::imu_timer() std::random_device rd; std::mt19937 gen(rd()); + std::normal_distribution<> dist_vx(0.0, kNoiseGssVx); + std::normal_distribution<> dist_vy(0.0, kNoiseGssVy); std::normal_distribution<> dist_ax(0.0, kNoiseImuAx); std::normal_distribution<> dist_ay(0.0, kNoiseImuAy); std::normal_distribution<> dist_r(0.0, kNoiseImuR); @@ -239,7 +242,7 @@ void Sensors::imu_timer() //Send CAN frames for IMU (GSS ids) std::map values = { {"IMU/ax", ax_ + dist_ax(gen)}, {"IMU/ay", ay_ + dist_ay(gen)}, {"IMU/yaw_rate", r_ + dist_r(gen)}, - {"GSS/vx", vx_}, {"GSS/vy", vy_} }; + {"GSS/vx", vx_ + dist_vx(gen)}, {"GSS/vy", vy_ + dist_vy(gen)} }; for (auto &frame : frames) { if (frame.id == 0x1A3 || frame.id == 0x1A4 || frame.id == 0x1A0) { From a6d71c6c842aa67d60117acb1fa42af9e31fd45b Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Thu, 13 Nov 2025 00:31:07 +0100 Subject: [PATCH 17/25] fix: up can when launching from terminal --- src/arussim/launch/arussim_launch.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/arussim/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index 279bee4f..8dceeea9 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -21,10 +21,18 @@ def generate_launch_description(): cmd=['bash', '-c', "ip link show can0 >/dev/null 2>&1 || sudo ip link add dev can0 type vcan"], shell=False ) + up_can0 = ExecuteProcess( + cmd=['sudo', 'ip', 'link', 'set', 'up', 'can0'], + shell=False + ) create_can1 = ExecuteProcess( cmd=['bash', '-c', "ip link show can1 >/dev/null 2>&1 || sudo ip link add dev can1 type vcan"], shell=False ) + up_can1 = ExecuteProcess( + cmd=['sudo', 'ip', 'link', 'set', 'up', 'can1'], + shell=False + ) # Chain processes to run one after another sequence = [ @@ -37,9 +45,21 @@ def generate_launch_description(): RegisterEventHandler( OnProcessExit( target_action=create_can0, + on_exit=[up_can0] + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=up_can0, on_exit=[create_can1] ) ), + RegisterEventHandler( + OnProcessExit( + target_action=create_can1, + on_exit=[up_can1] + ) + ) ] return LaunchDescription([ From 3891e6ffa3999099f5b9ea7fbb78b0f4695649ca Mon Sep 17 00:00:00 2001 From: jmlp05 Date: Fri, 14 Nov 2025 00:43:48 +0100 Subject: [PATCH 18/25] fix: reset control_sim on reset click --- .../arussim/control_sim/control_sim.hpp | 83 +++++++++---------- src/arussim/src/control_sim/control_sim.cpp | 14 ++++ 2 files changed, 51 insertions(+), 46 deletions(-) diff --git a/src/arussim/include/arussim/control_sim/control_sim.hpp b/src/arussim/include/arussim/control_sim/control_sim.hpp index 20898996..c30cc5a1 100644 --- a/src/arussim/include/arussim/control_sim/control_sim.hpp +++ b/src/arussim/include/arussim/control_sim/control_sim.hpp @@ -1,19 +1,13 @@ #include "rclcpp/rclcpp.hpp" #include #include -#include -#include #include #include #include #include #include #include -#include "std_msgs/msg/header.hpp" -#include "arussim_msgs/msg/cmd.hpp" -#include "arussim_msgs/msg/cmd4_wd.hpp" - -#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/bool.hpp" #include "arussim/control_sim/SensorData.h" #include "arussim/control_sim/Parameters.h" @@ -30,47 +24,44 @@ class ControlSim : public rclcpp::Node ControlSim(); - private: - SensorData sensors; - Parameters parameters; - DV dv; - TIRE tire; - PID pid; - - void Parameters_Init(Parameters *); - void receive_can(); - void send_torque(float torque_out[4]); - void send_state(); - void default_task(); - int can_socket_; - struct ifreq ifr_; - struct sockaddr_can addr_; - struct can_frame frame; - rclcpp::TimerBase::SharedPtr timer_receive_; - rclcpp::TimerBase::SharedPtr timer_defaultTask_; - rclcpp::Clock::SharedPtr clock_; - std::thread thread_; - - + private: + SensorData sensors; + Parameters parameters; + DV dv; + TIRE tire; + PID pid; - double kMass; - float kRdyn; - float kGearRatio; + void Parameters_Init(Parameters *); + void receive_can(); + void send_torque(float torque_out[4]); + void send_state(); + void default_task(); + int can_socket_; + struct ifreq ifr_; + struct sockaddr_can addr_; + struct can_frame frame; + rclcpp::TimerBase::SharedPtr timer_receive_; + rclcpp::TimerBase::SharedPtr timer_defaultTask_; + rclcpp::Clock::SharedPtr clock_; + std::thread thread_; + + rclcpp::Subscription::SharedPtr reset_sub_; + void reset_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg); - int16_t torque_i_; - int16_t vx_i_; - int16_t vy_i_; - int16_t r_i_; - int16_t acc_scaled_; - int16_t yaw_scaled_; - int16_t vx_scaled_; - int16_t vy_scaled_; - int16_t r_scaled_; - float state[3]; - float SR[4]; - float TC_out[4]; - float TV_out[4]; - float fx_request; + int16_t torque_i_; + int16_t vx_i_; + int16_t vy_i_; + int16_t r_i_; + int16_t acc_scaled_; + int16_t yaw_scaled_; + int16_t vx_scaled_; + int16_t vy_scaled_; + int16_t r_scaled_; + float state[3]; + float SR[4]; + float TC_out[4]; + float TV_out[4]; + float fx_request; }; \ No newline at end of file diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index 0cf34eec..7923af27 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -36,6 +36,10 @@ ControlSim::ControlSim() : Node("control_sim") { std::thread thread_(&ControlSim::receive_can, this); thread_.detach(); + // Reset subscriber + reset_sub_ = this->create_subscription("/arussim/reset", 1, + std::bind(&ControlSim::reset_callback, this, std::placeholders::_1)); + //Initialize state std::vector state = {0.0f, 0.0f, 0.0f}; pid.init = 0; @@ -254,6 +258,16 @@ void ControlSim::default_task() send_torque(TC_out); } +void ControlSim::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) +{ + memset(&sensors, 0, sizeof(sensors)); + memset(&dv, 0, sizeof(dv)); + memset(&tire, 0, sizeof(tire)); + memset(&pid, 0, sizeof(pid)); + memset(&state, 0, sizeof(state)); + +} + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); From 4ae4d40cd8c346f0c52955268afd16fe7823b8a4 Mon Sep 17 00:00:00 2001 From: jmlp05 Date: Fri, 14 Nov 2025 02:08:12 +0100 Subject: [PATCH 19/25] feat: implement EBS --- src/arussim/include/arussim/arussim_node.hpp | 21 +++-- src/arussim/src/arussim_node.cpp | 96 ++++++++++++++------ 2 files changed, 79 insertions(+), 38 deletions(-) diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index 82ae43bd..bccaa857 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -76,8 +76,6 @@ class Simulator : public rclcpp::Node std::string kTrackName; double kStateUpdateRate; - double kControllerRate; - bool kUseGSS; double kWheelBase; double kLidarFOV; double kCameraFOV; @@ -207,7 +205,8 @@ class Simulator : public rclcpp::Node */ void cone_visualization(); - void receive_can(); + void receive_can_0(); + void receive_can_1(); rclcpp::TimerBase::SharedPtr slow_timer_; rclcpp::TimerBase::SharedPtr fast_timer_; @@ -227,10 +226,16 @@ class Simulator : public rclcpp::Node rclcpp::TimerBase::SharedPtr receive_can_timer_; //CAN Communication - int can_socket_; - struct ifreq ifr_{}; - struct sockaddr_can addr_{}; - struct can_frame frame_; - std::thread thread_; + int can_socket_0_; + struct ifreq ifr_0_{}; + struct sockaddr_can addr_0_{}; + struct can_frame frame_0_; + std::thread thread_0_; + + int can_socket_1_; + struct ifreq ifr_1_{}; + struct sockaddr_can addr_1_{}; + struct can_frame frame_1_; + std::thread thread_1_; }; \ No newline at end of file diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index a5453237..9d715f36 100755 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -17,8 +17,6 @@ Simulator::Simulator() : Node("simulator") { this->declare_parameter("track", "FSG"); this->declare_parameter("state_update_rate", 1000); - this->declare_parameter("controller_rate", 100); - this->declare_parameter("use_gss", false); this->declare_parameter("vehicle.COG_front_dist", 1.9); this->declare_parameter("vehicle.COG_back_dist", -1.0); this->declare_parameter("vehicle.car_width", 0.8); @@ -41,8 +39,6 @@ Simulator::Simulator() : Node("simulator") this->get_parameter("track", kTrackName); this->get_parameter("state_update_rate", kStateUpdateRate); - this->get_parameter("controller_rate", kControllerRate); - this->get_parameter("use_gss", kUseGSS); this->get_parameter("vehicle.COG_front_dist", kCOGFrontDist); this->get_parameter("vehicle.COG_back_dist", kCOGBackDist); this->get_parameter("vehicle.car_width", kCarWidth); @@ -93,8 +89,11 @@ Simulator::Simulator() : Node("simulator") std::bind(&Simulator::on_fast_timer, this)); //Thread for CAN reception - std::thread thread_(&Simulator::receive_can, this); - thread_.detach(); + std::thread thread_0_(&Simulator::receive_can_0, this); + thread_0_.detach(); + + std::thread thread_1_(&Simulator::receive_can_1, this); + thread_1_.detach(); circuit_sub_ = this->create_subscription("/arussim/circuit", 1, std::bind(&Simulator::load_track, this, std::placeholders::_1)); @@ -122,12 +121,19 @@ Simulator::Simulator() : Node("simulator") marker_.lifetime = rclcpp::Duration::from_seconds(0.0); //CAN Socket setup - can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); - std::strcpy(ifr_.ifr_name, "can0"); - ioctl(can_socket_, SIOCGIFINDEX, &ifr_); - addr_.can_family = AF_CAN; - addr_.can_ifindex = ifr_.ifr_ifindex; - bind(can_socket_, (struct sockaddr *)&addr_, sizeof(addr_)); + can_socket_0_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + std::strcpy(ifr_0_.ifr_name, "can0"); + ioctl(can_socket_0_, SIOCGIFINDEX, &ifr_0_); + addr_0_.can_family = AF_CAN; + addr_0_.can_ifindex = ifr_0_.ifr_ifindex; + bind(can_socket_0_, (struct sockaddr *)&addr_0_, sizeof(addr_0_)); + + can_socket_1_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + std::strcpy(ifr_1_.ifr_name, "can1"); + ioctl(can_socket_1_, SIOCGIFINDEX, &ifr_1_); + addr_1_.can_family = AF_CAN; + addr_1_.can_ifindex = ifr_1_.ifr_ifindex; + bind(can_socket_1_, (struct sockaddr *)&addr_1_, sizeof(addr_1_)); // Initialize torque variable can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; @@ -331,15 +337,29 @@ void Simulator::on_fast_timer() { // Update state and broadcast transform rclcpp::Time current_time = clock_->now(); - if((current_time - time_last_cmd_).seconds() > 0.2 && vehicle_dynamics_.vx_ != 0) - { + if ((current_time - time_last_cmd_).seconds() > 0.2 && vehicle_dynamics_.vx_ != 0) { can_acc_ = 0; } + + if (as_status_ == 0x04 || as_status_ == 0x05) { + + // Trigger EBS + vehicle_dynamics_.torque_cmd_.fl_ = 0.; + vehicle_dynamics_.torque_cmd_.fr_ = 0.; + vehicle_dynamics_.torque_cmd_.rl_ = 0.; + vehicle_dynamics_.torque_cmd_.rr_ = 0.; + vehicle_dynamics_.vx_ = 0.0; + vehicle_dynamics_.vy_ = 0.0; + vehicle_dynamics_.r_ = 0.0; + + } else { + + double dt = 1.0 / kStateUpdateRate; + vehicle_dynamics_.update_simulation(can_delta_, can_torque_cmd_, dt); + + } - double dt = 1.0 / kStateUpdateRate; - vehicle_dynamics_.update_simulation(can_delta_, can_torque_cmd_, dt); - - if(use_tpl_){ + if (use_tpl_){ check_lap(); } @@ -399,21 +419,21 @@ void Simulator::on_fast_timer() marker_pub_->publish(marker_); } -void Simulator::receive_can() +void Simulator::receive_can_0() { - int flags = fcntl(can_socket_, F_GETFL, 0); - fcntl(can_socket_, F_SETFL, flags | O_NONBLOCK); + int flags = fcntl(can_socket_0_, F_GETFL, 0); + fcntl(can_socket_0_, F_SETFL, flags | O_NONBLOCK); while (rclcpp::ok()) { - int nbytes = read(can_socket_, &frame_, sizeof(struct can_frame)); + int nbytes = read(can_socket_0_, &frame_0_, sizeof(struct can_frame)); if (nbytes < 0) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); continue; } - if (frame_.can_id == 0x222) { - int16_t acc_scaled = static_cast((frame_.data[1] << 8) | frame_.data[0]); - int16_t yaw_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); - int16_t delta_scaled = static_cast((frame_.data[5] << 8) | frame_.data[4]); + if (frame_0_.can_id == 0x222) { + int16_t acc_scaled = static_cast((frame_0_.data[1] << 8) | frame_0_.data[0]); + int16_t yaw_scaled = static_cast((frame_0_.data[3] << 8) | frame_0_.data[2]); + int16_t delta_scaled = static_cast((frame_0_.data[5] << 8) | frame_0_.data[4]); can_acc_ = static_cast(acc_scaled) / 100.0f; can_target_r_ = static_cast(yaw_scaled) / 1000.0f; @@ -421,16 +441,31 @@ void Simulator::receive_can() time_last_cmd_ = clock_->now(); } - else if (frame_.can_id == 0x200 || frame_.can_id == 0x203 || - frame_.can_id == 0x206 || frame_.can_id == 0x209) { - int idx = (frame_.can_id - 0x200) / 3; // 0,1,2,3 - int16_t torque_scaled = static_cast((frame_.data[3] << 8) | frame_.data[2]); + else if (frame_0_.can_id == 0x200 || frame_0_.can_id == 0x203 || + frame_0_.can_id == 0x206 || frame_0_.can_id == 0x209) { + int idx = (frame_0_.can_id - 0x200) / 3; // 0,1,2,3 + int16_t torque_scaled = static_cast((frame_0_.data[3] << 8) | frame_0_.data[2]); can_torque_cmd_.at(idx) = torque_scaled * 9.8 / 1000.0 * kGearRatio; } } } +void Simulator::receive_can_1() +{ + int flags = fcntl(can_socket_1_, F_GETFL, 0); + fcntl(can_socket_1_, F_SETFL, flags | O_NONBLOCK); + while (rclcpp::ok()) { + int nbytes = read(can_socket_1_, &frame_1_, sizeof(struct can_frame)); + if (nbytes < 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + if (frame_1_.can_id == 0x261) { + as_status_ = frame_1_.data[1]; + } + } +} void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) { @@ -438,6 +473,7 @@ void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::Share can_delta_ = 0.0; can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; vehicle_dynamics_ = VehicleDynamics(); + as_status_ = 0x03; started_acc_ = false; if (prev_circuit_ != track_name_){ From 70f6cfd9638068d847d5a871be33daf501bf7dde Mon Sep 17 00:00:00 2001 From: Carmen Menendez <146075359+carmenmenendezda@users.noreply.github.com> Date: Fri, 14 Nov 2025 20:31:28 +0100 Subject: [PATCH 20/25] docs: Readme with instructions for passwordless virtual CAN setup Added instructions for configuring sudo to enable virtual CAN interfaces without a password. --- README.md | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 00000000..5598fc22 --- /dev/null +++ b/README.md @@ -0,0 +1,27 @@ +# Enabling Virtual CAN Interface for Launch Files + +Some launch files require setting up virtual CAN (`vcan`) interfaces automatically. +If any of these commands require `sudo`, the launch will fail because it cannot ask for a password interactively. + +To avoid this, you must configure sudo so that your user can execute the required `vcan` commands **without a password**. + +## 1. Create the sudoers rule + +Run: + +```bash +sudoedit /etc/sudoers.d/99-vcan +``` +Paste the following content, and replace with your actual Linux username: +```bash +Cmnd_Alias VCAN_CMDS = \ + /usr/sbin/modprobe vcan, \ + /usr/sbin/ip link add dev can0 type vcan, \ + /usr/sbin/ip link set up can0, \ + /usr/sbin/ip link set down can0, \ + /usr/sbin/ip link add dev can1 type vcan, \ + /usr/sbin/ip link set up can1, \ + /usr/sbin/ip link set down can1 + + ALL=(ALL) NOPASSWD: VCAN_CMDS +``` From 94e2bf24519c4f8cf6eb033518779d4712531b96 Mon Sep 17 00:00:00 2001 From: RafaGuil Date: Sun, 16 Nov 2025 22:08:08 +0100 Subject: [PATCH 21/25] feat: add AS status logic --- src/arussim/include/arussim/arussim_node.hpp | 12 +- .../include/arussim/main_interface.hpp | 1 + src/arussim/resources/tracks/test5N.pcd | 165 ++++++++++++++++++ src/arussim/src/arussim_node.cpp | 11 +- src/arussim/src/main_interface.cpp | 18 +- 5 files changed, 197 insertions(+), 10 deletions(-) create mode 100644 src/arussim/resources/tracks/test5N.pcd diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index bccaa857..df685fc3 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -107,7 +107,7 @@ class Simulator : public rclcpp::Node float can_target_r_; float can_delta_; std::vector can_torque_cmd_; - uint16_t as_status_; + uint16_t as_status_ = 0x02; visualization_msgs::msg::Marker marker_; pcl::PointCloud track_; @@ -173,6 +173,15 @@ class Simulator : public rclcpp::Node */ void reset_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg); + /** + * @brief Callback for receiving launch commands. + * + * This method sets AS Status in AS Driving + * + * @param msg The launch command message. + */ + void launch_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg); + /** * @brief Broadcasts the vehicle's current pose to the ROS TF system. * @@ -222,6 +231,7 @@ class Simulator : public rclcpp::Node rclcpp::Publisher::SharedPtr fixed_trajectory_pub_; std::shared_ptr tf_broadcaster_; rclcpp::Subscription::SharedPtr reset_sub_; + rclcpp::Subscription::SharedPtr launch_sub_; rclcpp::Subscription::SharedPtr circuit_sub_; rclcpp::TimerBase::SharedPtr receive_can_timer_; diff --git a/src/arussim/include/arussim/main_interface.hpp b/src/arussim/include/arussim/main_interface.hpp index 27ffc24a..d8672e48 100644 --- a/src/arussim/include/arussim/main_interface.hpp +++ b/src/arussim/include/arussim/main_interface.hpp @@ -97,6 +97,7 @@ private Q_SLOTS: private: rclcpp::Publisher::SharedPtr reset_pub_; + rclcpp::Publisher::SharedPtr launch_pub_; rclcpp::Publisher::SharedPtr circuit_pub_; rclcpp::Subscription::SharedPtr torque_sub_; diff --git a/src/arussim/resources/tracks/test5N.pcd b/src/arussim/resources/tracks/test5N.pcd new file mode 100644 index 00000000..10149a95 --- /dev/null +++ b/src/arussim/resources/tracks/test5N.pcd @@ -0,0 +1,165 @@ +# .PCD v0.7 - Point Cloud Data file format +VERSION 0.7 +FIELDS x y z color score +SIZE 4 4 4 4 4 +TYPE F F F I F +COUNT 1 1 1 1 1 +WIDTH 154 +HEIGHT 1 +VIEWPOINT 0 0 0 1 0 0 0 +POINTS 154 +DATA ascii +14.223622 5.98419279 0 0 1 +13.780501 1.7277696 0 1 1 +16.975018 5.13657829 0 0 1 +17.907997 1.8205702 0 1 1 +9.9250555 5.81828094 0 0 1 +20.407818 5.55294061 0 0 1 +21.737444 2.3247988 0 1 1 +25.425308 2.5441878 0 1 1 +23.625166 5.66828614 0 0 1 +28.121223 6.6085901 0 0 1 +29.738642 3.5711939 0 1 1 +32.725769 8.6368785 0 0 1 +34.164955 5.45977724 0 1 1 +35.919838 10.3412681 0 0 1 +38.761589 11.1506948 0 0 1 +37.953796 7.2695458 0 1 1 +42.06662 11.764596 0 0 1 +42.331264 8.4803157 0 1 1 +48.708218 3.4388704 0 1 1 +46.654606 11.0637484 0 0 1 +46.650772 6.9209198 0 1 1 +51.597481 5.55687201 0 0 1 +49.334389 9.4770365 0 0 1 +52.346882 2.8635857 0 0 1 +53.203068 -0.9746079 0 0 1 +49.875187 -1.2312565 0 1 1 +53.323139 -5.275153 0 0 1 +50.364399 -6.102749 0 1 1 +54.219181 -13.667179 0 0 1 +53.915901 -9.396274 0 0 1 +50.641438 -11.502142 0 1 1 +53.580254 -17.927986 0 0 1 +50.528336 -15.439575 0 1 1 +49.241966 -19.311449 0 1 1 +52.604149 -21.500561 0 0 1 +48.933628 -24.433556 0 1 1 +53.042301 -24.073719 0 0 1 +54.930538 -26.009993 0 0 1 +51.358269 -28.343616 0 1 1 +57.947254 -29.100845 0 0 1 +54.372696 -31.308205 0 1 1 +61.17701 -33.221188 0 0 1 +60.356464 -26.669502 0 0 1 +57.340233 -35.247543 0 1 1 +58.149754 -38.979206 0 1 1 +62.314705 -42.048321 0 0 1 +62.613815 -37.534348 0 0 1 +57.26984 -43.947796 0 1 1 +55.141979 -47.37188 0 1 1 +61.23035 -45.80249 0 0 1 +59.103619 -49.526138 0 0 1 +56.637581 -52.674244 0 0 1 +53.015507 -50.213207 0 1 1 +52.842896 -55.290417 0 0 1 +50.145168 -52.440159 0 1 1 +47.53154 -57.754833 0 0 1 +45.800705 -54.566399 0 1 1 +40.998466 -56.67268 0 1 1 +43.201633 -59.399681 0 0 1 +39.25658 -60.64299 0 0 1 +36.358665 -57.362274 0 1 1 +34.923035 -60.959846 0 0 1 +31.031452 -56.602402 0 1 1 +30.490492 -60.527336 0 0 1 +26.950975 -54.989452 0 1 1 +27.709358 -59.423836 0 0 1 +24.062603 -58.364426 0 0 1 +23.211956 -54.099232 0 1 1 +20.158579 -60.348022 0 0 1 +18.807369 -56.124264 0 1 1 +17.082617 -62.610168 0 0 1 +14.809249 -58.872139 0 1 1 +12.948959 -63.357292 0 0 1 +10.489794 -59.761383 0 1 1 +10.189308 -43.281467 0 1 1 +9.0862131 -63.901794 0 0 1 +6.234385 -59.996315 0 1 1 +5.6483779 -64.21032 0 0 1 +2.7975194 -64.609909 0 0 1 +-0.72883087 -60.038605 0 1 1 +2.333976 -60.472687 0 1 1 +-0.085303366 -64.817604 0 0 1 +-2.6500347 -64.74086 0 0 1 +-3.9766328 -59.390503 0 1 1 +-5.7535729 -64.127098 0 0 1 +-6.5302453 -57.944225 0 1 1 +-6.7087231 -55.617691 0 1 1 +-9.4257584 -62.469002 0 0 1 +-10.683232 -56.178883 0 0 1 +-11.045506 -59.220016 0 0 1 +-10.243578 -52.564419 0 0 1 +-5.000545 -52.449753 0 1 1 +-8.6100788 -50.08559 0 0 1 +-5.2963681 -47.731915 0 0 1 +-2.1976078 -50.224106 0 1 1 +-2.5162716 -45.500957 0 0 1 +0.58596164 -48.914982 0 1 1 +0.9815008 -43.163113 0 0 1 +4.3466625 -41.773621 0 0 1 +3.7765629 -47.58823 0 1 1 +7.562767 -40.347839 0 0 1 +6.4897003 -45.947369 0 1 1 +12.730247 -40.814663 0 1 1 +9.533371 -37.698753 0 0 1 +3.8143661 -30.184383 0 0 1 +7.0530334 -31.955444 0 0 1 +9.2097578 -34.486748 0 0 1 +14.184353 -37.498344 0 1 1 +13.804348 -34.704929 0 1 1 +12.925924 -31.551971 0 1 1 +9.558033 -28.709076 0 1 1 +5.6336675 -26.337027 0 1 1 +2.232271 -25.640343 0 1 1 +-0.10812477 -28.92408 0 0 1 +-2.3157873 -23.95108 0 1 1 +-3.7980635 -28.094204 0 0 1 +-5.9566855 -23.750002 0 1 1 +-8.0211754 -27.885426 0 0 1 +-9.2966633 -23.907169 0 1 1 +-12.989143 -28.149853 0 0 1 +-12.49553 -24.209963 0 1 1 +-17.918098 -27.364662 0 0 1 +-16.1136 -23.815435 0 1 1 +-19.113916 -22.101881 0 1 1 +-20.946474 -25.748753 0 0 1 +-20.869314 -14.447479 0 1 1 +-20.948503 -19.720161 0 1 1 +-24.152456 -23.670664 0 0 1 +-21.46352 -17.137035 0 1 1 +-25.844046 -21.221977 0 0 1 +-25.90893 -16.291836 0 0 1 +-25.036642 -12.638802 0 0 1 +-24.028463 -10.182631 0 0 1 +-21.98127 -7.901159 0 0 1 +-19.194138 -11.616842 0 1 1 +-19.725657 -6.036614 0 0 1 +-17.230211 -9.887754 0 1 1 +-15.090009 -7.483506 0 1 1 +-16.911655 -3.6393833 0 0 1 +-13.761188 -1.8580828 0 0 1 +-12.149392 -5.710672 0 1 1 +-11.482224 -1.4057136 0 0 1 +-8.6627035 -0.6211581 0 0 1 +-9.5758867 -5.079868 0 1 1 +-7.1955757 -4.2522173 0 1 1 +-5.0662551 0.7283335 0 0 1 +-4.0650554 -2.8703856 0 1 1 +-0.61997283 -2.1113009 0 1 1 +-1.4823751 1.7040827 0 0 1 +2.7384558 -1.9407558 0 1 1 +2.0724301 3.4695301 0 0 1 +6.7634597 -1.5902333 0 1 1 +6.2183757 4.6336565 0 0 1 +10.756487 -0.5432348 0 0 1 diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index 9d715f36..21906cb4 100755 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -103,6 +103,9 @@ Simulator::Simulator() : Node("simulator") reset_sub_ = this->create_subscription("/arussim/reset", 1, std::bind(&Simulator::reset_callback, this, std::placeholders::_1)); + launch_sub_ = this->create_subscription("/arussim/launch", 1, + std::bind(&Simulator::launch_callback, this, std::placeholders::_1)); + // Load the car mesh marker_.header.frame_id = "arussim/vehicle_cog"; marker_.type = visualization_msgs::msg::Marker::MESH_RESOURCE; @@ -341,7 +344,7 @@ void Simulator::on_fast_timer() can_acc_ = 0; } - if (as_status_ == 0x04 || as_status_ == 0x05) { + if (as_status_ != 0x03) { // Trigger EBS vehicle_dynamics_.torque_cmd_.fl_ = 0.; @@ -466,6 +469,10 @@ void Simulator::receive_can_1() } } } +void Simulator::launch_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) +{ + if (as_status_ == 0x02) as_status_ = 0x03; +} void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) { @@ -473,7 +480,7 @@ void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::Share can_delta_ = 0.0; can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; vehicle_dynamics_ = VehicleDynamics(); - as_status_ = 0x03; + as_status_ = 0x02; started_acc_ = false; if (prev_circuit_ != track_name_){ diff --git a/src/arussim/src/main_interface.cpp b/src/arussim/src/main_interface.cpp index edfa0a58..f43fbaa6 100644 --- a/src/arussim/src/main_interface.cpp +++ b/src/arussim/src/main_interface.cpp @@ -144,6 +144,7 @@ void MainInterface::onInitialize() // Publishers reset_pub_ = node->create_publisher("/arussim/reset", 1); + launch_pub_ = node->create_publisher("/arussim/launch", 1); circuit_pub_ = node->create_publisher("/arussim/circuit", 1); // Subscribers @@ -199,7 +200,12 @@ void MainInterface::update_lap_time_labels(double lap_time_) */ void MainInterface::launch_button_clicked() { - + QProcess can_process; + can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "up" << "can0"); + can_process.waitForFinished(); + can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "up" << "can1"); + can_process.waitForFinished(); + if (simulation_process_ == nullptr) { simulation_process_ = new QProcess(this); // Merge standard output and error @@ -212,12 +218,10 @@ void MainInterface::launch_button_clicked() args << "launch" << "common_meta" << launch_file; simulation_process_->start("ros2", args); } - QProcess can_process; - can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "up" << "can0"); - can_process.waitForFinished(); - can_process.start("sudo", QStringList() << "ip" << "link" << "set" << "up" << "can1"); - can_process.waitForFinished(); - + + auto msg = std_msgs::msg::Bool(); + msg.data = true; + launch_pub_->publish(msg); } /** From 6ca45fd7139ccff54e6fcc1413f4a64c0c303bb5 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Sun, 16 Nov 2025 23:52:41 +0100 Subject: [PATCH 22/25] fix: TV_Active double declaration --- src/arussim/src/control_sim/torque_vectoring.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/arussim/src/control_sim/torque_vectoring.c b/src/arussim/src/control_sim/torque_vectoring.c index 1972dd56..9c6b8652 100644 --- a/src/arussim/src/control_sim/torque_vectoring.c +++ b/src/arussim/src/control_sim/torque_vectoring.c @@ -8,7 +8,6 @@ #include "arussim/control_sim/torque_vectoring.h" float tire_load[4]; -uint8_t TV_active; float target_r; @@ -22,7 +21,7 @@ void TorqueVectoring_Init(PID *pid) { } void TorqueVectoring_Update(SensorData *sensors, Parameters *parameters, PID *pid, TIRE *tire, DV *dv, float fx_request, float *state, float *torque_out) { - if (TV_active == 1){ + if (TV_ACTIVE == 1){ float Mz_request = Target_Generation(sensors, parameters, pid, dv, state); // float fz_front_mean = 0.5f * (tire->tire_load[0] + tire->tire_load[1]); From 580ca78563be08bb27c9bbdd426d3a378919e104 Mon Sep 17 00:00:00 2001 From: Carmen Menendez Date: Mon, 17 Nov 2025 00:12:41 +0100 Subject: [PATCH 23/25] fix: autonomous mode at target generation --- src/arussim/src/control_sim/control_sim.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index 7923af27..9c99b771 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -48,6 +48,7 @@ ControlSim::ControlSim() : Node("control_sim") { PowerControl_Init(¶meters); TractionControl_Init(&pid, ¶meters); TorqueVectoring_Init(&pid); + dv.autonomous = 1; } From c170fe04c8025bbc262c45c8bcefad09e7f0fecf Mon Sep 17 00:00:00 2001 From: jmlp05 Date: Wed, 19 Nov 2025 18:58:05 +0100 Subject: [PATCH 24/25] fix: correct GSS speed conversions --- src/arussim/config/sensors_config.yaml | 4 ++-- src/arussim/include/arussim/sensors.hpp | 4 ++-- src/arussim/src/control_sim/control_sim.cpp | 4 ++-- src/arussim/src/sensors.cpp | 16 +++++++--------- 4 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/arussim/config/sensors_config.yaml b/src/arussim/config/sensors_config.yaml index 625bad4e..84a5b4b9 100755 --- a/src/arussim/config/sensors_config.yaml +++ b/src/arussim/config/sensors_config.yaml @@ -1,7 +1,7 @@ arussim: ros__parameters: gss: - gss_frequency: 100.0 # Hz Publish rate + gss_frequency: 200.0 # Hz Publish rate noise_gss_vx: 0.01 # kmh Noise standard deviation noise_gss_vy: 0.01 # kmh Noise standard deviation noise_imu_ax: 0.005 # m/s² Noise standard deviation @@ -9,7 +9,7 @@ arussim: noise_imu_r: 0.025 # º/s Noise standard deviation inverter: - inverter_frequency: 100.0 # Hz Publish rate + inverter_frequency: 200.0 # Hz Publish rate noise_motor_speed_front_right: 0.005 # rpm Noise standard deviation (front right wheel) noise_motor_speed_front_left: 0.005 # rpm Noise standard deviation (front left wheel) noise_motor_speed_rear_right: 0.005 # rpm Noise standard deviation (rear right wheel) diff --git a/src/arussim/include/arussim/sensors.hpp b/src/arussim/include/arussim/sensors.hpp index 9e1cba16..6e3be417 100644 --- a/src/arussim/include/arussim/sensors.hpp +++ b/src/arussim/include/arussim/sensors.hpp @@ -112,10 +112,10 @@ class Sensors : public rclcpp::Node void extensometer_timer(); /** - * @brief Timer function for the IMU + * @brief Timer function for the GSS * */ - void imu_timer(); + void gss_timer(); /** * @brief Timer function for the 4WD torque diff --git a/src/arussim/src/control_sim/control_sim.cpp b/src/arussim/src/control_sim/control_sim.cpp index 9c99b771..2d48d7a1 100644 --- a/src/arussim/src/control_sim/control_sim.cpp +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -76,8 +76,8 @@ ControlSim::ControlSim() : Node("control_sim") { vx_scaled_ = static_cast((frame.data[1] << 8) | frame.data[0]); vy_scaled_ = static_cast((frame.data[3] << 8) | frame.data[2]); - sensors.speed_x = vx_scaled_ * 0.2 / 3.6; - sensors.speed_y = vy_scaled_ * 0.2 / 3.6; + sensors.speed_x = vx_scaled_ * 0.02 / 3.6; + sensors.speed_y = vy_scaled_ * 0.02 / 3.6; } else if (frame.can_id == 0x1A3) { diff --git a/src/arussim/src/sensors.cpp b/src/arussim/src/sensors.cpp index fbb139f0..240b2fd0 100644 --- a/src/arussim/src/sensors.cpp +++ b/src/arussim/src/sensors.cpp @@ -77,9 +77,9 @@ Sensors::Sensors() : Node("sensors") ); // IMU - imu_timer_ = this->create_wall_timer( + gss_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1000/kGssFrequency)), - std::bind(&Sensors::imu_timer, this) + std::bind(&Sensors::gss_timer, this) ); @@ -98,8 +98,8 @@ Sensors::Sensors() : Node("sensors") frames = { {0x1A0, 4, { - {"GSS/vx", {0, 15, true, 0.0555556, 0.0}}, - {"GSS/vy", {16, 31, true, 0.0555556, 0.0}} + {"GSS/vx", {0, 15, true, 0.00555556, 0.0}}, + {"GSS/vy", {16, 31, true, 0.00555556, 0.0}} }}, {0x1A3, 4, { // GSS {"IMU/ax", {0, 15, true, 0.02, 0.0}}, @@ -223,10 +223,10 @@ void Sensors::inverter_timer(){ } /** - * @brief Timer function for the IMU + * @brief Timer function for the GSS * */ -void Sensors::imu_timer() +void Sensors::gss_timer() { // Random noise generation with different noise for each variable std::random_device rd; @@ -237,9 +237,7 @@ void Sensors::imu_timer() std::normal_distribution<> dist_ax(0.0, kNoiseImuAx); std::normal_distribution<> dist_ay(0.0, kNoiseImuAy); std::normal_distribution<> dist_r(0.0, kNoiseImuR); - - //TODO: Add noise to vx and vy - //Send CAN frames for IMU (GSS ids) + std::map values = { {"IMU/ax", ax_ + dist_ax(gen)}, {"IMU/ay", ay_ + dist_ay(gen)}, {"IMU/yaw_rate", r_ + dist_r(gen)}, {"GSS/vx", vx_ + dist_vx(gen)}, {"GSS/vy", vy_ + dist_vy(gen)} }; From 8e5e43d436a02181026ef108340a6fcff91af66f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ignacio=20S=C3=A1nchez?= Date: Wed, 19 Nov 2025 20:00:55 +0100 Subject: [PATCH 25/25] fix: rename gss timer in header file to match cpp file --- src/arussim/include/arussim/sensors.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arussim/include/arussim/sensors.hpp b/src/arussim/include/arussim/sensors.hpp index 6e3be417..47eee948 100644 --- a/src/arussim/include/arussim/sensors.hpp +++ b/src/arussim/include/arussim/sensors.hpp @@ -129,7 +129,7 @@ class Sensors : public rclcpp::Node // ROS Communication rclcpp::Subscription::SharedPtr state_sub_; // State subscriber - rclcpp::TimerBase::SharedPtr imu_timer_; // IMU timer + rclcpp::TimerBase::SharedPtr gss_timer_; // GSS timer rclcpp::TimerBase::SharedPtr ext_timer_; // Extensometer timer rclcpp::Publisher::SharedPtr torque_pub_; // Torque publisher rclcpp::TimerBase::SharedPtr inverter_timer_; // Inverter timer