diff --git a/safety_planning_request_adapters/src/add_safety.cpp b/safety_planning_request_adapters/src/add_safety.cpp index 876e38be6deb6ba04ab4111546e66a047d11c052..12b3004b8edcc1ff4b71adc1eaea90f91b01cb51 100644 --- a/safety_planning_request_adapters/src/add_safety.cpp +++ b/safety_planning_request_adapters/src/add_safety.cpp @@ -1,29 +1,30 @@ -#include <moveit/planning_request_adapter/planning_request_adapter.h> #include <class_loader/class_loader.hpp> +#include <moveit/planning_request_adapter/planning_request_adapter.h> #include <ros/console.h> namespace safety_planning_request_adapters { -class AddSafety : public planning_request_adapter::PlanningRequestAdapter -{ - int VELOCITY_LIMIT = 3; + class AddSafety : public planning_request_adapter::PlanningRequestAdapter + { + int VELOCITY_LIMIT = 3; -public: + public: - std::string getDescription() const override - { - return "Add Safety"; - } + std::string getDescription() const override + { + return "Add Safety"; + } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector<std::size_t>& added_path_index) const override - { - ROS_ERROR("ADAPTER PACKAGE: ADAPTER LOADED"); + bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, + std::vector<std::size_t>& added_path_index) const override + { + ROS_ERROR("ADAPTER PACKAGE: Adapter loaded"); - bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) // Only run after planning - { + bool result = planner(planning_scene, req, res); + if (result && res.trajectory_) // Only run after planning + { + ROS_ERROR("ADAPTER PACKAGE: After planning"); // ROS_DEBUG("Running '%s'", getDescription().c_str()); // if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, // req.max_acceleration_scaling_factor)) @@ -31,11 +32,11 @@ public: // ROS_ERROR("Time parametrization for the solution path failed."); // result = false; // } - } + } - return result; - } -}; + return result; + } + }; } // namespace safety_planning_request_adapters CLASS_LOADER_REGISTER_CLASS(safety_planning_request_adapters::AddSafety, planning_request_adapter::PlanningRequestAdapter);