Re-format C++ files using clang-format

mir_dwb_critics/include/mir_dwb_critics/path_angle.h

@@ -54,12 +54,11 @@ namespace mir_dwb_critics
  * score if the error is large. The PathAlignCritic doesn't take the path orientation into account though,
  * so that's why the PathAngleCritic is a useful addition.
  */
-class PathAngleCritic: public dwb_local_planner::TrajectoryCritic
+class PathAngleCritic : public dwb_local_planner::TrajectoryCritic
 {
 public:
   virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
-                       const geometry_msgs::Pose2D& goal,
-                       const nav_2d_msgs::Path2D& global_plan) override;
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
 
   virtual double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
 

mir_dwb_critics/include/mir_dwb_critics/path_dist_pruned.h

@@ -45,11 +45,11 @@ namespace mir_dwb_critics
  *        where the global path is pruned to only include waypoints ahead of the
  *        robot.
  */
-class PathDistPrunedCritic : public dwb_critics::PathDistCritic {
+class PathDistPrunedCritic : public dwb_critics::PathDistCritic
+{
 public:
   virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
-                       const geometry_msgs::Pose2D& goal,
-                       const nav_2d_msgs::Path2D& global_plan) override;
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
 };
 
 }  // namespace mir_dwb_critics

mir_dwb_critics/include/mir_dwb_critics/path_progress.h

@@ -37,7 +37,8 @@
 #include <dwb_critics/map_grid.h>
 #include <vector>
 
-namespace mir_dwb_critics {
+namespace mir_dwb_critics
+{
 /**
  * @class PathProgressCritic
  * @brief Calculates an intermediate goal along the global path and scores trajectories on the distance to that goal.
@@ -47,26 +48,21 @@ namespace mir_dwb_critics {
  * path as long as the path continues to move in one direction (+/-
  * angle_threshold).
  */
-class PathProgressCritic : public dwb_critics::MapGridCritic {
- public:
-  bool prepare(const geometry_msgs::Pose2D &pose,
-               const nav_2d_msgs::Twist2D &vel,
-               const geometry_msgs::Pose2D &goal,
-               const nav_2d_msgs::Path2D &global_plan) override;
+class PathProgressCritic : public dwb_critics::MapGridCritic
+{
+public:
+  bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, const geometry_msgs::Pose2D& goal,
+               const nav_2d_msgs::Path2D& global_plan) override;
 
   void onInit() override;
   void reset() override;
   double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
 
- protected:
-  bool getGoalPose(const geometry_msgs::Pose2D &robot_pose,
-                   const nav_2d_msgs::Path2D &global_plan,
-                   unsigned int &x,
-                   unsigned int &y,
-                   double &desired_angle);
+protected:
+  bool getGoalPose(const geometry_msgs::Pose2D& robot_pose, const nav_2d_msgs::Path2D& global_plan, unsigned int& x,
+                   unsigned int& y, double& desired_angle);
 
-  unsigned int getGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan,
-                            unsigned int start_index,
+  unsigned int getGoalIndex(const std::vector<geometry_msgs::Pose2D>& plan, unsigned int start_index,
                             unsigned int last_valid_index) const;
 
   double xy_local_goal_tolerance_;

mir_dwb_critics/src/path_angle.cpp

@@ -38,52 +38,63 @@
 #include <nav_grid/coordinate_conversion.h>
 #include <vector>
 
-namespace mir_dwb_critics {
-
-bool PathAngleCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
-                              const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) {
-  const nav_core2::Costmap &costmap = *costmap_;
-  const nav_grid::NavGridInfo &info = costmap.getInfo();
+namespace mir_dwb_critics
+{
+bool PathAngleCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
+                              const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan)
+{
+  const nav_core2::Costmap& costmap = *costmap_;
+  const nav_grid::NavGridInfo& info = costmap.getInfo();
   nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
 
-  if (global_plan.poses.empty()) {
+  if (global_plan.poses.empty())
+  {
     ROS_ERROR_NAMED("PathAngleCritic", "The global plan was empty.");
     return false;
   }
 
   // find the angle of the plan at the pose on the plan closest to the robot
   double distance_min = std::numeric_limits<double>::infinity();
   bool started_path = false;
-  for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); i++) {
+  for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); i++)
+  {
     double g_x = adjusted_global_plan.poses[i].x;
     double g_y = adjusted_global_plan.poses[i].y;
     unsigned int map_x, map_y;
-    if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION) {
+    if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION)
+    {
       double distance = nav_2d_utils::poseDistance(adjusted_global_plan.poses[i], pose);
-      if (distance_min > distance) {
+      if (distance_min > distance)
+      {
         // still getting closer
         desired_angle_ = adjusted_global_plan.poses[i].theta;
         distance_min = distance;
         started_path = true;
-      } else {
+      }
+      else
+      {
         // plan is going away from the robot again
         break;
       }
-    } else if (started_path) {
+    }
+    else if (started_path)
+    {
       // Off the costmap after being on the costmap.
       break;
     }
     // else, we have not yet found a point on the costmap, so we just continue
   }
 
-  if (!started_path) {
+  if (!started_path)
+  {
     ROS_ERROR_NAMED("PathAngleCritic", "None of the points of the global plan were in the local costmap.");
     return false;
   }
   return true;
 }
 
-double PathAngleCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj) {
+double PathAngleCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
+{
   double diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI));
   return diff * diff;
 }

mir_dwb_critics/src/path_dist_pruned.cpp

@@ -36,50 +36,53 @@
 
 #include "mir_dwb_critics/path_dist_pruned.h"
 
-namespace mir_dwb_critics {
-
-bool PathDistPrunedCritic::prepare(
-    const geometry_msgs::Pose2D &pose,
-    const nav_2d_msgs::Twist2D &vel,
-    const geometry_msgs::Pose2D &goal,
-    const nav_2d_msgs::Path2D &global_plan) {
-
-  const nav_core2::Costmap &costmap = *costmap_;
-  const nav_grid::NavGridInfo &info = costmap.getInfo();
+namespace mir_dwb_critics
+{
+bool PathDistPrunedCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
+                                   const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan)
+{
+  const nav_core2::Costmap& costmap = *costmap_;
+  const nav_grid::NavGridInfo& info = costmap.getInfo();
 
   auto plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
 
-
   // --- stolen from PathProgressCritic::getGoalPose ---
   // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
   unsigned int start_index = 0;
   double distance_to_start = std::numeric_limits<double>::infinity();
   bool started_path = false;
-  for (unsigned int i = 0; i < plan.size(); i++) {
+  for (unsigned int i = 0; i < plan.size(); i++)
+  {
     double g_x = plan[i].x;
     double g_y = plan[i].y;
     unsigned int map_x, map_y;
-    if (worldToGridBounded(info, g_x, g_y, map_x, map_y)
-        && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) {
+    if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
+    {
       // Still on the costmap. Continue.
       double distance = nav_2d_utils::poseDistance(plan[i], pose);
-      if (distance_to_start > distance) {
+      if (distance_to_start > distance)
+      {
         start_index = i;
         distance_to_start = distance;
         started_path = true;
-      } else {
+      }
+      else
+      {
         // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
         // even closer to the robot, but then we would skip over parts of the plan.
         break;
       }
-    } else if (started_path) {
+    }
+    else if (started_path)
+    {
       // Off the costmap after being on the costmap.
       break;
     }
     // else, we have not yet found a point on the costmap, so we just continue
   }
 
-  if (!started_path) {
+  if (!started_path)
+  {
     ROS_ERROR_NAMED("PathDistPrunedCritic", "None of the points of the global plan were in the local costmap.");
     return false;
   }
@@ -89,9 +92,7 @@ bool PathDistPrunedCritic::prepare(
   // disregard that part in the PathDistCritic implementation.
   nav_2d_msgs::Path2D global_plan_pruned;
   global_plan_pruned.header = global_plan.header;
-  global_plan_pruned.poses = std::vector<geometry_msgs::Pose2D>(
-      plan.begin() + start_index,
-      plan.end());
+  global_plan_pruned.poses = std::vector<geometry_msgs::Pose2D>(plan.begin() + start_index, plan.end());
 
   return dwb_critics::PathDistCritic::prepare(pose, vel, goal, global_plan_pruned);
 }