adjust param

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml

@@ -13,7 +13,7 @@
         type: "cached_lanelet"
         map_topic: "/map/vector_map"
         costmap_topic: "~/debug/cached_costmap"
-        inflation_radius: 1.8 # [m]
+        inflation_radius: 1.6 # [m]
         cached_costmap:
           min_x: 89607.0  # [m]
           max_x: 89687.0  # [m]

aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml

@@ -2,8 +2,8 @@
   ros__parameters:
       # mppi
       horizon : 25
-      num_samples : 2500
-      u_min : [-4.0, -0.25] # accel(m/s2), steer angle(rad)
+      num_samples : 3000
+      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 
@@ -13,9 +13,9 @@
       lookahead_distance : 0.1
       reference_path_interval : 0.83
       # cost weights
-      Qc : 20.0
-      Ql : 5.0
-      Qv : 0.5
+      Qc : 10.0
+      Ql : 1.0
+      Qv : 20.0
       Qo : 1000.0
       Qin : 0.01
       Qdin : 0.0

aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml

@@ -7,19 +7,21 @@
   <group>
     <push-ros-namespace namespace="control" />
 
-    <let name="steering_tire_angle_gain_var" value="1.0" if="$(var simulation)" />
-    <let name="steering_tire_angle_gain_var" value="1.639" unless="$(var simulation)" />
+
+    <let name="steering_tire_angle_gain_var" value="1.1" if="$(var simulation)" />
+    <!-- <let name="steering_tire_angle_gain_var" value="1.0" if="$(var simulation)" /> -->
+    <!-- <let name="steering_tire_angle_gain_var" value="1.639" unless="$(var simulation)" /> -->
 
     <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node"
       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="4.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_deceleration_vel" value="4.0" />
+      <param name="curve_param_max_steer_angle" value="0.25" />
+      <param name="curve_param_deceleration_vel" value="3.0" />
       <remap from="input/kinematics" to="/localization/kinematic_state" />
       <!-- global path or mppi-->
       <remap from="input/trajectory" to="/planning/output/mppi_planned_path" />

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);

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);
@@ -112,14 +113,14 @@ void SimplePurePursuit::onTimer()
       marker_msg.action = visualization_msgs::msg::Marker::ADD;
       marker_msg.pose.position.x = lookahead_point_x;
       marker_msg.pose.position.y = lookahead_point_y;
-      marker_msg.pose.position.z = 80;
+      marker_msg.pose.position.z = 0.0;
       marker_msg.pose.orientation.x = 0.0;
       marker_msg.pose.orientation.y = 0.0;
       marker_msg.pose.orientation.z = 0.0;
       marker_msg.pose.orientation.w = 1.0;
-      marker_msg.scale.x = 3.0;
-      marker_msg.scale.y = 3.0;
-      marker_msg.scale.z = 3.0;
+      marker_msg.scale.x = 1.5;
+      marker_msg.scale.y = 1.5;
+      marker_msg.scale.z = 1.5;
       marker_msg.color.r = 1.0f;
       marker_msg.color.g = 0.0f;
       marker_msg.color.b = 0.0f;
@@ -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,14 +149,14 @@ 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);
 
   }
 
   pub_cmd_->publish(cmd);
 
   // publish real-machine command
-  cmd.lateral.steering_tire_angle /=  steering_tire_angle_gain_;
+  //cmd.lateral.steering_tire_angle /=  steering_tire_angle_gain_;
   pub_raw_cmd_->publish(cmd);
 }
 

aichallenge/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
@@ -1903,49 +1936,21 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /simple_pure_pursuit_node/debug/markers
+        Value: /control/debug/lookahead_point
       Value: true
-    - Class: rviz_plugins/Trajectory
-      Color Border Vel Max: 8.300000190734863
+    - Class: rviz_default_plugins/Marker
       Enabled: true
-      Name: Trajectory
+      Name: Marker
+      Namespaces:
+        basic_shapes: true
       Topic:
-        Depth: 15
+        Depth: 5
         Durability Policy: Volatile
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /planning/debug/path
+        Value: /control/debug/pursuit_lookahead
       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 +2051,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 14.891090393066406
+      Scale: 19.18048667907715
       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
@@ -2097,7 +2102,7 @@ Window Geometry:
   Hide Right Dock: false
   InitialPoseButtonPanel:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Tool Properties:

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