progressed in Deadzon. Improved the code navigation in dist generator…

… hpp.

simulator/simple_planning_simulator/include/disturbance_generators/disturbance_generators.hpp

@@ -50,7 +50,6 @@ class DelayModelSISO
 
     void printModelTF();
 
-
 private:
     double meanTd_{0.0}; //  !<-@brief current delay that is used to compute a transfer function.
     size_t pade_order_{1}; //!<-@brief order of Pade approximation.
@@ -65,7 +64,7 @@ class DelayModelSISO
 
 };
 
-// -------------------- INTERFACE CLASSES ------------------------------------
+// -------------------- TIME DELAY DISTURBANCE CLASSES ------------------------------------
 /**
  * @class IDisturbanceInterface_TimeDelay
  * @brief Input Disturbance TimeDelay Interface.
@@ -76,7 +75,7 @@ class IDisturbanceInterface_TimeDelay
     // virtual ~disturbanceInterface() = default;
     virtual double getDisturbedInput(double const &input) = 0;
 
-    virtual double getCurrentTimeDelayValue() const = 0;
+    [[nodiscard]] virtual double getCurrentTimeDelayValue() const = 0;
 
     /**
      * @brief returns a pair of time-delayed and nondelayed input pairs.
@@ -94,28 +93,6 @@ class IDisturbanceInterface_TimeDelay
 
 };
 
-/**
- * @brief Input Disturbance Deadzone Interface
- * */
-
-class IDisturbanceInterface_DeadZone
-{
-public:
-    using pair_type = std::pair<std::pair<double, double>, std::pair<double, double>>;
-
-    virtual double getDisturbedInput(double const &input) = 0;
-
-    /**
-     * @brief Deadzone interface.
-     * @return pair_type pair ([ml, bl] , [mr, br])  where m is the slope of the deadzone, and b is x-intercept of
-     * the deadzone boundary
-     * */
-    [[nodiscard]] virtual pair_type getCurrentDeadZoneParameters() const = 0;
-
-};
-
-
-// -------------------- IDENTITY CLASSES ------------------------------------
 /**
  * @brief Identity class that maps the input as is when the time-delay value is zero in the settings.
  * */
@@ -138,30 +115,7 @@ class InputDisturbance_IdentityTimeDelay : public IDisturbanceInterface_TimeDela
 
 };
 
-/**
- * @brief Identity class that maps the input as is when the time-delay value is zero in the settings.
- * */
-class InputDisturbance_DeadZone : public IDisturbanceInterface_DeadZone
-{
-public:
-    double getDisturbedInput(double const &input) override
-    {
-        // ns_utils::print("Identity Input Disturbance is called ...");
-        return input;
-    }
-
-    [[nodiscard]] pair_type getCurrentDeadZoneParameters() const override
-    {
-        // m = 1, b =0 for both left and right parts of a dead-zone nonlinearity graph.
-        auto inactive_deadzone_params = std::make_pair(1.0, 0.0);
-
-        return std::make_pair(inactive_deadzone_params, inactive_deadzone_params);
-    }
-
-};
 
-
-// -------------------- DISTURBANCE CLASSES ------------------------------------
 /**
  * @brief Input disturbance classes.
  *
@@ -213,9 +167,93 @@ class InputDisturbance_TimeDelayPade : public IDisturbanceInterface_TimeDelay
 
 };
 
+// -------------------- DEADZONE DISTURBANCE CLASSES ------------------------------------
+
+/**
+ * @brief Input Disturbance Deadzone Interface
+ * */
+
+class IDisturbanceInterface_DeadZone
+{
+public:
+    using pair_type = std::pair<std::pair<double, double>, std::pair<double, double>>;
+
+    virtual double getDisturbedInput(double const &input) = 0;
+
+    /**
+     * @brief Deadzone interface.
+     * @return pair_type pair ([ml, bl] , [mr, br])  where m is the slope of the deadzone, and b is x-intercept of
+     * the deadzone boundary
+     * */
+    [[nodiscard]] virtual pair_type getCurrentDeadZoneParameters() const = 0;
+
+protected:
+    double current_deadzoned_input_{};
+
+};
+
+
+/**
+ * @brief Identity class that maps the input as is when the time-delay value is zero in the settings.
+ * */
+class InputDisturbance_DeadZoneIdentity : public IDisturbanceInterface_DeadZone
+{
+public:
+    double getDisturbedInput(double const &input) override
+    {
+        // ns_utils::print("Identity Input Disturbance is called ...");
+        current_deadzoned_input_ = input;
+        return current_deadzoned_input_;
+    }
+
+    [[nodiscard]] pair_type getCurrentDeadZoneParameters() const override
+    {
+        // m = 1, b =0 for both left and right parts of a dead-zone nonlinearity graph.
+        auto &&inactive_deadzone_params = std::make_pair(1.0, 0.0);
+
+        return std::make_pair(inactive_deadzone_params, inactive_deadzone_params);
+    }
 
 
-// -------------------- OUTPUT DISTURBANCE - INTERFACE CLASSES ------------------------------------
+};
+
+/**
+ * @brief Identity class that maps the input as is when the time-delay value is zero in the settings.
+ * */
+class InputDisturbance_DeadZone : public IDisturbanceInterface_DeadZone
+{
+public:
+
+    double getDisturbedInput(double const &input) override;
+
+    [[nodiscard]] pair_type getCurrentDeadZoneParameters() const override;
+
+
+private:
+/**
+ *  @brief [m, b]_{lr} are randomized. The equation of linear part (increasing) y = m*x + a*sin(2*pi*x+phi).
+ * */
+    std::mt19937 generator_;
+    std::exponential_distribution<> time_delay_exp_dist_{0.0};
+
+    std::uniform_real_distribution<> sampler_b_;
+    std::uniform_real_distribution<> sampler_m_;
+    std::uniform_real_distribution<> sampler_a_;
+    std::uniform_real_distribution<> sampler_phi_;
+
+    //@brief linear part function variables.
+    double m_line_slope_{1.};
+    double a_sin_mag_{};
+    double phi_phase_{};
+
+    // @brief Deadzone threshold left and right.
+    double bl_threshold_{};
+    double br_threshold_{};
+
+};
+
+
+// -------------------- OUTPUT DISTURBANCE - ROAD SLOPE ------------------------------------
 /**
  * @brief Input disturbance class.
  * */
@@ -233,14 +271,13 @@ class IOutputDisturbance_Interface
     [[nodiscard]] virtual double getCurrentDisturbanceParameterValue() const = 0;
 
 protected:
+
     double current_slope_disturbance_{};  //!<@brief g*sin(theta)
 
 private:
 };
 
 
-// -------------------- OUTPUT DISTURBANCE - IDENTITY CLASSES ------------------------------------
-
 class OutputDisturbance_AdditiveIdentity : public IOutputDisturbance_Interface
 {
 public:
@@ -256,9 +293,7 @@ class OutputDisturbance_AdditiveIdentity : public IOutputDisturbance_Interface
         return current_slope_disturbance_;
     }
 
-private:
 };
-// -------------------- OUTPUT DISTURBANCE  CLASSES ------------------------------------
 
 class OutputDisturbance_SlopeVariation : public IOutputDisturbance_Interface
 {
@@ -309,7 +344,7 @@ struct IDisturbanceCollection
 
 
     // Destructor
-    ~IDisturbanceCollection() = default;
+    // ~IDisturbanceCollection() = default;
 
     // Input disturbances time-varying time-delay.
     std::shared_ptr<IDisturbanceInterface_TimeDelay> steering_inputDisturbance_time_delay_ptr_;

simulator/simple_planning_simulator/src/disturbance_generators/disturbance_generators.cpp

@@ -197,3 +197,13 @@ double OutputDisturbance_SlopeVariation::getDisturbedOutput()
     current_slope_disturbance_ = g_ * sin(current_road_slope_);
     return current_slope_disturbance_;
 }
+
+double InputDisturbance_DeadZone::getDisturbedInput(const double &input)
+{
+    return input;
+}
+
+IDisturbanceInterface_DeadZone::pair_type InputDisturbance_DeadZone::getCurrentDeadZoneParameters() const
+{
+    return IDisturbanceInterface_DeadZone::pair_type();
+}

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -234,7 +234,7 @@ namespace simulation
             const float64_t steer_td_angular_speed = declare_parameter("steer_td_angular_speed", 0.1);
             const size_t steer_td_pade_order = static_cast<size_t>(declare_parameter("steer_td_pade_order", 2));
 
-            double dt = timer_sampling_time_ms_ / 1000.;
+            double dt = static_cast<double>(this->timer_sampling_time_ms_) / 1000.;
 
             // STEERING Time DELAY
             if (std::fabs(steer_time_delay) >= EPS)
@@ -322,7 +322,7 @@ namespace simulation
                 vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL;
                 vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerVel>(
                         vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase,
-                        timer_sampling_time_ms_ / 1000.0,
+                        static_cast<double>(this->timer_sampling_time_ms_) / 1000.0,
                         vel_time_delay, vel_time_constant, steer_time_delay,
                         steer_time_constant);
 
@@ -331,7 +331,7 @@ namespace simulation
                 vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC;
                 vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAcc>(
                         vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase,
-                        timer_sampling_time_ms_ / 1000.0,
+                        static_cast<double>(this->timer_sampling_time_ms_) / 1000.0,
                         acc_time_delay, acc_time_constant, steer_time_delay,
                         steer_time_constant);
 
@@ -353,7 +353,7 @@ namespace simulation
                 vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED;
                 vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAccGeared>(
                         vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase,
-                        timer_sampling_time_ms_ / 1000.0,
+                        static_cast<double>(this->timer_sampling_time_ms_) / 1000.0,
                         acc_time_delay, acc_time_constant, steer_time_delay,
                         steer_time_constant);
             } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED_DIST")
@@ -461,7 +461,7 @@ namespace simulation
             current_disturbance_gen_.road_slope_acc = static_cast<float32_t>(vehicle_model_ptr_->getCurrentRoadSlopeAccDisturbance());
 
             pub_dist_generator_->publish(current_disturbance_gen_);
-            ns_utils::print("Current engage ", current_engage_);
+            // ns_utils::print("Current engage ", current_engage_);
         }
 
         void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg)
@@ -640,9 +640,10 @@ namespace simulation
                     vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_DIST)
             {
                 state << x, y, yaw, vx, steer, accx;
-            }
-            vehicle_model_ptr_->setState(state);
+            } else
+            {}
 
+            vehicle_model_ptr_->setState(state);
             is_initialized_ = true;
         }
 
@@ -789,7 +790,7 @@ namespace simulation
             tf.transform.rotation = odometry.pose.pose.orientation;
 
             tf2_msgs::msg::TFMessage tf_msg{};
-            tf_msg.transforms.emplace_back(std::move(tf));
+            (void) tf_msg.transforms.emplace_back(std::move(tf));
             pub_tf_->publish(tf_msg);
         }
     }  // namespace simple_planning_simulator