add_safety.cpp

#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);