diff --git a/src/arussim/include/arussim/arussim_node.hpp b/src/arussim/include/arussim/arussim_node.hpp index 71986b64..627f6c33 100755 --- a/src/arussim/include/arussim/arussim_node.hpp +++ b/src/arussim/include/arussim/arussim_node.hpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include "arussim/csv_generator.hpp" @@ -182,6 +183,7 @@ class Simulator : public rclcpp::Node //raspi_sim control_raspi::ControlRaspi control_raspi_manager_; + std::array amk_enabled_ = {false, false, false, false}; visualization_msgs::msg::Marker marker_; pcl::PointCloud track_; @@ -230,6 +232,15 @@ class Simulator : public rclcpp::Node */ void on_controller_sim_timer(); + /** + * @brief Callback function for the raspi_sim timer. + * + * Periodically sends on can0 the status frames Control-RaspPi needs to + * command torque: 0x160 (autonomous mode), 0x161 (AS driving) and + * 0x221 (R2D enable flag). Only active while the simulation is launched. + */ + void on_raspi_sim_timer(); + /** * @brief Callback function for the fast timer. * @@ -376,6 +387,7 @@ class Simulator : public rclcpp::Node rclcpp::TimerBase::SharedPtr slow_timer_; rclcpp::TimerBase::SharedPtr fast_timer_; rclcpp::TimerBase::SharedPtr controller_sim_timer_; + rclcpp::TimerBase::SharedPtr raspi_sim_timer_; rclcpp::TimerBase::SharedPtr perception_timer_; rclcpp::Subscription::SharedPtr cmd_sub_; rclcpp::Subscription::SharedPtr ebs_sub_; diff --git a/src/arussim/include/arussim/control_raspi.hpp b/src/arussim/include/arussim/control_raspi.hpp index d41f5b74..bd82d0ea 100644 --- a/src/arussim/include/arussim/control_raspi.hpp +++ b/src/arussim/include/arussim/control_raspi.hpp @@ -20,9 +20,10 @@ namespace control_raspi { namespace { - // Relative path from the ControlRaspi executable to the Control-RaspPi executable - // Change this path if doesn't match your workspace structure - constexpr const char *kControlRaspRelativePath = "../../../../../ws_raspi/src/Control-RaspPi/build/ControlRaspi"; + // Path to the Control-RaspPi executable, relative to some ancestor directory of the + // arussim executable (searched upwards, so it works from both build/ and install/ layouts). + // Change this path if it doesn't match your workspace structure. + constexpr const char *kControlRaspFromAncestor = "ws_raspi/src/Control-RaspPi/build/ControlRaspi"; /** * @brief Get the directory of the current executable @@ -183,43 +184,49 @@ namespace control_raspi private: /** - * @brief Get the directory of the current executable - * - * Not found or Not executable are related to a feilure to resolve the relative path + * @brief Resolve the path to the Control-RaspPi executable * + * Walks up from the arussim executable directory looking for + * kControlRaspFromAncestor at each level, so it works regardless of + * whether arussim runs from install/ (real copy) or build/ (symlink install). */ std::string resolve_control_rasp_path(rclcpp::Logger logger) const { const std::string exe_dir = get_executable_dir(); if (exe_dir.empty()) - return std::string{}; - - std::error_code error; - fs::path candidate = fs::path(exe_dir) / kControlRaspRelativePath; - fs::path normalized = fs::weakly_canonical(candidate, error); - if (error) - normalized = candidate; - - if (!fs::exists(normalized)) { - RCLCPP_ERROR(logger, "ControlRaspi not found: %s", normalized.string().c_str()); + RCLCPP_ERROR(logger, "Failed to resolve arussim executable directory"); return std::string{}; } - if (access(normalized.c_str(), X_OK) != 0) + std::error_code error; + for (fs::path dir = fs::path(exe_dir); !dir.empty(); dir = dir.parent_path()) { - RCLCPP_ERROR(logger, "ControlRaspi not executable: %s", normalized.string().c_str()); - return std::string{}; + fs::path candidate = dir / kControlRaspFromAncestor; + if (fs::exists(candidate, error) && !error) + { + if (access(candidate.c_str(), X_OK) != 0) + { + RCLCPP_ERROR(logger, "ControlRaspi found but not executable: %s", candidate.string().c_str()); + return std::string{}; + } + return candidate.string(); + } + + if (dir == dir.parent_path()) + break; } - return normalized.string(); + RCLCPP_ERROR(logger, "ControlRaspi not found: searched for '%s' in every ancestor of %s", + kControlRaspFromAncestor, exe_dir.c_str()); + return std::string{}; } /** * @brief Resolve the working directory for Control-RaspPi * - * This function assumes that the working directory is the parent directory of the Control-RaspPi executable. - * It checks if the directory exists and contains a parameters.yaml file to validate it. + * The working directory must be the Control-RaspPi repo root (parent of build/), + * since the executable loads its parameters internally from ./include/ART-Parameters/. */ std::string resolve_control_rasp_working_dir(const std::string &control_rasp_path, rclcpp::Logger logger) const { @@ -240,13 +247,6 @@ namespace control_raspi return std::string{}; } - fs::path parameters_yaml = repo_dir / "config" / "parameters.yaml"; - if (!fs::exists(parameters_yaml)) - { - RCLCPP_ERROR(logger, "parameters.yaml not found at: %s", parameters_yaml.string().c_str()); - return std::string{}; - } - return repo_dir.string(); } @@ -254,43 +254,4 @@ namespace control_raspi std::atomic stopping_{false}; }; -} // namespace control_raspi - -/*#pragma once - -#include -#include - -#include - -#include -#include -#include - -namespace control_raspi -{ - - class ControlRaspi : public rclcpp::Node - { - public: - explicit ControlRaspi(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - private: - void launch_callback(const std_msgs::msg::Bool::SharedPtr msg); - void reset_callback(const std_msgs::msg::Bool::SharedPtr msg); - - void start_control_rasp(); - void stop_control_rasp(); - - std::string resolve_control_rasp_path() const; - std::string resolve_control_rasp_working_dir(const std::string &control_rasp_path) const; - - rclcpp::Subscription::SharedPtr launch_raspi_; - rclcpp::Subscription::SharedPtr reset_raspi_; - - std::optional control_rasp_pid_; - std::atomic stopping_{false}; - }; - -} // namespace control_raspi -*/ \ No newline at end of file +} // namespace control_raspi \ No newline at end of file diff --git a/src/arussim/include/arussim/sensors.hpp b/src/arussim/include/arussim/sensors.hpp index 3877494c..f76b46c6 100644 --- a/src/arussim/include/arussim/sensors.hpp +++ b/src/arussim/include/arussim/sensors.hpp @@ -14,9 +14,11 @@ #include #include "arussim_msgs/msg/four_wheel_drive.hpp" #include "std_msgs/msg/float32.hpp" +#include #include #include #include +#include #include #include #include diff --git a/src/arussim/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index b526277f..6f5a1f1c 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -23,8 +23,7 @@ def generate_launch_description(): ), create_node(pkg='arussim', exec='arussim_exec', - config='simulator_config.yaml', - params=[{'vehicle.torque_vectoring': True}]), + config='simulator_config.yaml'), create_node(pkg='arussim', exec='sensors_exec', config='sensors_config.yaml'), diff --git a/src/arussim/src/arussim_node.cpp b/src/arussim/src/arussim_node.cpp index ad75cdc7..ca20d7e3 100644 --- a/src/arussim/src/arussim_node.cpp +++ b/src/arussim/src/arussim_node.cpp @@ -238,6 +238,10 @@ Simulator::Simulator() : Node("simulator") RCLCPP_ERROR(this->get_logger(), "Failed loading car parameters: %s", e.what()); } + // Needed in every mode: raspi_sim/hil use car_parameters_.gear_ratio to convert + // the CAN torque command (motor side) to wheel torque + this->load_control_parameters(); + if (kSimulationMode == "default") { this->load_control_parameters(); @@ -251,6 +255,9 @@ Simulator::Simulator() : Node("simulator") else if(kSimulationMode == "raspi_sim") { RCLCPP_WARN(this->get_logger(), "Raspi control simulation enabled."); + raspi_sim_timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&Simulator::on_raspi_sim_timer, this)); } else if(kSimulationMode == "hil") { @@ -719,6 +726,43 @@ void Simulator::on_controller_sim_timer() write(can_socket_0_, &frame, sizeof(struct can_frame)); } +/** + * @brief Timer callback for raspi_sim mode. + * + * While the simulation is launched (as_status_ == 0x03), periodically sends on can0 + * the frames Control-RaspPi needs to command torque: + * - 0x160: data[2] != 0 -> dv_.autonomous = 1 + * - 0x161: data[0] == 0x03 -> dv_.driving = 1 + * - 0x221: data[0] -> enable_flag_ (R2D), required to start the inverters enable sequence + */ +void Simulator::on_raspi_sim_timer() +{ + if (as_status_ != 0x03) + return; + + struct can_frame frame; + std::memset(&frame, 0, sizeof(frame)); + + // Autonomous mode + frame.can_id = 0x160; + frame.can_dlc = 3; + frame.data[2] = 0x01; + write(can_socket_0_, &frame, sizeof(struct can_frame)); + + // AS status: driving + std::memset(frame.data, 0, sizeof(frame.data)); + frame.can_id = 0x161; + frame.can_dlc = 1; + frame.data[0] = 0x03; + write(can_socket_0_, &frame, sizeof(struct can_frame)); + + // R2D enable flag + frame.can_id = 0x221; + frame.can_dlc = 1; + frame.data[0] = 0x01; + write(can_socket_0_, &frame, sizeof(struct can_frame)); +} + /** * @brief Fast timer callback for state updates. * @@ -929,6 +973,47 @@ void Simulator::receive_can_1() continue; } + // Setpoints from Control-RaspPi: enable sequence and torque command per motor + if (frame_1_.can_id == 0x200 || frame_1_.can_id == 0x203 || + frame_1_.can_id == 0x206 || frame_1_.can_id == 0x209) + { + int idx = (frame_1_.can_id - 0x200) / 3; // 0,1,2,3 + + int16_t torque_scaled = static_cast((frame_1_.data[3] << 8) | frame_1_.data[2]); + can_torque_cmd_.at(idx) = torque_scaled * 9.8 / 1000.0 * car_parameters_.gear_ratio; + + // DEBUG: confirm torque reception and scaling (1 Hz) + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "[CAN1 rx] torque motor %d: raw=%d -> %.2f Nm rueda (gear_ratio=%.2f) | cmd=[%.2f %.2f %.2f %.2f]", + idx, torque_scaled, can_torque_cmd_.at(idx), car_parameters_.gear_ratio, + can_torque_cmd_[0], can_torque_cmd_[1], can_torque_cmd_[2], can_torque_cmd_[3]); + + if (kSimulationMode == "raspi_sim") + { + if (!amk_enabled_.at(idx)) + { + amk_enabled_.at(idx) = true; + RCLCPP_INFO(this->get_logger(), + "Enable sequence detected for motor %d, replying inverter enabled", idx); + } + + // Emulate inverter status: (data[1] & 0x62) == 0x60 -> enable_amk_[idx] = 1 + static constexpr uint32_t kAmkStatusIds[4] = {0x100, 0x104, 0x108, 0x112}; + struct can_frame status_frame; + std::memset(&status_frame, 0, sizeof(status_frame)); + status_frame.can_id = kAmkStatusIds[idx]; + status_frame.can_dlc = 8; + status_frame.data[1] = 0x60; + if (idx == 0) + { + // 0x100 also carries the DC bus voltage in data[6..7] + int16_t dc_voltage = static_cast(sensors_.battery_voltage); + status_frame.data[6] = dc_voltage & 0xFF; + status_frame.data[7] = (dc_voltage >> 8) & 0xFF; + } + write(can_socket_1_, &status_frame, sizeof(struct can_frame)); + } + } } } @@ -945,13 +1030,6 @@ void Simulator::receive_can_2() continue; } - 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 * car_parameters_.gear_ratio; - } } } @@ -1141,6 +1219,7 @@ void Simulator::reset_callback([[maybe_unused]] const std_msgs::msg::Bool::Share can_delta_ = 0.0; can_target_r_ = 0.0; can_torque_cmd_ = {0.0, 0.0, 0.0, 0.0}; + amk_enabled_ = {false, false, false, false}; as_status_ = 0x02; vehicle_dynamics_ = VehicleDynamics(); diff --git a/src/arussim/src/sensors.cpp b/src/arussim/src/sensors.cpp index 4c289384..4456c6ba 100644 --- a/src/arussim/src/sensors.cpp +++ b/src/arussim/src/sensors.cpp @@ -175,14 +175,23 @@ Sensors::Sensors() : Node("sensors") {"GSS/yaw_rate", {32, 47, true, 0.02*M_PI/180, 0.0}} }, Sensors::CanBus::kCan1 }, - {0x1B0, 4, { // IMU accelerations + {0x1B0, 6, { // IMU accelerations (Control-RaspPi reads it on can2) {"IMU/ax", {0, 15, true, 0.01, 0.0}}, {"IMU/ay", {16, 31, true, 0.01, 0.0}}, - }, Sensors::CanBus::kCan1 + }, Sensors::CanBus::kCan2 }, - {0x1B1, 6, { // IMU rates - {"IMU/yaw_rate", {32, 47, true, 1.0, 0.0}} - }, Sensors::CanBus::kCan1 + {0x1B1, 6, { // IMU rates (Control-RaspPi decodes rad/s with factor 0.001) + {"IMU/yaw_rate", {32, 47, true, 0.001, 0.0}} + }, Sensors::CanBus::kCan2 + }, + {0x540, 8, { // Doppla GSS speeds (Control-RaspPi reads it on can2, m/s x0.01) + {"Doppla/vx", {32, 47, true, 0.01, 0.0}}, + {"Doppla/vy", {48, 63, true, 0.01, 0.0}} + }, Sensors::CanBus::kCan2 + }, + {0x54A, 1, { // Doppla GSS status (bit 0 of data0 = 1 -> GSS OK) + {"Doppla/status", {0, 7, false, 1.0, 0.0}} + }, Sensors::CanBus::kCan2 }, {0x134, 2, { // Extensometer {"extensometer", {0, 15, true, 0.00007332, -0.5228}} @@ -195,25 +204,43 @@ Sensors::Sensors() : Node("sensors") {"rr_inv_speed", {96, 127, true, 0.01, 0.0}}, // Rear Right inverter }, Sensors::CanBus::kCan2 }, + // AMK inverters actual values: motor speed (raw x0.0001 = rpm) and + // motor torque (raw x0.0098 = Nm). Control-RaspPi reads them on can1. + {0x102, 8, { // Motor 0: Front Left + {"motor_speed", {0, 31, true, 0.0001 * 2 * M_PI / 60, 0.0}}, + {"motor_torque", {32, 47, true, 0.1 * 9.8 / 100, 0.0}} + }, Sensors::CanBus::kCan1 + }, + {0x106, 8, { // Motor 1: Front Right + {"motor_speed", {0, 31, true, 0.0001 * 2 * M_PI / 60, 0.0}}, + {"motor_torque", {32, 47, true, 0.1 * 9.8 / 100, 0.0}} + }, Sensors::CanBus::kCan1 + }, + {0x110, 8, { // Motor 2: Rear Left + {"motor_speed", {0, 31, true, 0.0001 * 2 * M_PI / 60, 0.0}}, + {"motor_torque", {32, 47, true, 0.1 * 9.8 / 100, 0.0}} + }, Sensors::CanBus::kCan1 + }, + {0x114, 8, { // Motor 3: Rear Right + {"motor_speed", {0, 31, true, 0.0001 * 2 * M_PI / 60, 0.0}}, + {"motor_torque", {32, 47, true, 0.1 * 9.8 / 100, 0.0}} + }, Sensors::CanBus::kCan1 + }, {0x100, 2, { {"enable_amk_status_byte1", {8, 15, false, 1.0, 0.0}} - }, Sensors::CanBus::kCan2 + }, Sensors::CanBus::kCan1 }, {0x104, 2, { {"enable_amk_status_byte1", {8, 15, false, 1.0, 0.0}} - }, Sensors::CanBus::kCan2 + }, Sensors::CanBus::kCan1 }, {0x108, 2, { {"enable_amk_status_byte1", {8, 15, false, 1.0, 0.0}} - }, Sensors::CanBus::kCan2 + }, Sensors::CanBus::kCan1 }, {0x112, 2, { {"enable_amk_status_byte1", {8, 15, false, 1.0, 0.0}} - }, Sensors::CanBus::kCan2 - }, - {0x161, 2, { - {"dv_driving", {0, 7, false, 1.0, 0.0}}, - }, Sensors::CanBus::kCan0 + }, Sensors::CanBus::kCan1 }, {0x221, 1, { {"enable_flag", {0, 7, false, 1.0, 0.0}} @@ -401,17 +428,30 @@ void Sensors::inverter_timer() //Send Inverter CAN frames std::map values = { - {"fl_inv_speed", motor_speed_msg.front_left}, - {"fr_inv_speed", motor_speed_msg.front_right}, - {"rl_inv_speed", motor_speed_msg.rear_left}, + {"fl_inv_speed", motor_speed_msg.front_left}, + {"fr_inv_speed", motor_speed_msg.front_right}, + {"rl_inv_speed", motor_speed_msg.rear_left}, {"rr_inv_speed", motor_speed_msg.rear_right}, {"enable_amk_status_byte1", 96.0} - }; + }; - for (auto &frame : frames) { - if (frame.id == 0x123) { + // AMK actual values per motor (0 = FL, 1 = FR, 2 = RL, 3 = RR), read by Control-RaspPi on can1 + const std::array>, 4> amk_values = {{ + {0x102, {{"motor_speed", motor_speed_msg.front_left}, {"motor_torque", torque_msg.front_left}}}, + {0x106, {{"motor_speed", motor_speed_msg.front_right}, {"motor_torque", torque_msg.front_right}}}, + {0x110, {{"motor_speed", motor_speed_msg.rear_left}, {"motor_torque", torque_msg.rear_left}}}, + {0x114, {{"motor_speed", motor_speed_msg.rear_right}, {"motor_torque", torque_msg.rear_right}}} + }}; + + for (auto &frame : frames) { + if (frame.id == 0x123) { send_can_frame(frame, values); } + for (const auto &amk : amk_values) { + if (frame.id == amk.first) { + send_can_frame(frame, amk.second); + } + } } } @@ -436,15 +476,18 @@ void Sensors::groundspeed_timer() gss_vx_pub_->publish(msg_vx); gss_vy_pub_->publish(msg_vy); - std::map values = { {"GSS/ax", ax_ + dist_ax(gen)}, - {"GSS/ay", ay_ + dist_ay(gen)}, {"GSS/yaw_rate", r_ + dist_r(gen)}, - {"GSS/vx", vx_ + dist_vx(gen)}, {"GSS/vy", vy_ + dist_vy(gen)} }; + std::map values = { {"GSS/ax", ax_ + dist_ax(gen)}, + {"GSS/ay", ay_ + dist_ay(gen)}, {"GSS/yaw_rate", r_ + dist_r(gen)}, + {"GSS/vx", vx_ + dist_vx(gen)}, {"GSS/vy", vy_ + dist_vy(gen)}, + {"Doppla/vx", vx_ + dist_vx(gen)}, {"Doppla/vy", vy_ + dist_vy(gen)}, + {"Doppla/status", 1.0} }; + + for (auto &frame : frames) { + if (frame.id == 0x1A3 || frame.id == 0x1A4 || frame.id == 0x1A0 || + frame.id == 0x540 || frame.id == 0x54A) { + send_can_frame(frame, values); + } - for (auto &frame : frames) { - if (frame.id == 0x1A3 || frame.id == 0x1A4 || frame.id == 0x1A0) { - send_can_frame(frame, values); - } - } } @@ -499,12 +542,13 @@ void Sensors::bms_timer() { void Sensors::as_timer() { - std::map values = { {"dv_driving", as_status_}, {"enable_flag", 1.0} }; + // 0x161 (dv_driving) is sent by the arussim node in raspi_sim mode + std::map values = { {"enable_flag", 1.0} }; - for (auto &frame : frames) { - if (frame.id == 0x161 || frame.id == 0x221) { - send_can_frame(frame, values); - } + for (auto &frame : frames) { + if (frame.id == 0x221) { + send_can_frame(frame, values); + } } }