Format!

Author: sjahr

moveit_core/cost_functions/src/cost_functions.cpp

@@ -45,22 +45,22 @@ namespace cost_functions
   // Create cost function
   return [](const moveit::core::RobotState& robot_state, const planning_interface::MotionPlanRequest& request,
             const planning_scene::PlanningSceneConstPtr& planning_scene) {
-
-  std::vector<double> joint_pos;
-  robot_state.copyJointGroupPositions(request.group_name, joint_pos);
-  for(auto const val : joint_pos){
-    std::cout << "Joint pos: " << val << std::endl;
-  }
+    std::vector<double> joint_pos;
+    robot_state.copyJointGroupPositions(request.group_name, joint_pos);
+    for (auto const val : joint_pos)
+    {
+      std::cout << "Joint pos: " << val << std::endl;
+    }
     auto const shortest_distance_to_collision = planning_scene->distanceToCollisionUnpadded(robot_state);
 
     // Return cost based on shortest_distance if the robot is not in contact or penetrated a collision object
     if (shortest_distance_to_collision > 0.0)
     {
-      std::cout << "Cost: " << 1.0/ shortest_distance_to_collision << std::endl;
+      std::cout << "Cost: " << 1.0 / shortest_distance_to_collision << std::endl;
       // The closer the collision object the higher the cost
       return 1.0 / shortest_distance_to_collision;
     }
-    std::cout << "Cost: " <<  std::numeric_limits<double>::infinity() << std::endl;
+    std::cout << "Cost: " << std::numeric_limits<double>::infinity() << std::endl;
     return std::numeric_limits<double>::infinity();  // Return a max cost cost by default
   };
 }

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -378,8 +378,7 @@ void ompl_interface::ModelBasedPlanningContext::useConfig(const planning_scene::
 
   if (state_cost_function_ != nullptr)
   {
-    RCLCPP_WARN(LOGGER, "%s: Use user defined optimization function!",
-                  name_.c_str());
+    RCLCPP_WARN(LOGGER, "%s: Use user defined optimization function!", name_.c_str());
     objective = std::make_shared<ompl_interface::PlanningInterfaceObjective>(ompl_simple_setup_->getSpaceInformation(),
                                                                              planning_scene, req, state_cost_function_);
     ompl_simple_setup_->setOptimizationObjective(objective);