コメント追記

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

@@ -96,14 +96,18 @@
 
       <node pkg="path_to_trajectory" exec="csv_to_trajectory_node" name="csv_to_trajectory" output="screen">
         <param name="csv_file_path" value="$(find-pkg-share booars_launch)/map/csv/traj_mincurv_3.0m.csv"/>
-        <param name="velocity_rate" value="1.0"/>
+        <!-- trajectoryの速度設定 -->
+        <param name="velocity" value="30.0"/>
+        <!-- 車両前方のtrajectoryの長さ -->
         <param name="trajectory_length" value="150.0"/>
-        <param name="trajectory_margin" value="2.0"/>
+        <!-- 車両後方のtrajectoryの長さ -->
         <param name="trajectory_rear_length" value="20.0"/>
+        <!-- trajectoryの間隔 -->
+        <param name="trajectory_margin" value="2.0"/>
+        <!-- pose.position.zの値 -->
         <param name="z_position" value="0.0"/>
         <remap from="output" to="/planning/scenario_planning/trajectory"/>
         <remap from="in_odom" to="/localization/kinematic_state"/>
-        <remap from="now_waypoint" to="/now_waypoint"/>
       </node>
 
     </group>

aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/csv_to_trajectory.hpp

@@ -42,7 +42,7 @@ class CsvToTrajectory : public rclcpp::Node {
   rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_now_point_;
   std::vector<TrajectoryPoint> trajectory_points_;
   size_t current_point_index_ = 0;
-  float velocity_rate_ = 1.0f;
+  float velocity_ = 1.0f;
   float trajectory_length_ = 200.0f;
   float trajectory_margin_ = 2.0f;
   float trajectory_rear_length_ = 30.0f;

aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/csv_to_trajectory.cpp

@@ -22,15 +22,15 @@ CsvToTrajectory::CsvToTrajectory() : Node("csv_to_trajectory_node")
  {
   using std::placeholders::_1;
   this->declare_parameter<std::string>("csv_file_path", "");
-  this->declare_parameter<float>("velocity_rate", 1.0f);
+  this->declare_parameter<float>("velocity", 30.0f);
   this->declare_parameter<float>("trajectory_length", 100.0f);
   this->declare_parameter<float>("trajectory_margin", 2.0f);
   this->declare_parameter<float>("trajectory_rear_length", 10.0f);
   this->declare_parameter<float>("z_position", 0.0f);
 
   std::string csv_file_path;
   this->get_parameter("csv_file_path", csv_file_path);
-  this->get_parameter("velocity_rate", this->velocity_rate_);
+  this->get_parameter("velocity", this->velocity_);
   this->get_parameter("trajectory_length", this->trajectory_length_);
   this->get_parameter("trajectory_margin", this->trajectory_margin_);
   this->get_parameter("trajectory_rear_length", this->trajectory_rear_length_);
@@ -74,7 +74,7 @@ void CsvToTrajectory::readCsv(const std::string& file_path) {
     point.pose.orientation.y = 0.0;
     point.pose.orientation.z = sin(yaw / 2);
     point.pose.orientation.w = cos(yaw / 2);
-    point.longitudinal_velocity_mps = 30.0;//values[5] * this->velocity_rate_;
+    point.longitudinal_velocity_mps = velocity_;//values[5] * this->velocity_;
     point.acceleration_mps2 = 0.0; //values[6];
 
     trajectory_points_.push_back(point);
@@ -108,23 +108,23 @@ void CsvToTrajectory::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom
 
   if (kdtree_.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
   {
-      std::cout << "The closest point index: " << pointIdxNKNSearch[0] << std::endl;
-      std::cout << "Distance: " << pointNKNSquaredDistance[0] << std::endl;
+      // std::cout << "The closest point index: " << pointIdxNKNSearch[0] << std::endl;
+      // std::cout << "Distance: " << pointNKNSquaredDistance[0] << std::endl;
   }
   else
   {
-      std::cout << "No neighbors found!" << std::endl;
+      // std::cout << "No neighbors found!" << std::endl;
       return;
   }
-  const int start_index = pointIdxNKNSearch[0]- std::round(trajectory_rear_length_/trajectory_margin_);
-  const int points_num = std::round(trajectory_length_/trajectory_margin_)+std::round(trajectory_rear_length_/trajectory_margin_);
+  int start_index = pointIdxNKNSearch[0]- std::round(trajectory_rear_length_/trajectory_margin_);
+  if(start_index<0){
+    start_index+=trajectory_points_.size();
+  }
+  const int points_num = std::round((trajectory_length_+trajectory_rear_length_)/trajectory_margin_);
   for(int i = 0; i < points_num; i++){
     int p = start_index + i;
     if(p>=trajectory_points_.size()){
-      p-=trajectory_points_.size();
-    }
-    else if(p<0){
-      p+=trajectory_points_.size();
+      p-=(trajectory_points_.size());
     }
     trajectory.points.push_back(trajectory_points_[p]);
   }