Skip to content

Planner Moving Average #395

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Planner Moving Average #395

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0fe1bf0
changed avg to mvg avg
Kyle657 Aug 31, 2021
fd474f4
changed avg to mvg avg
Kyle657 Aug 31, 2021
2ec25fe
changed avg to mvg avg
Kyle657 Aug 31, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 11 additions & 9 deletions rr_common/src/planner/planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <rr_msgs/steering.h>
#include <visualization_msgs/Marker.h>

#include <numeric>
#include <rr_common/linear_tracking_filter.hpp>

constexpr int ctrl_dim = 2;
Expand Down Expand Up @@ -51,8 +52,8 @@ reverse_state_t reverse_state;
double steering_gain;
double viz_path_scale;

double total_planning_time;
size_t total_plans;
std::vector<double> time_window;
const int time_window_size = 50;

void update_messages(double speed, double angle) {
auto now = ros::Time::now();
Expand Down Expand Up @@ -265,9 +266,6 @@ int main(int argc, char** argv) {
g_effector_tracker =
std::make_unique<rr::EffectorTracker>(ros::NodeHandle(nhp, "effector_tracker"), speed_message, steer_message);

total_planning_time = 0;
total_plans = 0;

g_steer_model->Reset(0, ros::Time::now().toSec());
g_speed_model->Reset(0, ros::Time::now().toSec());

Expand All @@ -291,10 +289,14 @@ int main(int argc, char** argv) {
g_map_cost_interface->SetMapStale();

double seconds = (ros::WallTime::now() - start).toSec();
total_planning_time += seconds;
total_plans++;
double sec_avg = total_planning_time / total_plans;
ROS_INFO("PlanningOptimizer took %0.1fms, average %0.2fms", seconds * 1000, sec_avg * 1000);

time_window.push_back(seconds);
if (time_window.size() >= time_window_size) {
time_window.erase(time_window.begin());
}
double sec_mvg_avg = std::accumulate(time_window.begin(), time_window.end(), 0.0) / time_window.size();

ROS_INFO("PlanningOptimizer took %0.1fms, average %0.2fms", seconds * 1000, sec_mvg_avg * 1000);
}
}

Expand Down