search

rayvburn / humap_local_planner

Human-aware robot trajectory planner using a hybrid trajectory candidates generation and spatiotemporal cost functions
BSD 3-Clause "New" or "Revised" License
0 stars 1 forks source link

investigate long computation times of social trajectory generation #88

Closed rayvburn closed 1 year ago

rayvburn commented 1 year ago

Timing of SocialTrajectoryGenerator::computeForces:

[computeForces] full 56 us | sfm 44 us | fis 6 us (processor 0 us, conductor 4 us)

whereas generation of all trajectories (checked in the last call to SocialTrajectoryGenerator::nextTrajectory):

[SocialTrajectoryGenerator] full trajectory generation (105 trajs) took 288 ms

So main thing is checking why SFM calculations take so long

rayvburn commented 1 year ago

It seems that there are some resources interactions because the same execution of SocialForceModel::computeInteractionForce can take: 21/15/18/134 microseconds (for 7 static objects).

It seems logical that the thread of a planner is managed by the move_base node so it can be the source of problems? Maybe it is crucial to make planner finish previous computation step before starting another because move_base blindly calls for a new velocity command?

This is another interesting example:

[computeForces] full 37 us | sfm 29 us | fis 5 us (processor 0 us, conductor 2 us)
[computeForces] full 41 us | sfm 33 us | fis 4 us (processor 0 us, conductor 2 us)
[SFM us] FUL 535, RST 0, GRBT 0, ITRNL 1, GOBJ 3, ITRSTAT 522 (5 objects), ITRDYN 0 (0 objects), COEFF 0, NONLIN 5
[computeForces] full 566 us | sfm 551 us | fis 9 us (processor 0 us, conductor 4 us)
[computeForces] full 55 us | sfm 43 us | fis 7 us (processor 0 us, conductor 4 us)
[computeForces] full 57 us | sfm 45 us | fis 7 us (processor 0 us, conductor 4 us)
rayvburn commented 1 year ago

Diffs for debugging:

unfold by clicking here...

```diff diff --git a/src/sfm/social_force_model.cpp b/src/sfm/social_force_model.cpp index 2fd0a7c..5dcf66c 100644 --- a/src/sfm/social_force_model.cpp +++ b/src/sfm/social_force_model.cpp @@ -135,14 +135,18 @@ bool SocialForceModel::computeSocialForce( std::vector& meaningful_static, std::vector& meaningful_dynamic ) { + auto ts_full_start = std::chrono::high_resolution_clock::now(); reset(); + auto ts_reset_end = std::chrono::high_resolution_clock::now(); auto robot = world.getRobotData(); + auto ts_getrobotdata_end = std::chrono::high_resolution_clock::now(); // ----------------------------------------------------------------------------------------- // ------- internal force calculation ------------------------------------------------------ // ----------------------------------------------------------------------------------------- force_internal_ = computeInternalForce(robot); + auto ts_force_internal_end = std::chrono::high_resolution_clock::now(); // check whether it is needed to calculate interaction force or only the internal one // should be considered @@ -158,13 +162,17 @@ bool SocialForceModel::computeSocialForce( // ------ interaction forces calculation (repulsive, attractive) --------------------------- // ----------------------------------------------------------------------------------------- // environment model + auto ts_getting_objects_start = std::chrono::high_resolution_clock::now(); auto objects_static = world.getStaticObjectsData(); auto objects_dynamic = world.getDynamicObjectsData(); + auto ts_getting_objects_end = std::chrono::high_resolution_clock::now(); // added up to the `force_interaction_` Vector f_alpha_beta; // investigate STATIC objects of the environment + // --- + auto ts_force_interaction_static_start = std::chrono::high_resolution_clock::now(); for (const StaticObject& object: objects_static) { f_alpha_beta = computeInteractionForce(robot, object, dt); force_interaction_static_ += f_alpha_beta; @@ -172,8 +180,23 @@ bool SocialForceModel::computeSocialForce( meaningful_static.push_back(Distance {.robot = object.robot, .object = object.object}); } } + auto ts_force_interaction_static_end = std::chrono::high_resolution_clock::now(); + // // --- + // force_interaction_static_ = Vector(); + // meaningful_static.clear(); + // auto ts_force_interaction_static2_start = std::chrono::high_resolution_clock::now(); + // for (const StaticObject& object: objects_static) { + // f_alpha_beta = computeInteractionForce(robot, object, dt); + // force_interaction_static_ += f_alpha_beta; + // if (f_alpha_beta.calculateLength() >= 1e-06) { + // meaningful_static.push_back(Distance {.robot = object.robot, .object = object.object}); + // } + // } + // auto ts_force_interaction_static2_end = std::chrono::high_resolution_clock::now(); + // // --- // investigate DYNAMIC objects of the environment + auto ts_force_interaction_dynamic_start = std::chrono::high_resolution_clock::now(); for (const DynamicObject& object: objects_dynamic) { f_alpha_beta = computeInteractionForce(robot, object); force_interaction_dynamic_ += f_alpha_beta; @@ -181,17 +204,57 @@ bool SocialForceModel::computeSocialForce( meaningful_dynamic.push_back(Distance {.robot = object.robot, .object = object.object}); } } + auto ts_force_interaction_dynamic_end = std::chrono::high_resolution_clock::now(); // ----------------------------------------------------------------------------------------- // ------- additional calculations --------------------------------------------------------- // ----------- (i.a. smoothing) ------------------------------------------------------------ // ----------------------------------------------------------------------------------------- // multiply force vector components by parameter values + auto ts_force_coeffs_start = std::chrono::high_resolution_clock::now(); factorInForceCoefficients(); + auto ts_force_coeffs_end = std::chrono::high_resolution_clock::now(); // extend or truncate force vectors if needed + auto ts_nonlin_operations_start = std::chrono::high_resolution_clock::now(); applyNonlinearOperations(world.getDistanceClosestStaticObject(), world.getDistanceClosestDynamicObject()); + auto ts_nonlin_operations_end = std::chrono::high_resolution_clock::now(); + auto ts_full_end = std::chrono::high_resolution_clock::now(); + auto full_time = std::chrono::duration_cast(ts_full_end - ts_full_start).count(); + auto reset_time = std::chrono::duration_cast(ts_reset_end - ts_full_start).count(); + auto getrobotdata_time = std::chrono::duration_cast(ts_getrobotdata_end - ts_reset_end).count(); + auto force_internal_time = std::chrono::duration_cast(ts_force_internal_end - ts_getrobotdata_end).count(); + auto getting_objects_time = std::chrono::duration_cast(ts_getting_objects_end - ts_getting_objects_start).count(); + auto f_interaction_static_time = std::chrono::duration_cast(ts_force_interaction_static_end - ts_force_interaction_static_start).count(); + // auto f_interaction_static1_time = std::chrono::duration_cast(ts_force_interaction_static1_end - ts_force_interaction_static1_start).count(); + // auto f_interaction_static2_time = std::chrono::duration_cast(ts_force_interaction_static2_end - ts_force_interaction_static2_start).count(); + // auto f_interaction_static3_time = std::chrono::duration_cast(ts_force_interaction_static3_end - ts_force_interaction_static3_start).count(); + // auto f_interaction_static4_time = std::chrono::duration_cast(ts_force_interaction_static4_end - ts_force_interaction_static4_start).count(); + auto f_interaction_dynamic_time = std::chrono::duration_cast(ts_force_interaction_dynamic_end - ts_force_interaction_dynamic_start).count(); + auto f_coeffs_time = std::chrono::duration_cast(ts_force_coeffs_end - ts_force_coeffs_start).count(); + auto f_nonlin_time = std::chrono::duration_cast(ts_nonlin_operations_end - ts_nonlin_operations_start).count(); + + if (full_time > 100) { + std::cout << + "\x1B[33m" + "[SFM us] FUL " << full_time << ", RST " << reset_time << ", GRBT " << getrobotdata_time << ", ITRNL " << force_internal_time << + ", GOBJ " << getting_objects_time << ", ITRSTAT " << f_interaction_static_time << " (" << objects_static.size() << " objects)" + ", ITRDYN " << f_interaction_dynamic_time << " (" << objects_dynamic.size() << " objects)" + ", COEFF " << f_coeffs_time << ", NONLIN " << f_nonlin_time << + "\x1B[0m" + << std::endl; + // std::cout << + // "\x1B[33m" + // "[SFM us] FUL " << full_time << ", RST " << reset_time << ", GRBT " << getrobotdata_time << ", ITRNL " << force_internal_time << + // ", GOBJ " << getting_objects_time << + // ", ITRSTAT " << f_interaction_static1_time << "/" << f_interaction_static2_time << "/" << f_interaction_static3_time << "/" << f_interaction_static4_time << + // " (" << objects_static.size() << " objects)" + // ", ITRDYN " << f_interaction_dynamic_time << " (" << objects_dynamic.size() << " objects)" + // ", COEFF " << f_coeffs_time << ", NONLIN " << f_nonlin_time << + // "\x1B[0m" + // << std::endl; + } return true; } @@ -414,23 +477,28 @@ Vector SocialForceModel::computeInteractionForce( Vector SocialForceModel::computeInteractionForce(const Robot& robot, const StaticObject& object, const double &dt) { /* elliptical formulation - `14 article - equations (3) and (4) */ + auto ts_full_start = std::chrono::high_resolution_clock::now(); // distance vector Vector d_alpha_i = -object.dist_v; // note the proper direction d_alpha_i.setZ(0.00); // planar (this could be ommitted) double d_alpha_i_len = object.dist; + auto ts_d_alpha_i_end = std::chrono::high_resolution_clock::now(); // length (vβ * ∆t) of the stride (step size) Vector y_alpha_i = robot.vel * dt; y_alpha_i.setZ(0.00); // planar (this could be ommitted) + auto ts_y_alpha_i_end = std::chrono::high_resolution_clock::now(); // semi-minor axis of the elliptic formulation double w_alpha_i = 0.5 * sqrt( std::pow((d_alpha_i_len + (d_alpha_i - y_alpha_i).calculateLength()),2) - std::pow(y_alpha_i.calculateLength(), 2) ); + auto ts_w_alpha_i_end = std::chrono::high_resolution_clock::now(); // division by ~0 prevention - returning zeros vector instead of NaNs bool w_alpha_i_small = std::abs(w_alpha_i) < 1e-08; bool w_alpha_i_nan = std::isnan(w_alpha_i); bool d_alpha_i_short = d_alpha_i_len < 1e-08; + auto ts_d_alpha_i_cond_end = std::chrono::high_resolution_clock::now(); if (w_alpha_i_small && !w_alpha_i_nan && !d_alpha_i_short) { debug_print_verbose("----static obstacle interaction abort ---- \r\n"); debug_print_verbose("\t FAIL w_alpha_i small \r\n"); @@ -450,9 +518,9 @@ Vector SocialForceModel::computeInteractionForce(const Robot& robot, const Stati } // ~force (acceleration) calculation + auto ts_f_alpha_i_start = std::chrono::high_resolution_clock::now(); Vector f_alpha_i; + auto ts_f_alpha_i_end = std::chrono::high_resolution_clock::now(); /* * Check whether beta is within the field of view to determine a proper factor for force in case @@ -462,6 +530,7 @@ Vector SocialForceModel::computeInteractionForce(const Robot& robot, const Stati double fov_factor = computeFactorFOV(angle_relative); f_alpha_i *= fov_factor; + auto ts_fov_end = std::chrono::high_resolution_clock::now(); debug_print_verbose("----static obstacle interaction \r\n"); debug_print_verbose("\t d_alpha_i: %2.3f %2.3f, len: %2.3f \r\n", @@ -480,6 +549,23 @@ Vector SocialForceModel::computeInteractionForce(const Robot& robot, const Stati ); debug_print_verbose("---- \r\n"); + auto full_time = std::chrono::duration_cast(ts_fov_end - ts_full_start).count(); + auto d_alpha_i_time = std::chrono::duration_cast(ts_d_alpha_i_end - ts_full_start).count(); + auto y_alpha_i_time = std::chrono::duration_cast(ts_y_alpha_i_end - ts_d_alpha_i_end).count(); + auto w_alpha_i_time = std::chrono::duration_cast(ts_w_alpha_i_end - ts_y_alpha_i_end).count(); + auto d_alpha_i_cond_time = std::chrono::duration_cast(ts_d_alpha_i_cond_end - ts_w_alpha_i_end).count(); + auto f_alpha_i_time = std::chrono::duration_cast(ts_f_alpha_i_end - ts_f_alpha_i_start).count(); + auto fov_time = std::chrono::duration_cast(ts_fov_end - ts_f_alpha_i_end).count(); + if (full_time > 10) { + std::cout << + "\x1B[34m" + "[SFM STAT us] FUL " << full_time << ", DAI " << d_alpha_i_time << ", YAI " << y_alpha_i_time << ", WAI " << w_alpha_i_time << + ", DAI_COND " << d_alpha_i_cond_time << ", FAI " << f_alpha_i_time << + ", FOV " << fov_time << + "\x1B[0m" + << std::endl; + } + return f_alpha_i; } ```

unfold by clicking here...

```diff diff --git a/src/social_trajectory_generator.cpp b/src/social_trajectory_generator.cpp index 7ec84bd..1ec12be 100644 --- a/src/social_trajectory_generator.cpp +++ b/src/social_trajectory_generator.cpp @@ -8,6 +8,9 @@ namespace hubero_local_planner { using namespace base_local_planner; using namespace geometry; +bool g_just_initialised = false; +auto g_ts_stg_start = std::chrono::high_resolution_clock::now(); + SocialTrajectoryGenerator::SocialTrajectoryGenerator(): params_set_(false), limits_planner_ptr_(nullptr), @@ -72,6 +75,8 @@ void SocialTrajectoryGenerator::initialise( const double& robot_mass, bool discretize_by_time ) { + g_just_initialised = true; + // save copies for later use world_model_ = world_model; vel_local_ = robot_local_vel; @@ -246,6 +251,11 @@ void SocialTrajectoryGenerator::initialise( } bool SocialTrajectoryGenerator::nextTrajectory(Trajectory& traj) { + if (g_just_initialised) { + g_ts_stg_start = std::chrono::high_resolution_clock::now(); + g_just_initialised = false; + } + bool result = false; if (hasMoreTrajectories()) { // try to generate a new trajectory, starting with the initial world model given in `initialise` @@ -258,6 +268,11 @@ bool SocialTrajectoryGenerator::nextTrajectory(Trajectory& traj) { } } next_sample_index_++; + if (!hasMoreTrajectories()) { + auto ts_full_end = std::chrono::high_resolution_clock::now(); + auto full_time = std::chrono::duration_cast(ts_full_end - g_ts_stg_start).count(); + std::cout << "\x1B[35m [SocialTrajectoryGenerator] full trajectory generation (" << sample_amplifier_params_v_.size() << " trajs) took " << full_time << " ms \x1B[0m" << std::endl; + } return result; } ```

unfold by clicking here...

```diff diff --git a/src/social_trajectory_generator.cpp b/src/social_trajectory_generator.cpp index 7ec84bd..e9b0a78 100644 --- a/src/social_trajectory_generator.cpp +++ b/src/social_trajectory_generator.cpp @@ -570,6 +570,8 @@ void SocialTrajectoryGenerator::computeForces( geometry::Vector& force_human_action, bool update_motion_data ) { + auto ts_full_start = std::chrono::high_resolution_clock::now(); + // store vectors of poses of closest points between robot and other objects; makes use out of obstacles // representation (extracted from costmap) and robot's footprint; // these, in fact, are used only for visualisation @@ -603,12 +605,14 @@ void SocialTrajectoryGenerator::computeForces( social_conductor_.setEquationParameters(sample_amplifiers.fis_as_amplifier); // begin calculations + auto ts_sfm_start = std::chrono::high_resolution_clock::now(); sfm_.computeSocialForce( world_model, dt, meaningful_interaction_static, meaningful_interaction_dynamic ); + auto ts_sfm_end = std::chrono::high_resolution_clock::now(); // actual `social` vector geometry::Vector human_action_force; @@ -616,6 +620,11 @@ void SocialTrajectoryGenerator::computeForces( std::vector objects_static = world_model.getStaticObjectsData(); std::vector objects_dynamic = world_model.getDynamicObjectsData(); + auto ts_fis_start = std::chrono::high_resolution_clock::now(); + auto ts_fis_processor_start = std::chrono::high_resolution_clock::now(); + auto ts_fis_processor_end = std::chrono::high_resolution_clock::now(); + auto ts_fis_conductor_start = std::chrono::high_resolution_clock::now(); + auto ts_fis_conductor_end = std::chrono::high_resolution_clock::now(); // evaluate whether more complex forces are supposed to be calculated if (!sfm_.areInteractionForcesDisabled() && !social_conductor_.areFuzzyBehavioursDisabled()) { // All multi-element data are vectors of the same length whose corresponding elements are related @@ -644,9 +653,12 @@ void SocialTrajectoryGenerator::computeForces( } // execute fuzzy operations block + ts_fis_processor_start = std::chrono::high_resolution_clock::now(); fuzzy_processor_.process(dir_alpha, dir_beta_dynamic, rel_loc_dynamic, dist_angle_dynamic); + ts_fis_processor_end = std::chrono::high_resolution_clock::now(); // create a force vector according to the activated `social behaviour` + ts_fis_conductor_start = std::chrono::high_resolution_clock::now(); social_conductor_.computeBehaviourForce( world_model.getRobotData().centroid, world_model.getRobotData().speed, @@ -654,10 +666,12 @@ void SocialTrajectoryGenerator::computeForces( speed_dynamic, dist_dynamic ); + ts_fis_conductor_end = std::chrono::high_resolution_clock::now(); // assign `social` vector human_action_force = social_conductor_.getSocialVector(); } + auto ts_fis_end = std::chrono::high_resolution_clock::now(); force_internal = sfm_.getForceInternal(); force_interaction_dynamic = sfm_.getForceInteractionDynamic(); @@ -697,6 +711,14 @@ void SocialTrajectoryGenerator::computeForces( diag_closest_points_dynamic_.push_back(dist_dynamic.robot); } } + + auto ts_full_end = std::chrono::high_resolution_clock::now(); + auto full_time = std::chrono::duration_cast(ts_full_end - ts_full_start).count(); + auto sfm_time = std::chrono::duration_cast(ts_sfm_end - ts_sfm_start).count(); + auto fis_time = std::chrono::duration_cast(ts_fis_end - ts_fis_start).count(); + auto fis_processor_time = std::chrono::duration_cast(ts_fis_processor_end - ts_fis_processor_start).count(); + auto fis_conductor_time = std::chrono::duration_cast(ts_fis_conductor_end - ts_fis_conductor_start).count(); + std::cout << "[computeForces] full " << full_time << " us | sfm " << sfm_time << " us | fis " << fis_time << " us (processor " << fis_processor_time << " us, conductor " << fis_conductor_time << " us)" << std::endl; } } // namespace hubero_local_planner ```

rayvburn commented 1 year ago

Note that all above timings are for system clock, not for ROS time that the move_base uses.