adjust param

booars · Oct 22, 2024 · 189858a · 189858a
1 parent 2fbf6e3
commit 189858a
Show file tree

Hide file tree

Showing 7 changed files with 193 additions and 195 deletions.
diff --git a/...workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml b/...workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml
@@ -3,7 +3,7 @@
       # mppi
       horizon : 25
       num_samples : 2500
-      u_min : [-4.0, -0.25] # accel(m/s2), steer angle(rad)
+      u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad)
       u_max : [2.0, 0.25]
       sigmas : [2.0, 0.25] # sample range
       lambda : 1.0 
@@ -14,8 +14,8 @@
       reference_path_interval : 0.83
       # cost weights
       Qc : 20.0
-      Ql : 5.0
-      Qv : 0.5
+      Ql : 1.0
+      Qv : 10.0
       Qo : 1000.0
       Qin : 0.01
       Qdin : 0.0

diff --git a/...lenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/...lenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml
@@ -14,11 +14,11 @@
       output="screen" unless="$(var use_stanley)">
       <param name="use_external_target_vel" value="false" />
       <param name="external_target_vel" value="8.3" />
-      <param name="lookahead_gain" value="0.25" />
-      <param name="lookahead_min_distance" value="1.6" />
+      <param name="lookahead_gain" value="0.6" />
+      <param name="lookahead_min_distance" value="2.0" />
       <param name="speed_proportional_gain" value="3.0" />
       <param name="steering_tire_angle_gain" value="$(var steering_tire_angle_gain_var)"/>
-      <param name="curve_param_max_steer_angle" value="0.1" />
+      <param name="curve_param_max_steer_angle" value="0.15" />
       <param name="curve_param_deceleration_vel" value="4.0" />
       <remap from="input/kinematics" to="/localization/kinematic_state" />
       <!-- global path or mppi-->

diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_race_cl.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_race_cl.csv
diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/csv_to_trajectory.cpp b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/csv_to_trajectory.cpp
@@ -74,7 +74,7 @@ void CsvToTrajectory::readCsv(const std::string& file_path) {
     point.pose.position.y = values[2];
     point.pose.position.z = z_position_;
     //const double yaw = std::atan2(point.pose.position.y-old_y, point.pose.position.x-old_x);
-    const double yaw = values[3];
+    const double yaw = values[3] + M_PI/2;
     point.pose.orientation.x = 0.0;
     point.pose.orientation.y = 0.0;
     point.pose.orientation.z = sin(yaw / 2);

diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp
@@ -78,11 +78,12 @@ void SimplePurePursuit::onTimer()
     RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal");
   } else {
     // get closest trajectory point from current position
-    TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx);
+    //TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx);
 
     // calc lateral control
     //// calc lookahead distance
-    double lookahead_distance = lookahead_gain_ * closet_traj_point.longitudinal_velocity_mps + lookahead_min_distance_;
+    // double lookahead_distance = lookahead_gain_ * closet_traj_point.longitudinal_velocity_mps + lookahead_min_distance_;
+    double lookahead_distance = lookahead_gain_ * odometry_->twist.twist.linear.x + lookahead_min_distance_;
     //// calc center coordinate of rear wheel
     double rear_x = odometry_->pose.pose.position.x -
                     wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z);
@@ -140,7 +141,6 @@ void SimplePurePursuit::onTimer()
     // calc longitudinal speed and acceleration
     double target_longitudinal_vel =
       use_external_target_vel_ ? external_target_vel_ : lookahead_point_itr->longitudinal_velocity_mps;
-    double current_longitudinal_vel = odometry_->twist.twist.linear.x;
 
     // ステアリング角度が大きい場合は減速する
     if (std::abs(cmd.lateral.steering_tire_angle) > curve_param_max_steer_angle_) {
@@ -149,7 +149,7 @@ void SimplePurePursuit::onTimer()
 
     cmd.longitudinal.speed = target_longitudinal_vel;
     cmd.longitudinal.acceleration =
-      speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel);
+      speed_proportional_gain_ * (target_longitudinal_vel - odometry_->twist.twist.linear.x);
 
   }
 

diff --git a/...enge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz b/...enge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz
@@ -6,22 +6,14 @@ Panels:
       Expanded:
         - /System1/TF1
         - /System1/Grid1
-        - /Map1
         - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1
         - /Sensing1/GNSS1/PoseWithCovariance1
         - /Sensing1/Odometry1/Shape1
         - /Sensing1/BaseLink1
         - /Sensing1/GNSSLink1
         - /Sensing1/Sensorkit1
-        - /Localization1
         - /Localization1/EKF1/PoseHistory1
-        - /Planning1
-        - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1
-        - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Velocity1
-        - /Planning1/ScenarioPlanning1/LaneDriving1
-        - /Planning1/MPPITrajectory1
-        - /Trajectory1
-        - /Trajectory1/Topic1
+        - /Planning1/MPPITrajectory1/View Path1
       Splitter Ratio: 0.557669460773468
     Tree Height: 385
   - Class: rviz_common/Selection
@@ -705,7 +697,7 @@ Visualization Manager:
         - Class: rviz_common/Group
           Displays:
             - Class: rviz_plugins/Trajectory
-              Color Border Vel Max: 3
+              Color Border Vel Max: 8
               Enabled: true
               Name: ScenarioTrajectory
               Topic:
@@ -722,14 +714,14 @@ Visualization Manager:
                 Offset from BaseLink: 0
                 Rear Overhang: 1.0299999713897705
                 Value: false
-                Vehicle Length: 4.769999980926514
-                Vehicle Width: 1.8300000429153442
+                Vehicle Length: 3
+                Vehicle Width: 1
               View Path:
-                Alpha: 0.9990000128746033
+                Alpha: 1
                 Color: 0; 0; 0
                 Constant Color: false
                 Value: true
-                Width: 0.30000001192092896
+                Width: 1
               View Point:
                 Alpha: 1
                 Color: 0; 60; 255
@@ -1839,6 +1831,47 @@ Visualization Manager:
             Constant Color: false
             Scale: 0.30000001192092896
             Value: true
+        - Class: rviz_plugins/Trajectory
+          Color Border Vel Max: 8.300000190734863
+          Enabled: false
+          Name: Trajectory
+          Topic:
+            Depth: 15
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /planning/debug/path
+          Value: false
+          View Footprint:
+            Alpha: 1
+            Color: 230; 230; 50
+            Offset from BaseLink: 0
+            Rear Overhang: 1.0299999713897705
+            Value: false
+            Vehicle Length: 4.769999980926514
+            Vehicle Width: 1.8300000429153442
+          View Path:
+            Alpha: 1
+            Color: 0; 0; 0
+            Constant Color: false
+            Value: true
+            Width: 1
+          View Point:
+            Alpha: 1
+            Color: 0; 60; 255
+            Offset: 0
+            Radius: 0.10000000149011612
+            Value: false
+          View Text Velocity:
+            Scale: 0.30000001192092896
+            Value: false
+          View Velocity:
+            Alpha: 1
+            Color: 0; 0; 0
+            Constant Color: false
+            Scale: 0.30000001192092896
+            Value: true
       Enabled: true
       Name: Planning
     - Class: rviz_common/Group
@@ -1905,47 +1938,6 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /simple_pure_pursuit_node/debug/markers
       Value: true
-    - Class: rviz_plugins/Trajectory
-      Color Border Vel Max: 8.300000190734863
-      Enabled: true
-      Name: Trajectory
-      Topic:
-        Depth: 15
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /planning/debug/path
-      Value: true
-      View Footprint:
-        Alpha: 1
-        Color: 230; 230; 50
-        Offset from BaseLink: 0
-        Rear Overhang: 1.0299999713897705
-        Value: false
-        Vehicle Length: 4.769999980926514
-        Vehicle Width: 1.8300000429153442
-      View Path:
-        Alpha: 1
-        Color: 0; 0; 0
-        Constant Color: false
-        Value: true
-        Width: 1
-      View Point:
-        Alpha: 1
-        Color: 0; 60; 255
-        Offset: 0
-        Radius: 0.10000000149011612
-        Value: false
-      View Text Velocity:
-        Scale: 0.30000001192092896
-        Value: false
-      View Velocity:
-        Alpha: 1
-        Color: 0; 0; 0
-        Constant Color: false
-        Scale: 0.30000001192092896
-        Value: true
   Enabled: true
   Global Options:
     Background Color: 10; 10; 10
@@ -2046,11 +2038,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 14.891090393066406
+      Scale: 31.983718872070312
       Target Frame: base_link
       Value: TopDownOrtho (rviz_default_plugins)
-      X: 0
-      Y: 0
+      X: -2.4108195304870605
+      Y: -1.57545006275177
     Saved:
       - Class: rviz_default_plugins/ThirdPersonFollower
         Distance: 18

diff --git a/memo b/memo
@@ -0,0 +1,6 @@
+ros2 param set /planning/mppi_controller_node V_MAX 4.0
+ros2 param set /control/simple_pure_pursuit_node curve_param_deceleration_vel 4.0
+ros2 param set /control/simple_pure_pursuit_node curve_param_max_steer_angle 0.2
+ros2 param set /planning/scenario_planning/csv_to_trajectory velocity_coef 1.0
+
+rviz2 -d workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz