Feature/arussim/control simulator v2

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 <your-username> 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
+
+<your-username> ALL=(ALL) NOPASSWD: VCAN_CMDS
+```

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

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:

src/arussim/config/simulator_config.yaml

@@ -2,15 +2,14 @@ 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
 
     vehicle:
       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

src/arussim/include/arussim/arussim_node.hpp

@@ -41,11 +41,14 @@
 #include <nlohmann/json.hpp>
 #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 <linux/can.h>       
+#include <linux/can/raw.h>   
+#include <net/if.h>          
+#include <sys/ioctl.h>       
+#include <sys/socket.h>    
+#include <fcntl.h>
+#include <thread> 
 
 /**
  * @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<double> torque_cmd_;
+
+    //Can
+    float can_acc_;
+    float can_target_r_;
+    float can_delta_;
+    std::vector<double> can_torque_cmd_;
+    uint16_t as_status_ = 0x02;
 
     visualization_msgs::msg::Marker marker_;
     pcl::PointCloud<ConeXYZColorScore> 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<arussim_msgs::msg::Cmd>::SharedPtr cmd_sub_;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr ebs_sub_;
     rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr rviz_telep_sub_;
     rclcpp::Publisher<arussim_msgs::msg::State>::SharedPtr state_pub_;
-    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr control_vx_pub_;
-    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr control_vy_pub_;
-    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr control_r_pub_;
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr track_pub_;
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_perception_pub_;
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr camera_perception_pub_;
@@ -245,6 +231,21 @@ class Simulator : public rclcpp::Node
     rclcpp::Publisher<arussim_msgs::msg::Trajectory>::SharedPtr fixed_trajectory_pub_;
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr reset_sub_;
+    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr launch_sub_;
     rclcpp::Subscription<std_msgs::msg::String>::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_;
+
 };