Skip to content
Snippets Groups Projects
Commit ee7db8db authored by David Tiede's avatar David Tiede
Browse files

Reformat

parent a9fd2668
No related branches found
No related tags found
No related merge requests found
#include <moveit/planning_request_adapter/planning_request_adapter.h>
#include <class_loader/class_loader.hpp> #include <class_loader/class_loader.hpp>
#include <moveit/planning_request_adapter/planning_request_adapter.h>
#include <ros/console.h> #include <ros/console.h>
namespace safety_planning_request_adapters namespace safety_planning_request_adapters
{ {
class AddSafety : public planning_request_adapter::PlanningRequestAdapter class AddSafety : public planning_request_adapter::PlanningRequestAdapter
{ {
int VELOCITY_LIMIT = 3; int VELOCITY_LIMIT = 3;
public: public:
std::string getDescription() const override std::string getDescription() const override
{ {
return "Add Safety"; return "Add Safety";
} }
bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
std::vector<std::size_t>& added_path_index) const override std::vector<std::size_t>& added_path_index) const override
{ {
ROS_ERROR("ADAPTER PACKAGE: ADAPTER LOADED"); ROS_ERROR("ADAPTER PACKAGE: Adapter loaded");
bool result = planner(planning_scene, req, res); bool result = planner(planning_scene, req, res);
if (result && res.trajectory_) // Only run after planning if (result && res.trajectory_) // Only run after planning
{ {
ROS_ERROR("ADAPTER PACKAGE: After planning");
// ROS_DEBUG("Running '%s'", getDescription().c_str()); // ROS_DEBUG("Running '%s'", getDescription().c_str());
// if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, // if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor,
// req.max_acceleration_scaling_factor)) // req.max_acceleration_scaling_factor))
...@@ -31,11 +32,11 @@ public: ...@@ -31,11 +32,11 @@ public:
// ROS_ERROR("Time parametrization for the solution path failed."); // ROS_ERROR("Time parametrization for the solution path failed.");
// result = false; // result = false;
// } // }
} }
return result; return result;
} }
}; };
} // namespace safety_planning_request_adapters } // namespace safety_planning_request_adapters
CLASS_LOADER_REGISTER_CLASS(safety_planning_request_adapters::AddSafety, planning_request_adapter::PlanningRequestAdapter); CLASS_LOADER_REGISTER_CLASS(safety_planning_request_adapters::AddSafety, planning_request_adapter::PlanningRequestAdapter);
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment