Computational efficiency of a global path planner

[rayvburn]

Currently, only the local trajectory planner can be evaluated with $m_{cef}$.

[rayvburn]

The global path planner usually operates with a different frequency than the local trajectory planner. The most obvious approach is to:

• create a GlobalPlannerData class that stores a duration of path planning procedure
• create an update overloaded method in the RobotLogger class that will mimic the behaviour of RobotLogger::update(timestamp, robot) but for the global path planning
• let the RobotLogger save global path planning data into a separate file
• in the srpb_move_base:

  diff --git a/src/move_base.cpp b/src/move_base.cpp
  index 64d65804..54dc1f32 100644
  --- a/src/move_base.cpp
  +++ b/src/move_base.cpp
  @@ -498,8 +498,27 @@ namespace move_base {
   
       const geometry_msgs::PoseStamped& start = global_pose;
   
  +    //start timing
  +    const auto start_t = ros::Time::now();
  +
       //if the planner fails or returns a zero length plan, planning failed
  -    if(!planner_->makePlan(start, goal, plan) || plan.empty()){
  +    bool planning_success = planner_->makePlan(start, goal, plan) && !plan.empty();
  +
  +    //end timing
  +    const auto end_t = ros::Time::now();
  +    const auto time_diff = (end_t - start_t).toSec();
  +
  +    robot_logger_.update(
  +      start_t.toSec(),
  +      srpb::logger::GlobalPlannerData(
  +        planning_success,
  +        robot_logger_.transformPose(start),
  +        robot_logger_.transformPose(goal),
  +        time_diff
  +      )
  +    );
  +
  +    if (!planning_success) {
         ROS_DEBUG_NAMED("move_base","Failed to find a  plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
         return false;
       }

[rayvburn]

The logger package has been updated with the necessary changes. The evaluation package needs some adjustments now

[rayvburn]

A proper metric needs to be created in evaluation package