-
David Tiede authoredDavid Tiede authored
add_safety.cpp 2.87 KiB
#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
{
double VELOCITY_LIMIT = 1;
public:
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 result = planner(planning_scene, req, res);
if (result && res.trajectory_) // Only run after planning
{
ROS_ERROR("ADAPTER PACKAGE: After planning");
robot_trajectory::RobotTrajectory& trajectory = *res.trajectory_;
const robot_model::JointModelGroup* group = trajectory.getGroup();
if (!group)
{
ROS_ERROR("ADAPTER PACKAGE: It looks like the planner did not set the group the plan was computed for");
return false;
}
const std::vector<int>& idx = group->getVariableIndexList();
const unsigned num_joints = group->getVariableCount();
const unsigned num_points = trajectory.getWayPointCount();
std::list<Eigen::VectorXd> points;
double duration_from_start = 0;
for (size_t p = 0; p <= num_points; ++p) {
ROS_ERROR("ADAPTER PACKAGE: %zu", p);
robot_state::RobotState waypoint = trajectory.getWayPoint(p);
for (size_t j = 0; j < num_joints; ++j) {
waypoint.setVariableVelocity(idx[j], waypoint.getVariableVelocity(idx[j]) * VELOCITY_LIMIT);
waypoint.setVariableAcceleration(idx[j], waypoint.getVariableAcceleration(idx[j]) * VELOCITY_LIMIT * VELOCITY_LIMIT);
}
ROS_ERROR("ADAPTER PACKAGE: Previous %f", trajectory.getWayPointDurationFromPrevious(p));
duration_from_start = duration_from_start + (trajectory.getWayPointDurationFromPrevious(p) / VELOCITY_LIMIT);
ROS_ERROR("ADAPTER PACKAGE: Previous %f", duration_from_start);
trajectory.insertWayPoint(p, waypoint, duration_from_start);
}
}
return result;
};
};
} // namespace safety_planning_request_adapters
CLASS_LOADER_REGISTER_CLASS(safety_planning_request_adapters::AddSafety, planning_request_adapter::PlanningRequestAdapter);