Update algorithm

safety_planning_request_adapters/src/add_safety.cpp

@@ -6,7 +6,7 @@ namespace safety_planning_request_adapters
 {
     class AddSafety : public planning_request_adapter::PlanningRequestAdapter
     {
-        double VELOCITY_LIMIT = 1;
+        double VELOCITY_LIMIT = 0.1;
 
     public:
 
@@ -24,7 +24,7 @@ namespace safety_planning_request_adapters
             bool result = planner(planning_scene, req, res);
             if (result && res.trajectory_) // Only run after planning
             {
-                ROS_ERROR("ADAPTER PACKAGE: After planning");
+                ROS_ERROR("ADAPTER PACKAGE: Start plugin");
 
                 robot_trajectory::RobotTrajectory& trajectory = *res.trajectory_;
                 const robot_model::JointModelGroup* group = trajectory.getGroup();
@@ -38,25 +38,19 @@ namespace safety_planning_request_adapters
                 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) {
 
-                for (size_t p = 0; p <= num_points; ++p) {
-                    ROS_ERROR("ADAPTER PACKAGE: %zu",  p);
-                    robot_state::RobotState waypoint = trajectory.getWayPoint(p);
+                    robot_state::RobotStatePtr waypoint = trajectory.getWayPointPtr(p);
+                    trajectory.setWayPointDurationFromPrevious(p, trajectory.getWayPointDurationFromPrevious(p) / VELOCITY_LIMIT);
+                    //ROS_WARN("ADAPTER PACKAGE: vel1: %f",  waypoint->getVariableVelocity(idx[0]));
 
                     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);
+                        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);
                 }
             }
-
+            ROS_ERROR("ADAPTER PACKAGE: End plugin");
             return result;
         };
     };