Algorithm to change velocity

safety_planning_request_adapters/src/add_safety.cpp

@@ -6,7 +6,7 @@ namespace safety_planning_request_adapters
 {
     class AddSafety : public planning_request_adapter::PlanningRequestAdapter
     {
-        int VELOCITY_LIMIT = 3;
+        double VELOCITY_LIMIT = 1;
 
     public:
 
@@ -25,17 +25,40 @@ namespace safety_planning_request_adapters
             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))
-//          {
-//              ROS_ERROR("Time parametrization for the solution path failed.");
-//              result = false;
-//          }
+
+                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