Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions src/arussim/include/arussim/arussim_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <thread>
#include <queue>
#include <chrono>
#include <array>

#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include "arussim/csv_generator.hpp"
Expand Down Expand Up @@ -182,6 +183,7 @@ class Simulator : public rclcpp::Node

//raspi_sim
control_raspi::ControlRaspi control_raspi_manager_;
std::array<bool, 4> amk_enabled_ = {false, false, false, false};

visualization_msgs::msg::Marker marker_;
pcl::PointCloud<ConeXYZColorScore> track_;
Expand Down Expand Up @@ -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.
*
Expand Down Expand Up @@ -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<arussim_msgs::msg::Cmd>::SharedPtr cmd_sub_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr ebs_sub_;
Expand Down
99 changes: 30 additions & 69 deletions src/arussim/include/arussim/control_raspi.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
{
Expand All @@ -240,57 +247,11 @@ 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();
}

std::optional<pid_t> control_rasp_pid_;
std::atomic<bool> stopping_{false};
};

} // namespace control_raspi

/*#pragma once

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>

#include <sys/types.h>

#include <atomic>
#include <optional>
#include <string>

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<std_msgs::msg::Bool>::SharedPtr launch_raspi_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr reset_raspi_;

std::optional<pid_t> control_rasp_pid_;
std::atomic<bool> stopping_{false};
};

} // namespace control_raspi
*/
} // namespace control_raspi
2 changes: 2 additions & 0 deletions src/arussim/include/arussim/sensors.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,11 @@
#include <sensor_msgs/msg/imu.hpp>
#include "arussim_msgs/msg/four_wheel_drive.hpp"
#include "std_msgs/msg/float32.hpp"
#include <array>
#include <map>
#include <random>
#include <string>
#include <utility>
#include <vector>
#include <linux/can.h>
#include <linux/can/raw.h>
Expand Down
3 changes: 1 addition & 2 deletions src/arussim/launch/arussim_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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'),
Expand Down
93 changes: 86 additions & 7 deletions src/arussim/src/arussim_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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")
{
Expand Down Expand Up @@ -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.
*
Expand Down Expand Up @@ -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<int16_t>((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<int16_t>(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));
}
}
}
}

Expand All @@ -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<int16_t>((frame_0_.data[3] << 8) | frame_0_.data[2]);
can_torque_cmd_.at(idx) = torque_scaled * 9.8 / 1000.0 * car_parameters_.gear_ratio;
}
}
}

Expand Down Expand Up @@ -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();
Expand Down
Loading
Loading