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 +``` diff --git a/src/arussim/CMakeLists.txt b/src/arussim/CMakeLists.txt index 2c2a2b4e..275d2c05 100755 --- a/src/arussim/CMakeLists.txt +++ b/src/arussim/CMakeLists.txt @@ -117,11 +117,27 @@ 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 + 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 + 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/sensors_config.yaml b/src/arussim/config/sensors_config.yaml index 2bd39ac3..84a5b4b9 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: 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 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: 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) + 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 d8939ea5..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 @@ -11,6 +9,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 bd146cc9..df685fc3 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -41,11 +41,14 @@ #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 +#include +#include +#include +#include +#include /** * @class Simulator @@ -70,12 +73,9 @@ class Simulator : public rclcpp::Node private: VehicleDynamics vehicle_dynamics_; - ControllerSim controller_sim_; std::string kTrackName; double kStateUpdateRate; - double kControllerRate; - bool kUseGSS; double kWheelBase; double kLidarFOV; double kCameraFOV; @@ -92,6 +92,7 @@ class Simulator : public rclcpp::Node double kSimulationSpeedMultiplier; bool kTorqueVectoring; bool kDebug; + double kGearRatio; //Car boundaries double kCOGFrontDist; @@ -100,10 +101,13 @@ class Simulator : public rclcpp::Node 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_; + std::vector can_torque_cmd_; + uint16_t as_status_ = 0x02; visualization_msgs::msg::Marker marker_; pcl::PointCloud track_; @@ -140,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. @@ -156,15 +153,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 +163,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. @@ -193,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. * @@ -225,16 +214,13 @@ class Simulator : public rclcpp::Node */ void cone_visualization(); + void receive_can_0(); + void receive_can_1(); + 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_; - 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_; @@ -245,6 +231,21 @@ 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_; + + //CAN Communication + 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/include/arussim/control_sim/Parameters.h b/src/arussim/include/arussim/control_sim/Parameters.h new file mode 100644 index 00000000..d691935f --- /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.00001 +#define GRAVITY 9.81 +#define DEFAULT_MAX_POWER 50000 +#define DEFAULT_MIN_POWER -30000 + +#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. +#define DEFAULT_TL_NEG -21. + +//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 new file mode 100644 index 00000000..c30cc5a1 --- /dev/null +++ b/src/arussim/include/arussim/control_sim/control_sim.hpp @@ -0,0 +1,67 @@ +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include "std_msgs/msg/bool.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: + 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_; + + 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; + + +}; \ No newline at end of file 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/include/arussim/main_interface.hpp b/src/arussim/include/arussim/main_interface.hpp index 1c1f1cf2..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_; @@ -111,7 +112,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 4e9eff0f..47eee948 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; @@ -39,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; @@ -59,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; @@ -92,41 +112,33 @@ 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 + * @brief Timer function for the GSS * */ - void imu_timer(); + void gss_timer(); /** * @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 - 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::TimerBase::SharedPtr ws_timer_; // Wheel speed timer - - rclcpp::Publisher::SharedPtr ext_pub_; // Extensometer publisher + rclcpp::TimerBase::SharedPtr gss_timer_; // GSS timer 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/include/arussim/vehicle_dynamics.hpp b/src/arussim/include/arussim/vehicle_dynamics.hpp index 54f8eeb5..f8864cd1 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: @@ -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/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/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index c90f8615..8dceeea9 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -1,19 +1,70 @@ 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, 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', 'arussim_rviz_config.rviz' ) - + modprobe_vcan = ExecuteProcess( + cmd=['sudo','modprobe', 'vcan'], + shell=False + ) + 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 = 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( + 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, + *sequence, Node( package='rviz2', executable='rviz2', @@ -30,7 +81,9 @@ def generate_launch_description(): config='sensors_config.yaml'), create_node(pkg='arussim', exec='supervisor_exec', - config='supervisor_config.yaml') + config='supervisor_config.yaml'), + create_node(pkg='arussim', + exec='control_sim_exec'), ]) @@ -49,5 +102,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/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/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 708930b8..21906cb4 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); @@ -37,11 +35,10 @@ 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); - 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); @@ -60,18 +57,13 @@ 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); 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,22 +87,25 @@ 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)); + + //Thread for CAN reception + std::thread thread_0_(&Simulator::receive_can_0, this); + thread_0_.detach(); + + std::thread thread_1_(&Simulator::receive_can_1, this); + thread_1_.detach(); - 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)); + 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; @@ -127,13 +122,25 @@ Simulator::Simulator() : Node("simulator") marker_.color.b = 70.0/255.0; marker_.color.a = 1.0; marker_.lifetime = rclcpp::Duration::from_seconds(0.0); - - // Set controller_sim period and GSS usage - controller_sim_.init(1/kControllerRate, kUseGSS); + + //CAN Socket setup + 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 - 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) { csv_generator_state_ = std::make_shared("state"); @@ -322,38 +329,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(input_acc_, 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. @@ -365,16 +340,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) - { - input_acc_ = 0; + if ((current_time - time_last_cmd_).seconds() > 0.2 && vehicle_dynamics_.vx_ != 0) { + can_acc_ = 0; + } + + if (as_status_ != 0x03) { + + // 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(input_delta_, torque_cmd_, dt); - - if(use_tpl_){ + if (use_tpl_){ check_lap(); } @@ -434,41 +422,65 @@ 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_0() { - input_acc_ = msg->acc; - input_delta_ = msg->delta; - target_r_ = msg->target_r; - time_last_cmd_ = clock_->now(); + 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_0_, &frame_0_, sizeof(struct can_frame)); + if (nbytes < 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + + 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; + can_delta_ = static_cast(delta_scaled) / 100.0f; + time_last_cmd_ = clock_->now(); + } + + 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::ebs_callback(const std_msgs::msg::Bool::SharedPtr msg) +void Simulator::receive_can_1() { - 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; + 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::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) { - input_acc_ = 0.0; - input_delta_ = 0.0; - torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; - + can_acc_ = 0.0; + can_delta_ = 0.0; + can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; vehicle_dynamics_ = VehicleDynamics(); + as_status_ = 0x02; started_acc_ = false; if (prev_circuit_ != track_name_){ @@ -730,4 +742,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/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 new file mode 100644 index 00000000..2d48d7a1 --- /dev/null +++ b/src/arussim/src/control_sim/control_sim.cpp @@ -0,0 +1,278 @@ +/** + * @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") { + + + clock_ = std::make_shared(RCL_SYSTEM_TIME); + // Timers + 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); + 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_)); + + + 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; + Parameters_Init(¶meters); + Estimation_Init(); + PowerControl_Init(¶meters); + TractionControl_Init(&pid, ¶meters); + TorqueVectoring_Init(&pid); + dv.autonomous = 1; + + +} + + void ControlSim::receive_can() +{ + 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.02 / 3.6; + sensors.speed_y = vy_scaled_ * 0.02 / 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[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[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[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[2] << 16)| (frame.data[1] << 8) | frame.data[0]); + sensors.motor_speed[3] = ws_scaled_3 * 0.0001047197551196; + } + + } + +} + +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 (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[2] = torque_i_ & 0xFF; + frame.data[3] = (torque_i_ >> 8) & 0xFF; + + 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; + 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)(state[0]* 100.0f); + frame.data[0] = vx_i_ & 0xFF; + frame.data[1] = (vx_i_ >> 8) & 0xFF; + + vy_i_ = (int)(state[1] * 100.0f); + frame.data[2] = vy_i_ & 0xFF; + frame.data[3] = (vy_i_ >> 8) & 0xFF; + + 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)); + + +} + +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); + + 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); + + if (pid.init == 0){ + pid.init = 1; + } + + 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); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file 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..b87d0497 --- /dev/null +++ b/src/arussim/src/control_sim/power_control.c @@ -0,0 +1,42 @@ +/* + * 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]; + + 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++) { + torque_out[i] = torque_out[i] * (1 - PW_extra); + } + + } else if (PW_total < PW_min){ + + float PW_extra = (PW_total - PW_min)/PW_total; + for (int i = 2; i < 4; i++) { + torque_out[i] = torque_out[i] * (1 - PW_extra); + } + + } + +} \ 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..9c6b8652 --- /dev/null +++ b/src/arussim/src/control_sim/torque_vectoring.c @@ -0,0 +1,119 @@ +/* + * torque_vectoring.c + * + * 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]; +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] = 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]) { + 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 { + 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]; + } 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 diff --git a/src/arussim/src/main_interface.cpp b/src/arussim/src/main_interface.cpp index c803f6d4..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,6 +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 @@ -211,6 +218,10 @@ void MainInterface::launch_button_clicked() args << "launch" << "common_meta" << launch_file; simulation_process_->start("ros2", args); } + + auto msg = std_msgs::msg::Bool(); + msg.data = true; + launch_pub_->publish(msg); } /** @@ -234,6 +245,13 @@ void MainInterface::stop_button_clicked() */ 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; reset_pub_->publish(msg); diff --git a/src/arussim/src/sensors.cpp b/src/arussim/src/sensors.cpp index b882de32..240b2fd0 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 @@ -15,52 +15,60 @@ */ 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); + //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( @@ -69,45 +77,61 @@ 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) + gss_timer_ = this->create_wall_timer( + std::chrono::milliseconds((int)(1000/kGssFrequency)), + std::bind(&Sensors::gss_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); ext_timer_ = this->create_wall_timer( std::chrono::milliseconds((int)(1000/kExtensometerFrequency)), std::bind(&Sensors::extensometer_timer, this) ); - // Torque cmd + // Inverter torque_pub_ = this->create_publisher("/arussim/torque4WD", 10); - torque_timer_ = this->create_wall_timer( - std::chrono::milliseconds((int)(1000/kTorqueFrequency)), - std::bind(&Sensors::torque_cmd_timer, this) + inverter_timer_ = this->create_wall_timer( + std::chrono::milliseconds((int)(1000/kInverterFrequency)), + std::bind(&Sensors::inverter_timer, this) ); + + frames = { + {0x1A0, 4, { + {"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}}, + {"IMU/ay", {16, 31, true, 0.02, 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}} + }}, + {0x102, 5, { // Front Left inverter + {"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.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.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.0001047197551196, 0.0}}, + {"rr_inv_torque", {24, 39, true, 0.0098, 0.0}} + }}, + {0x161, 2, { + {"as_status", {8, 15, false, 1.0, 0.0}} + }} + }; } /** @@ -127,15 +151,25 @@ 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 +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()); @@ -147,10 +181,10 @@ void Sensors::torque_cmd_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(); @@ -162,78 +196,61 @@ 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, 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_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_*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) { + send_can_frame(frame, values); + } +} } /** - * @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; 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); - // 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); + 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)} }; - // Publish the IMU message - ax_pub_->publish(msg_ax); - ay_pub_->publish(msg_ay); - r_pub_->publish(msg_r); + for (auto &frame : frames) { + if (frame.id == 0x1A3 || frame.id == 0x1A4 || frame.id == 0x1A0) { + 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 * @@ -245,10 +262,33 @@ void Sensors::extensometer_timer() std::mt19937 gen(rd()); std::normal_distribution<> dist(0.0, kNoiseExtensometer); + //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); + } + } +} - auto message = std_msgs::msg::Float32(); - message.data = delta_ + dist(gen); - ext_pub_->publish(message); +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_)); } /**