diff --git a/safety_planning_request_adapters/src/add_safety.cpp b/safety_planning_request_adapters/src/add_safety.cpp index 12b3004b8edcc1ff4b71adc1eaea90f91b01cb51..242134c3507a7c27ee49cb238061a67e5dc76b45 100644 --- a/safety_planning_request_adapters/src/add_safety.cpp +++ b/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