From e90e60f6ea4871aedfca2c5d6ddc81d2959a616b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 15 May 2023 11:42:40 +0200 Subject: [PATCH] Format! --- .../cost_functions/src/cost_functions.cpp | 16 ++++++++-------- .../src/model_based_planning_context.cpp | 3 +-- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/moveit_core/cost_functions/src/cost_functions.cpp b/moveit_core/cost_functions/src/cost_functions.cpp index eecdb0d431c..ea6e4ef2659 100644 --- a/moveit_core/cost_functions/src/cost_functions.cpp +++ b/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 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 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::infinity() << std::endl; + std::cout << "Cost: " << std::numeric_limits::infinity() << std::endl; return std::numeric_limits::infinity(); // Return a max cost cost by default }; } diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index f1f9c93fb18..6ac2d51aca0 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/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_simple_setup_->getSpaceInformation(), planning_scene, req, state_cost_function_); ompl_simple_setup_->setOptimizationObjective(objective);