Temp/simulation

config/planning/pacsim.yaml

@@ -16,10 +16,10 @@ planning:
   smoothing_spline_coeffs_ratio: 3.0
   smoothing_spline_precision: 10 
   publishing_visualization_msg: true
-  pre_defined_velocity_planning: 4.0
+  pre_defined_velocity_planning: 2.0
   use_outlier_removal: false
   use_path_smoothing: true
   minimum_velocity: 2.0
   braking_acceleration: -1.0
   normal_acceleration: 4.0
-  use_velocity_planning: true
+  use_velocity_planning: false

src/control/src/adapter_control/pacsim.cpp

@@ -25,8 +25,8 @@ PacSimAdapter::PacSimAdapter(const ControlParameters& params)
     car_velocity_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
         "/pacsim/velocity", 10, [this](const geometry_msgs::msg::TwistWithCovarianceStamped& msg) {
           velocity_ = std::sqrt(std::pow(msg.twist.twist.linear.x, 2) +
-                                std::pow(msg.twist.twist.linear.y, 2) +
-                                std::pow(msg.twist.twist.linear.z, 2));
+                                            std::pow(msg.twist.twist.linear.y, 2) +
+                                            std::pow(msg.twist.twist.linear.z, 2));
         });
   }
 
@@ -50,7 +50,8 @@ void PacSimAdapter::timer_callback() {
     pose.x = t.transform.translation.x;
     pose.y = t.transform.translation.y;
 
-    RCLCPP_DEBUG(get_logger(), "Pose info. Position:%f, %f, Theta %f", pose.x, pose.y, pose.theta);
+    RCLCPP_DEBUG(get_logger(), "Pose info. Position:%f, %f, Theta %f", pose.x,
+                 pose.y, pose.theta);
     this->publish_control(pose);
   }
 }

src/control/src/node_/node_control.cpp

@@ -102,6 +102,8 @@ Control::Control(const ControlParameters& params)
 
 // This function is called when a new pose is received
 void Control::publish_control(const custom_interfaces::msg::Pose& vehicle_state_msg) {
+
+  RCLCPP_INFO(rclcpp::get_logger("control"), "Velocity: %f", this->velocity_);
   if (!go_signal_) {
     RCLCPP_INFO(rclcpp::get_logger("control"), "Go Signal Not received");
 
@@ -147,8 +149,7 @@ void Control::publish_control(const custom_interfaces::msg::Pose& vehicle_state_
   }
 
   // calculate longitudinal control: PI-D
-  double torque = this->long_controller_.update(closest_point_velocity,
-                                                this->point_solver_.vehicle_pose_.velocity_);
+  double torque = this->long_controller_.update(lookahead_velocity, this->velocity_);
 
   // calculate Lateral Control: Pure Pursuit
   double steering_angle = this->lat_controller_.pp_steering_control_law(

src/launcher/launch/slam_pacsim.launch.py

@@ -25,10 +25,20 @@ def generate_launch_description():
             )
         ),
     )
+
+    control_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("control"), "launch", "control.launch.py"]
+            )
+        ),
+    )
+
     return LaunchDescription(
         [
             slam_launch_description,
             pacsim_launch_description,
             evaluator_launch_description,
+            control_launch_description
         ],
     )

src/launcher/launch/state_est_pacsim.launch.py

@@ -36,11 +36,29 @@ def generate_launch_description():
             )
         ),
     )
+
+    planning_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("planning"), "launch", "planning.launch.py"]
+            )
+        ),
+    )
+    control_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("control"), "launch", "control.launch.py"]
+            )
+    ),)
+
+
     return LaunchDescription(
         [
             slam_launch_description,
             ve_launch_description,
             pacsim_launch_description,
             evaluator_launch_description,
+            planning_launch_description,
+            control_launch_description,
         ],
     )

src/pacsim/config/vehicleModel.yaml

@@ -18,8 +18,8 @@ vehicle_model:
     Izz: 101.082
     wheelRadius: 0.203
     gearRatio: 4.0
-    innerSteeringRatio: 0.1869
-    outerSteeringRatio: 0.1582
+    innerSteeringRatio: 0.255625
+    outerSteeringRatio: 0.20375
     nominalVoltageTS: 600.0
     powerGroundForce: 0.0
     # powertrainEfficiency: 0.95

src/pacsim/include/VehicleModel/VehicleModelBicycle.hpp

@@ -56,7 +56,7 @@ class VehicleModelBicycle : public IVehicleModel {
     void setOrientation(Eigen::Vector3d orientation) override;
 
     // Simulation methods
-    void forwardIntegrate(double dt) override;
+    void forwardIntegrate(double dt, Wheels frictionCoefficients);
     std::array<Eigen::Vector3d, 4> getWheelPositions() override;
 
 private:
@@ -217,7 +217,7 @@ class VehicleModelBicycle : public IVehicleModel {
         const Eigen::Vector3d& vRL, const Eigen::Vector3d& vRR);
 
     // Physics calculation methods
-    Eigen::Vector3d getDynamicStates(double dt);
+    Eigen::Vector3d getDynamicStates(double dt, Wheels frictionCoefficients);
     void calculateNormalForces(double& Fz_Front, double& Fz_Rear) const;
     void calculateSlipAngles(double& kappaFront, double& kappaRear) const;
     void updateWheelSpeeds(double dt);

src/pacsim/include/VehicleModel/VehicleModelInterface.hpp

@@ -43,6 +43,6 @@ class IVehicleModel {
     virtual void setOrientation(Eigen::Vector3d orientation) = 0;
 
     // Simulation methods
-    virtual void forwardIntegrate(double dt) = 0;
+    virtual void forwardIntegrate(double dt, Wheels frictionCoefficients) = 0;
     virtual std::array<Eigen::Vector3d, 4> getWheelPositions() = 0;
 };