From 3d8ea0b166d37b032f5a23007d8ecc0d392f5419 Mon Sep 17 00:00:00 2001 From: 1487quantum <1487quantum@users.noreply.github.com> Date: Sat, 25 Jun 2022 16:03:54 +0800 Subject: [PATCH] Add benchmark metrics Change global planner topic Add success count --- include/waypointgen/setpoint_server.h | 26 +++++-- launch/setpoint_server.launch | 3 +- src/setpoint_server.cpp | 106 +++++++++++++++++++------- wp_list/a.yaml | 71 +++++++++++------ 4 files changed, 144 insertions(+), 62 deletions(-) diff --git a/include/waypointgen/setpoint_server.h b/include/waypointgen/setpoint_server.h index 434db52..7a7d5ea 100644 --- a/include/waypointgen/setpoint_server.h +++ b/include/waypointgen/setpoint_server.h @@ -14,6 +14,8 @@ #include #include +#include +#include #include #include #include @@ -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 ¤tWP, + bool p2p(const int ¤tWP, const geometry_msgs::Pose &qpt); // Point 2 Point void begin_playback(); private: - typedef actionlib::SimpleActionClient MoveBaseClient; @@ -125,6 +128,8 @@ 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 @@ -132,4 +137,13 @@ class waypointgen_server { std::vector 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>; + + std::chrono::time_point path_timer{clock_type::now()}; + + //Benchmark vals + std::vector wpt_benchmark_success; }; diff --git a/launch/setpoint_server.launch b/launch/setpoint_server.launch index e9341c4..127c931 100644 --- a/launch/setpoint_server.launch +++ b/launch/setpoint_server.launch @@ -1,7 +1,6 @@ - - + diff --git a/src/setpoint_server.cpp b/src/setpoint_server.cpp index 7107d81..ce7f845 100644 --- a/src/setpoint_server.cpp +++ b/src/setpoint_server.cpp @@ -1,8 +1,9 @@ #include "waypointgen/setpoint_server.h" +#include 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 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(clock_type::now() - path_timer) + .count(); +} + // Point to point navigation -void waypointgen_server::p2p(const int ¤tWP, +bool waypointgen_server::p2p(const int ¤tWP, 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 ¤tWP, 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 ¤tWP, 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(); diff --git a/wp_list/a.yaml b/wp_list/a.yaml index 74500d3..b64bd9d 100644 --- a/wp_list/a.yaml +++ b/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 \ No newline at end of file + - 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 \ No newline at end of file