diff --git a/doc/getting_started.md b/doc/getting_started.md index 209361f..027c819 100644 --- a/doc/getting_started.md +++ b/doc/getting_started.md @@ -8,7 +8,7 @@ To use PacSim, you need to create a message converter node to match your own int | TOPIC | MESSAGE TYPE | DESCRIPTION | USED WITH DEFAULT MODEL? | |------------------------------|----------------------------|---------------------------------------------------------------------------------------|---| | /pacsim/steering_setpoint | pacsim::msg::StampedScalar | Target steering angle at the steering wheel/sensor (rad) | YES | -| /pacsim/powerground_setpoint | pacsim::msg::StampedScalar | Powered ground multiplier. 0 for no powered aero, 1 for full capacity (Dimensionless) | YES | +| /pacsim/powerground_setpoint | pacsim::msg::StampedScalar | Longitudinal force command (N) | YES | | /pacsim/wheelspeed_setpoints | pacsim::msg::Wheels | Target wheel speeds at each wheel's motor (RPM) | NO | | /pacsim/torques_min | pacsim::msg::Wheels | Lower bound torque value at each wheel's motor (Nm) | NO | | /pacsim/torques_max | pacsim::msg::Wheels | Upper bound torque value at each wheel's motor (Nm) | YES | diff --git a/include/FSSIMVehicleModel.hpp b/include/FSSIMVehicleModel.hpp index a45746e..77ac598 100644 --- a/include/FSSIMVehicleModel.hpp +++ b/include/FSSIMVehicleModel.hpp @@ -163,8 +163,8 @@ class FSSIMVehicleModel { // Integrates dynamics given dc (throttle), delta (steer), and time step dt [s] const State& step(double dc, double delta, double dt) { - // First-order steering actuator model (simple low-pass) - const double T_sample = 0.1; + // First-order steering actuator model (simple low-pass) using actual integration step dt + const double T_sample = std::max(1e-6, dt); const double T_steering = 0.145; input_.delta = (T_sample / (T_steering + T_sample)) * delta + (T_steering / (T_steering + T_sample)) * old_delta_; diff --git a/include/VehicleModel/deadTime.hpp b/include/VehicleModel/deadTime.hpp index cb681d7..3c64153 100644 --- a/include/VehicleModel/deadTime.hpp +++ b/include/VehicleModel/deadTime.hpp @@ -43,7 +43,6 @@ template class DeadTime std::lock_guard l(_mutex); if (this->deadTimeQueue.size() >= 1) { - T elem = this->deadTimeQueue.front(); if (time >= (this->times.front() + this->deadTime)) { ret = true; diff --git a/include/configParser.hpp b/include/configParser.hpp index a92ca2d..abb336d 100644 --- a/include/configParser.hpp +++ b/include/configParser.hpp @@ -43,7 +43,7 @@ class ConfigElement } return this->node[i]; } - template inline T getElement(string elementName) { return this->node[elementName].as(); }; + template inline T getElement(string elementName) { return this->node[elementName].as(); } template inline bool getElement(T* res, string elementName) { *res = this->node[elementName].as(); diff --git a/src/VehicleModel/VehicleModelBicycle.cpp b/src/VehicleModel/VehicleModelBicycle.cpp index 9d3be61..fb31cae 100644 --- a/src/VehicleModel/VehicleModelBicycle.cpp +++ b/src/VehicleModel/VehicleModelBicycle.cpp @@ -13,10 +13,10 @@ class VehicleModelBicycle : public IVehicleModel this->angularVelocity = Eigen::Vector3d(0.0, 0.0, 0.0); this->acceleration = Eigen::Vector3d(0.0, 0.0, 0.0); - this->torques = { 0.0, 0.0, 0.0, 0.0 }; - this->steeringAngles = { 0.0, 0.0, 0.0, 0.0 }; - this->wheelOrientations = { 0.0, 0.0, 0.0, 0.0 }; - this->wheelspeeds = { 0.0, 0.0, 0.0, 0.0 }; + this->torques = { 0.0, 0.0, 0.0, 0.0, 0.0 }; + this->steeringAngles = { 0.0, 0.0, 0.0, 0.0, 0.0 }; + this->wheelOrientations = { 0.0, 0.0, 0.0, 0.0, 0.0 }; + this->wheelspeeds = { 0.0, 0.0, 0.0, 0.0, 0.0 }; } bool readConfig(ConfigElement& config) @@ -114,13 +114,12 @@ class VehicleModelBicycle : public IVehicleModel void setSteeringSetpointFront(double in) { setSteeringFront(in); } - void setSteeringSetpointRear(double in) { return; } + void setSteeringSetpointRear(double /*in*/) { return; } void setPowerGroundSetpoint(double in) { this->powerGroundSetpoint = std::min(std::max(in, 0.0), 1.0); } void setSteeringFront(double in) { - double avgRatio = 0.5 * (this->innerSteeringRatio + this->outerSteeringRatio); if (in > 0) { this->steeringAngles.FL = this->innerSteeringRatio * in; @@ -143,14 +142,10 @@ class VehicleModelBicycle : public IVehicleModel } // ax, ay, rdot - Eigen::Vector3d getDynamicStates(double dt, Wheels frictionCoefficients) + Eigen::Vector3d getDynamicStates(double /*dt*/, Wheels frictionCoefficients) { double l = this->lr + this->lf; double vx = this->velocity.x(); - double vy = this->velocity.y(); - double ax = this->acceleration.x(); - double ay = this->acceleration.y(); - double r = this->angularVelocity.z(); // Downforce double F_aero_downforce = 0.5 * 1.29 * this->aeroArea * this->cla * (vx * vx) + this->powerGroundSetpoint * this->powerGroundForce; @@ -306,9 +301,9 @@ class VehicleModelBicycle : public IVehicleModel double powerGroundForce = 700.0; double powertrainEfficiency = 1.0; - Wheels minTorques = { -0.0, -0.0, -0.0, -0.0 }; - Wheels maxTorques = { 0.0, 0.0, 0.0, 0.0 }; - Wheels rpmSetpoints = { 0.0, 0.0, 0.0, 0.0 }; - Wheels currentFx = { 0.0, 0.0, 0.0, 0.0 }; - Wheels currentFy = { 0.0, 0.0, 0.0, 0.0 }; + Wheels minTorques = { -0.0, -0.0, -0.0, -0.0, 0.0 }; + Wheels maxTorques = { 0.0, 0.0, 0.0, 0.0, 0.0 }; + Wheels rpmSetpoints = { 0.0, 0.0, 0.0, 0.0, 0.0 }; + Wheels currentFx = { 0.0, 0.0, 0.0, 0.0, 0.0 }; + Wheels currentFy = { 0.0, 0.0, 0.0, 0.0, 0.0 }; }; diff --git a/src/VehicleModel/VehicleModelFSSIM.cpp b/src/VehicleModel/VehicleModelFSSIM.cpp index ed29b55..0275f11 100644 --- a/src/VehicleModel/VehicleModelFSSIM.cpp +++ b/src/VehicleModel/VehicleModelFSSIM.cpp @@ -4,15 +4,19 @@ #include #include +namespace { +constexpr double kCmdEps = 1e-6; +} + // Wrapper implementing IVehicleModel using the FSSIMVehicleModel via composition class VehicleModelFSSIM : public IVehicleModel { public: VehicleModelFSSIM() { // Reasonable defaults (kept minimal) - this->torques = {0.0, 0.0, 0.0, 0.0}; - this->steeringAngles = {0.0, 0.0, 0.0, 0.0}; - this->wheelOrientations = {0.0, 0.0, 0.0, 0.0}; - this->wheelspeeds = {0.0, 0.0, 0.0, 0.0}; + this->torques = {0.0, 0.0, 0.0, 0.0, 0.0}; + this->steeringAngles = {0.0, 0.0, 0.0, 0.0, 0.0}; + this->wheelOrientations = {0.0, 0.0, 0.0, 0.0, 0.0}; + this->wheelspeeds = {0.0, 0.0, 0.0, 0.0, 0.0}; wheelRadius = 0.206; gearRatio = 12.0; @@ -80,7 +84,11 @@ class VehicleModelFSSIM : public IVehicleModel { // Setters / inputs void setTorques(Wheels in) override { this->torques = in; } - void setRpmSetpoints(Wheels in) override { this->rpmSetpoints = in; } + void setRpmSetpoints(Wheels in) override { + this->rpmSetpoints = in; + this->hasRpmSetpoint_ = true; + this->rpmCommandAgeS_ = 0.0; + } void setMinTorques(Wheels in) override { this->minTorques = in; } void setMaxTorques(Wheels in) override { this->maxTorques = in; } @@ -93,10 +101,10 @@ class VehicleModelFSSIM : public IVehicleModel { } void setSteeringSetpointRear(double) override { /* not supported */ } void setPowerGroundSetpoint(double in) override { - // Interpret as gravity factor - gravityFactor_ = in; - auto& p = fssim_.params(); - //p.inertia.g = base_g_ * gravityFactor_; + // Interpret command as longitudinal force in Newton from upstream controller. + targetLongitudinalForceN_ = in; + hasLongitudinalForceCommand_ = true; + forceCommandAgeS_ = 0.0; } void setPosition(Eigen::Vector3d position) override { this->position = position; @@ -114,16 +122,37 @@ class VehicleModelFSSIM : public IVehicleModel { void forwardIntegrate(double dt, Wheels /*frictionCoefficients*/) override { // Save previous for finite-difference accelerations + const auto params = fssim_.getParam(); const double total_max_torque = maxTorques.FL + maxTorques.FR + maxTorques.RL + maxTorques.RR; - if (std::abs(total_max_torque) > 1e-6) { - const auto params = fssim_.getParam(); - const double target_force = gearRatio * total_max_torque / std::max(1e-6, wheelRadius); - fssim_dc_ = std::clamp(target_force / std::max(1e-6, params.driveTrain.cm1), -1.0, 1.0); + const bool use_force_command = hasLongitudinalForceCommand_; + + if (use_force_command) { + // Map force command in N to drivetrain command input. + fssim_dc_ = std::clamp( + targetLongitudinalForceN_ / std::max(kCmdEps, params.driveTrain.cm1), -1.0, 1.0); + + // Optional safety: only apply rpm cap when the upstream actually provided rpm setpoints + // (avoids accidental zeroing when rpmSetpoints are still at their default of 0). + if (hasRpmSetpoint_) { + double rpm_avg = 0.25 * (rpmSetpoints.FL + rpmSetpoints.FR + rpmSetpoints.RL + rpmSetpoints.RR); + double omega = rpm_avg * 2.0 * M_PI / 60.0; + double v_target = (omega * wheelRadius) / std::max(kCmdEps, gearRatio); + if (v_target > kCmdEps) { + const double v_current = fssim_.getState().v.x(); + if (v_current > v_target && fssim_dc_ > 0.0) { + fssim_dc_ = 0.0; + } + } + } + } else if (std::abs(total_max_torque) > kCmdEps) { + // Backward compatibility: legacy torque path. + const double target_force = gearRatio * total_max_torque / std::max(kCmdEps, wheelRadius); + fssim_dc_ = std::clamp(target_force / std::max(kCmdEps, params.driveTrain.cm1), -1.0, 1.0); } else { - // If RPM target provided, PI control body vx to compute dc + // Velocity path: use wheel speed setpoint as target body velocity. double rpm_avg = 0.25 * (rpmSetpoints.FL + rpmSetpoints.FR + rpmSetpoints.RL + rpmSetpoints.RR); double omega = rpm_avg * 2.0 * M_PI / 60.0; - double v_target = (omega * wheelRadius) / std::max(1e-6, gearRatio); + double v_target = (omega * wheelRadius) / std::max(kCmdEps, gearRatio); updateVelocityPI(v_target, dt); } @@ -136,7 +165,7 @@ class VehicleModelFSSIM : public IVehicleModel { // Wheelspeeds: approximate from body x velocity and gear ratio double rpm = (s.v.x() / (wheelRadius * 2.0 * M_PI)) * 60.0 * gearRatio; - this->wheelspeeds = {rpm, rpm, rpm, rpm}; + this->wheelspeeds = {rpm, rpm, rpm, rpm, 0.0}; // Wheel orientations integrate trivially from wheelspeeds double dtheta = (rpm / (60.0 * gearRatio)) * dt * 2.0 * M_PI; @@ -144,6 +173,27 @@ class VehicleModelFSSIM : public IVehicleModel { this->wheelOrientations.FR = std::fmod(this->wheelOrientations.FR + dtheta, 2.0 * M_PI); this->wheelOrientations.RL = std::fmod(this->wheelOrientations.RL + dtheta, 2.0 * M_PI); this->wheelOrientations.RR = std::fmod(this->wheelOrientations.RR + dtheta, 2.0 * M_PI); + + // Keep the last force/rpm setpoints sticky so that PacSim ticks faster than the + // upstream controller (e.g. 200 Hz sim vs 40 Hz MPC) do not fall back to the + // velocity-PI brake-to-zero path between commands. Drop the command only after a + // staleness timeout indicating the upstream publisher really stopped. + constexpr double kCommandStaleTimeoutS = 0.2; + if (hasLongitudinalForceCommand_) { + forceCommandAgeS_ += dt; + if (forceCommandAgeS_ > kCommandStaleTimeoutS) { + hasLongitudinalForceCommand_ = false; + targetLongitudinalForceN_ = 0.0; + forceCommandAgeS_ = 0.0; + } + } + if (hasRpmSetpoint_) { + rpmCommandAgeS_ += dt; + if (rpmCommandAgeS_ > kCommandStaleTimeoutS) { + hasRpmSetpoint_ = false; + rpmCommandAgeS_ = 0.0; + } + } } private: @@ -171,9 +221,14 @@ class VehicleModelFSSIM : public IVehicleModel { double nominalVoltageTS{0.0}; double base_g_{9.81}; double gravityFactor_{1.0}; - Wheels minTorques{ -0.0, -0.0, -0.0, -0.0 }; - Wheels maxTorques{ 0.0, 0.0, 0.0, 0.0 }; - Wheels rpmSetpoints{ 0.0, 0.0, 0.0, 0.0 }; + double targetLongitudinalForceN_{0.0}; + bool hasLongitudinalForceCommand_{false}; + bool hasRpmSetpoint_{false}; + double forceCommandAgeS_{0.0}; + double rpmCommandAgeS_{0.0}; + Wheels minTorques{ -0.0, -0.0, -0.0, -0.0, 0.0 }; + Wheels maxTorques{ 0.0, 0.0, 0.0, 0.0, 0.0 }; + Wheels rpmSetpoints{ 0.0, 0.0, 0.0, 0.0, 0.0 }; FSSIMVehicleModel::State last_state_{}; double last_dt_{0.0}; diff --git a/src/pacsim_main.cpp b/src/pacsim_main.cpp index 4d8ccab..43e4ddc 100644 --- a/src/pacsim_main.cpp +++ b/src/pacsim_main.cpp @@ -171,8 +171,6 @@ int threadMainLoopFunc(std::shared_ptr node) deadTimeMinTorques = DeadTime(0.02); deadTimePowerGroundSetpoint = DeadTime(0.05); - double current_wheel_speed_angle = 0.0; - auto nextLoopTime = std::chrono::steady_clock::now(); cl = std::make_shared(logger, lms, mainConfig); @@ -394,7 +392,7 @@ void cbFuncLat(const pacsim::msg::StampedScalar& msg) void cbFuncTorquesInverterMin(const pacsim::msg::Wheels& msg) { std::lock_guard l(mutexSimTime); - Wheels min; + Wheels min { 0.0, 0.0, 0.0, 0.0, simTime }; min.FL = msg.fl; min.FR = msg.fr; min.RL = msg.rl; @@ -405,7 +403,7 @@ void cbFuncTorquesInverterMin(const pacsim::msg::Wheels& msg) void cbFuncTorquesInverterMax(const pacsim::msg::Wheels& msg) { std::lock_guard l(mutexSimTime); - Wheels max; + Wheels max { 0.0, 0.0, 0.0, 0.0, simTime }; max.FL = msg.fl; max.FR = msg.fr; max.RL = msg.rl; @@ -416,7 +414,7 @@ void cbFuncTorquesInverterMax(const pacsim::msg::Wheels& msg) void cbWheelspeeds(const pacsim::msg::Wheels& msg) { std::lock_guard l(mutexSimTime); - Wheels w { msg.fl, msg.fr, msg.rl, msg.rr }; + Wheels w { msg.fl, msg.fr, msg.rl, msg.rr, simTime }; deadTimeWspdSetpoints.addVal(w, simTime); } @@ -426,8 +424,8 @@ void cbPowerGround(const pacsim::msg::StampedScalar& msg) deadTimePowerGroundSetpoint.addVal(msg.value, simTime); } -void cbFinishSignal(const std::shared_ptr request, - std::shared_ptr response) +void cbFinishSignal(const std::shared_ptr /*request*/, + std::shared_ptr /*response*/) { cl->setFinish(true); } diff --git a/src/sensorModels/imuSensor.cpp b/src/sensorModels/imuSensor.cpp index 15f0e38..53bd21e 100644 --- a/src/sensorModels/imuSensor.cpp +++ b/src/sensorModels/imuSensor.cpp @@ -26,7 +26,7 @@ void ImuSensor::readConfig(ConfigElement& config) this->orientation = Eigen::Vector3d(orientation[0], orientation[1], orientation[2]); } -bool ImuSensor::RunTick(ImuData& in, Eigen::Vector3d& alpha, double time) +bool ImuSensor::RunTick(ImuData& in, Eigen::Vector3d& /*alpha*/, double time) { // TODO rigid body tranform of accleration if (this->sampleReady(time)) diff --git a/src/sensorModels/perceptionSensor.cpp b/src/sensorModels/perceptionSensor.cpp index bc650f7..e238f52 100644 --- a/src/sensorModels/perceptionSensor.cpp +++ b/src/sensorModels/perceptionSensor.cpp @@ -203,8 +203,6 @@ std::vector PerceptionSensor::addNoise(std::vector& in) std::vector PerceptionSensor::addClassProbailities(std::vector& in) { std::vector listNew; - bool detect_big_orange = true; - bool detect_timekeeping = true; std::default_random_engine random_generator(this->numFrames); std::uniform_real_distribution unif(0, 1.0); diff --git a/src/sensorModels/wheelsSensor.cpp b/src/sensorModels/wheelsSensor.cpp index cf12fe0..1b2ef0c 100644 --- a/src/sensorModels/wheelsSensor.cpp +++ b/src/sensorModels/wheelsSensor.cpp @@ -15,7 +15,7 @@ void WheelsSensor::readConfig(ConfigElement& config) config["error"].getElement(&this->error_sigma, "sigma"); } -bool WheelsSensor::RunTick(Wheels& in, Eigen::Vector3d& trans, Eigen::Vector3d& rot, double time) +bool WheelsSensor::RunTick(Wheels& in, Eigen::Vector3d& /*trans*/, Eigen::Vector3d& /*rot*/, double time) { if (this->sampleReady(time)) { diff --git a/src/track/gripMap.cpp b/src/track/gripMap.cpp index 0f0fb22..75bd04a 100644 --- a/src/track/gripMap.cpp +++ b/src/track/gripMap.cpp @@ -46,6 +46,7 @@ Wheels gripMap::getGripValues(std::array in) ret.FR = this->total_scale * temp[1]; ret.RL = this->total_scale * temp[2]; ret.RR = this->total_scale * temp[3]; + ret.timestamp = 0.0; return ret; } @@ -55,7 +56,7 @@ void gripMap::transformPoints(Eigen::Vector3d trans, Eigen::Vector3d rot) Eigen::Matrix3d rotationMatrix = eulerAnglesToRotMat(rot); Eigen::Vector3d transInverse = inverseTranslation(trans, rot); - for (int i = 0; i < this->mapPoints.size(); ++i) + for (std::size_t i = 0; i < this->mapPoints.size(); ++i) { this->mapPoints[i].first = rotationMatrix * this->mapPoints[i].first; this->mapPoints[i].first += transInverse;