Add benchmark metrics

Change global planner topic
Add success count

include/waypointgen/setpoint_server.h

@@ -14,6 +14,8 @@
 #include <nav_msgs/Path.h>
 #include <std_msgs/Float32.h>
 
+#include <thread>
+#include <chrono>
 #include <filesystem>
 #include <fstream>
 #include <yaml-cpp/yaml.h>
@@ -34,15 +36,13 @@ class waypointgen_server {
 
   // Status of server
   /*
-  Server would have 5 states:
+  Server would have 4 states:
   PLAY-> Run the server
   STOP-> Stop the server
   PAUSE -> Pause server
-  //Below 2 are not callable by topic
   IDLE->Wait for wpg_server_status topic
-  DONE->Server complete waypoint list
   */
-  enum class ServerState { PLAY, STOP, PAUSE, IDLE, DONE };
+  enum class ServerState { PLAY, STOP, PAUSE, IDLE };
 
   // Callbacks
   bool start_p2p(waypointgen::Trigger::Request &req,
@@ -89,13 +89,16 @@ class waypointgen_server {
     return &this->currentWaypoint;
   }
 
+  // Timer
+  void reset_timer();
+  double elapsed_time();
+
   // Play back
-  void p2p(const int &currentWP,
+  bool p2p(const int &currentWP,
            const geometry_msgs::Pose &qpt); // Point 2 Point
   void begin_playback();
 
 private:
-
   typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
       MoveBaseClient;
 
@@ -125,11 +128,22 @@ class waypointgen_server {
   ros::Timer timerGoal; // Refresh and get distance to goal
   float ecDist;         // Euclidean distance from goal
   float gpDist;         // Global Path distance from goal
+  float ecDistMax;      // Euclidean distance from goal, Max
+  float gpDistMax;      // Global Path distance from goal, Max
 
   ServerState current_state;
   int s_state_delay; // Delay before starting the server play, in
                      // seconds
 
   std::vector<geometry_msgs::Pose> wpList; // Waypoint listed
   geometry_msgs::Pose currentWaypoint;     // current waypoint
+
+  // Timer for benchmarking
+  using clock_type = std::chrono::steady_clock;
+  using d_type = std::chrono::duration<double, std::ratio<1>>;
+
+  std::chrono::time_point<clock_type> path_timer{clock_type::now()};
+
+  //Benchmark vals
+  std::vector<bool> wpt_benchmark_success;
 };

launch/setpoint_server.launch

@@ -1,7 +1,6 @@
 <launch>
   <arg name="list_path" default="$(find waypointgen)/wp_list/a.yaml" />
-  <arg name="globalplan_topic" default="/move_base/TebLocalPlannerROS/global_plan" />
-
+  <arg name="globalplan_topic" default="/move_base/NavfnROS/plan" />  <!-- global path planner, not local -->
 
   <node pkg="waypointgen" type="setpoint_server" respawn="false" name="setpoint_server" output="screen">
     <param name="waypoint_listpath" type="str" value="$(arg list_path)"/>

src/setpoint_server.cpp

@@ -1,8 +1,9 @@
 #include "waypointgen/setpoint_server.h"
+#include <vector>
 
 waypointgen_server::waypointgen_server(ros::NodeHandle nh)
     : wp_count(0), numOfWaypoints(0), distToGoal(0.0), ecDist(0.0), gpDist(0.0),
-      s_state_delay(0), current_state(ServerState::IDLE),
+      s_state_delay(0), current_state(ServerState::IDLE), gpDistMax(-1.0),
       global_path_topic("/move_base/TebLocalPlannerROS/global_plan") {
   this->nh_ = nh;
 }
@@ -77,7 +78,6 @@ bool waypointgen_server::start_p2p(waypointgen::Trigger::Request &req,
     set_s_state_delay(req.play_delay);
     res.play_triggered = true;
     ROS_INFO("Setting [%d] delay before playback", req.play_delay);
-
     begin_playback();
   }
   return true;
@@ -108,9 +108,9 @@ void waypointgen_server::goalFeedbackCB(
 
   // Calculate displacement from goal
   ecDist = getPathDist(dFromGoal);
-#ifdef DEBUG
-// ROS_INFO("Euclidean distance: %f",ecDist);
-#endif
+  if (ecDist > ecDistMax)
+    ecDistMax = ecDist;
+  // ROS_INFO("Euclidean distance: %f", ecDist);
 
   // ROS_INFO("[X]:%f [Y]:%f [W]:
   // %f",feedback->base_position.pose.position.x,feedback->base_position.pose.position.y,feedback->base_position.pose.orientation.w);
@@ -120,22 +120,23 @@ void waypointgen_server::goalFeedbackCB(
 void waypointgen_server::gPlanCallback(const nav_msgs::Path &msg) {
   std::vector<geometry_msgs::PoseStamped> path_poses = msg.poses;
   gpDist = getPathDist(path_poses); // Calculate path to target waypoint
-#ifdef DEBUG
-// ROS_INFO("Global Path len: %f", gpDist);
-#endif
+  if (gpDist > gpDistMax)
+    gpDistMax = gpDist;
+
+  // ROS_INFO("Global Path len: %f", gpDist);
 }
 
 // Publish average of Global path length & Euclidean distance from target if
 // both are non zero
 void waypointgen_server::timerGoalCallback(const ros::TimerEvent &event) {
   std_msgs::Float32 tgoal;
-  if (ecDist == 0) {
+  if (ecDist == 0)
     tgoal.data = gpDist;
-  } else if (gpDist == 0) {
+  else if (gpDist == 0)
     tgoal.data = ecDist;
-  } else {
+  else
     tgoal.data = (ecDist + gpDist) / 2;
-  }
+
   distToGoalPub.publish(tgoal);
   // ROS_INFO("Avg distance: %f",tgoal.data);
 }
@@ -243,28 +244,36 @@ geometry_msgs::PoseStamped waypointgen_server::convertToPoseStamped(
   poseStamped.header.stamp = ros::Time::now();
 
   poseStamped.pose.position = poseTarget.position; // Set position
-
-  tf::Quaternion q;
-  q.setRPY(0, 0, PI); // rotate by pi (offset)
-
   poseStamped.pose.orientation =
-      poseTarget.orientation * q; // Set rotation (Quaternion)
+      poseTarget.orientation; // Set rotation (Quaternion)
 
   return poseStamped;
 }
 
+// Timer
+void waypointgen_server::reset_timer() { path_timer = clock_type::now(); }
+
+double waypointgen_server::elapsed_time() {
+  return std::chrono::duration_cast<d_type>(clock_type::now() - path_timer)
+      .count();
+}
+
 // Point to point navigation
-void waypointgen_server::p2p(const int &currentWP,
+bool waypointgen_server::p2p(const int &currentWP,
                              const geometry_msgs::Pose &qpt) {
   // tell the action client that we want to spin a thread by default
   MoveBaseClient ac("move_base", true);
-  ROS_INFO("Moving out soon...");
+  // ROS_INFO("Moving out soon...");
 
   // wait for the action server to come up
   while (!ac.waitForServer(ros::Duration(5.0))) {
     ROS_INFO("Waiting for the move_base action server to come up");
   }
 
+  // Reset distance;
+  ecDistMax = 0;
+  gpDistMax = 0;
+
   // Create goal waypoint
   move_base_msgs::MoveBaseGoal goal;
   goal.target_pose.header.frame_id = "map"; // reference to map
@@ -288,7 +297,7 @@ void waypointgen_server::p2p(const int &currentWP,
   yaw *= 180 / PI; // Convert to degrees
 
   // Current waypoint, total waypoint, x pos, y pos, angular (yaw)
-  ROS_INFO("Sending next goal [%i/%i]: (%.2f,%.2f, %.2f)", currentWP + 1,
+  ROS_INFO(">> Sending next goal [%i/%i]: (%.2f,%.2f, %.2f)", currentWP + 1,
            wp_count, qpt.position.x, qpt.position.y, yaw);
 
   // ac.sendGoal(goal,boost::bind(&waypointgen::goalDoneCB,
@@ -298,21 +307,42 @@ void waypointgen_server::p2p(const int &currentWP,
               MoveBaseClient::SimpleActiveCallback(),
               boost::bind(&waypointgen_server::goalFeedbackCB, this, _1));
 
+  reset_timer(); // Restart timer
+
   // Publish current waypoint goal
   pointPubGoal.publish(goalPose);
 
   ac.waitForResult();
 
-  // wayptCounter++;
+  // Benchmark
+  auto tt{elapsed_time()};
+  auto egp{gpDistMax / ecDistMax};
+  auto isSuccessful{false};
 
+  // todo: Add final dist and ang from goal post when action server returns val,
+  // count num of success/abort
+  std::string results_msg{"<< "};
   if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
-    ROS_INFO("Done!");
+    results_msg += "Reached!";
+    isSuccessful = true;
   } else if (ac.getState() == actionlib::SimpleClientGoalState::LOST) {
-    ROS_INFO("Skipping to next goal");
-    exit(0);
+    results_msg += "Lost, skipping to next goal...";
   } else {
-    ROS_INFO("The base failed to move forward for some reason");
+    results_msg += "The base failed to move forward for some reason";
   }
+
+  ROS_INFO("%s", results_msg.c_str());
+  ROS_INFO("== Results ==");
+  ROS_INFO("Reached goal?\t\t\t\t%s ", isSuccessful ? "Yes" : "No");
+  ROS_INFO("Time taken:\t\t\t\t%.4f s", tt);
+  ROS_INFO("Global Path Distance:\t\t\t%.4f m", gpDistMax);
+  ROS_INFO("Euclidean Path Distance:\t\t\t%.4f m", ecDistMax); // To way point
+  ROS_INFO("Global Path/Euclidean Ratio:\t\t%.4f ", egp);      // Normalised
+  ROS_INFO("Average speed:\t\t\t\t%.4f ms-1",
+           0.5 * (gpDistMax + ecDistMax) / tt);
+  ROS_INFO("==========================");
+
+  return isSuccessful;
 }
 
 void waypointgen_server::begin_playback() {
@@ -325,9 +355,28 @@ void waypointgen_server::begin_playback() {
   // Start Navigation
   for (int i = 0; i < get_wpList()->size(); ++i) {
     set_currentWaypoint(get_wpList()->at(i));
-    p2p(i, *get_currentWaypoint());
+    wpt_benchmark_success.push_back(p2p(i, *get_currentWaypoint()));
   }
 
+  // Print results
+  auto q{0};
+  for (const auto &i : wpt_benchmark_success)
+    q += i;
+  ROS_INFO("Success ratio: %i/%i", q, wpt_benchmark_success.size());
+
+  // ROS_INFO("%s\n== Results ==", results_msg.c_str());
+  // ROS_INFO("Reached goal?\t\t\t\t%s ", isSuccessful ? "Yes" : "No");
+  // ROS_INFO("Time taken:\t\t\t\t%.4f s", tt);
+  // ROS_INFO("Global Path Distance:\t\t\t%.4f m", gpDistMax);
+  // ROS_INFO("Euclidean Path Distance:\t\t%.4f m", ecDistMax); // To way point
+  // ROS_INFO("Global Path/Euclidean Ratio:\t\t%.4f ", egp);    // Normalised
+  // val ROS_INFO("Time / (Euclidean/Global) Ratio:\t%.4f s", tt / egp);
+  // ROS_INFO("Average speed:\t\t\t\t%.4f ms-1",
+  //          0.5 * (gpDistMax + ecDistMax) / tt);
+
+  // ROS_INFO("==========================");
+
+  set_state(waypointgen_server::ServerState::IDLE);
   ROS_INFO("Completed playback!");
 }
 
@@ -340,9 +389,8 @@ int main(int argc, char **argv) {
 
   // Start Multithreading Process(Async thread):
   // http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning
-  // std::thread::hardware_concurrency -> Returns the number of concurrent
-  // threads supported by the implementation
-  ros::AsyncSpinner spinner(boost::thread::hardware_concurrency());
+
+  ros::AsyncSpinner spinner(std::thread::hardware_concurrency());
   ros::Rate r(10); // Run at 10Hz
 
   spinner.start();

wp_list/a.yaml

@@ -1,29 +1,50 @@
-count: 4
+count: 7
 WP0:
-  - 2.539377927780151
-  - 1.280890941619873
-  - -5.391525892406435e-09
-  - -3.520319799554805e-11
-  - 0.09011995792388916
-  - 0.9959309697151184
-WP1:
-  - 11.39962387084961
-  - -11.74889087677002
-  - 5.697565441664665e-08
-  - 5.811706671465799e-11
-  - -0.9500159025192261
-  - 0.3122022449970245
-WP2:
-  - 13
-  - 0
-  - 4.352147087161029e-08
-  - 1.901655395653012e-11
-  - -0.730354368686676
-  - 0.6830685138702393
-WP3:
-  - -0.4851150512695312
-  - -14.90365123748779
+  - 5.5
+  - 1
   - 0
   - 0
   - 0
-  - 1
+  - 1
+WP1:
+  - 7.024173259735107
+  - -8.903743743896484
+  - 5.325580687554066e-08
+  - 7.173401872506702e-10
+  - -0.8941627144813538
+  - 0.4477424621582031
+WP2:
+  - 1.505575656890869
+  - -16.35747146606445
+  - -2.658391551335626e-08
+  - 7.192246798126689e-11
+  - 0.4478384256362915
+  - 0.8941146731376648
+WP3:
+  - 9.778033256530762
+  - -28.15631866455078
+  - 2.217753625188834e-08
+  - 1.486431810970856e-10
+  - -0.3723661303520203
+  - 0.9280859231948853
+WP4:
+  - 25.94416999816895
+  - -15.67919826507568
+  - 1.223247281245676e-09
+  - 4.396502259473856e-11
+  - -0.02128663100302219
+  - 0.9997734427452087
+WP5:
+  - 24.23751068115234
+  - -0.1601438522338867
+  - -4.687328925001566e-08
+  - 1.428518275892188e-10
+  - 0.7855000495910645
+  - 0.6188615560531616
+WP6:
+  - 16.34604835510254
+  - 15.62290477752686
+  - -5.280491066628201e-08
+  - -1.039316896433995e-10
+  - 0.8894365429878235
+  - 0.4570586383342743