waypoint navigationを滞りなく実行したい

前提

ROS melodic (rosversion : 1.14.13) を用いて台車ロボットを動かしています。

実現したいこと

waypoint navigation を用いて設定したポイントを通過しゴールポイントに到達できるようにしたいです。

発生している問題・エラーメッセージ

roslaunch megarover_samples megarover_move_base_dwa.launch を実行した後にrviz上で2D_Pose_Estimateを選択して初期位置を設定してから、navigationを実行するとnavigationが始まるはずですが、最初のwaypointに到達後次のwaypointに移動しません。シェルを見てみると以下のようなエラーが出ていました。

process[map_server-1]: started with pid [8013]
process[cirkit_waypoint_navigator_node-2]: started with pid [8014]
process[cirkit_waypoint_server-3]: started with pid [8015]
[ INFO] [1668746370.305372772]: Loading map from image "/home/agvros/catkin_ws/src/megarover_samples/map/YM5F20221109.pgm"
process[rviz-4]: started with pid [8025]
[ INFO] [1668746370.315080777]: Read a 1184 X 1184 map @ 0.050 m/cell
[ INFO] [1668746370.316589902]: 8waypoints are loaded.
[ INFO] [1668746370.343292089]: [Waypoints file name] : /home/agvros/catkin_ws/src/cirkit_waypoint_manager/cirkit_waypoint_navigator/waypoints/2022-11-14-15-29-51.csv
[ INFO] [1668746370.345981094]: Reading Waypoints.
[ INFO] [1668746370.346134690]: Waiting for action server to start.
[ INFO] [1668746370.518078373]: Next Waypoint : 0
[ INFO] [1668746370.518128996]: Next WayPoint is got
[ INFO] [1668746370.518148232]: Go next_waypoint.
[ERROR] [1668746370.518301655]: "map" passed to lookupTransform argument target_frame does not exist. 
[ERROR] [1668746370.518415125]: "map" passed to lookupTransform argument target_frame does not exist. 
[ERROR] [1668746370.522877975]: "map" passed to lookupTransform argument target_frame does not exist. 
[ERROR] [1668746370.622842646]: Lookup would require extrapolation at time 1668746370.612398459, but only time 1668746370.746266661 is in the buffer, when looking up transform from frame [base_link] to frame [map]
[ERROR] [1668746370.722842929]: Lookup would require extrapolation into the past.  Requested time 1668746370.712370635 but the earliest data is at time 1668746370.746266661, when looking up transform from frame [base_link] to frame [map]
[ INFO] [1668746380.522812069]: !! WAYPOINT_NAV_PLANNING_ABORTED !!
[ INFO] [1668746380.522864525]: cancelGoal() is called !!
[ INFO] [1668746380.622822948]: Next Waypoint : 0
[ INFO] [1668746380.622898545]: Next WayPoint is got
[ INFO] [1668746380.622913915]: Go next_waypoint.

該当のソースコード

c++
1  // 通常のwaypointの場合
2  void setNextGoal(WayPoint waypoint) {
3    reach_threshold_ = waypoint.reach_threshold_;
4    this->sendNextWaypointMarker(waypoint.goal_.target_pose.pose, 0); // 現在目指しているwaypointを表示する
5    this->sendNewGoal(waypoint.goal_.target_pose.pose);
6  }
7
8  double getReachThreshold() {
9    return reach_threshold_;
10  }
11
12  geometry_msgs::Pose getRobotCurrentPosition() {
13    // tfを使ってロボットの現在位置を取得する
14    tf::StampedTransform transform;
15    geometry_msgs::Pose pose;
16    try {
17      listener_.lookupTransform("/map", "/base_link", ros::Time(0), transform);
18      // laser scan matcher navigationの時は以下に設定
19      //listener_.lookupTransform("/odom", "/base_link", ros::Time(0), transform);
20    } catch (tf::TransformException ex) {
21      ROS_ERROR("%s", ex.what());
22    }
23    pose.position.x = transform.getOrigin().x();
24    pose.position.y = transform.getOrigin().y();
25    //ROS_INFO_STREAM("c)x :" << pose.position.x << ", y :" << pose.position.y);
26    return pose;
27  }
28
29  geometry_msgs::Pose getNowGoalPosition() {
30    //ROS_INFO_STREAM("g)x :" << now_goal_.position.x << ", y :" << now_goal_.position.y);
31    return now_goal_;
32  }

試したこと

navigationでは[map]→[odom]のtfはamclで更新しており、amclのパーティクル数が増え、処理時間が増えたことでタイムスタンプの更新が遅くなる。その結果としてタイムスタンプが遅れている。という記事を拝見しましたのでamclのパラメータである"max_particles"を下げる対応をしましたが、状況は変わりません。