From e9e1db3b14ee749c4c8e8de439677623eada0d65 Mon Sep 17 00:00:00 2001
From: David Tiede <david.tiede@mailbox.tu-dresden.de>
Date: Wed, 5 May 2021 13:25:49 +0200
Subject: [PATCH] Algorithm to change velocity
---
.../src/add_safety.cpp | 41 +++++++++++++++----
1 file changed, 32 insertions(+), 9 deletions(-)
diff --git a/safety_planning_request_adapters/src/add_safety.cpp b/safety_planning_request_adapters/src/add_safety.cpp
index 12b3004..242134c 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
--
GitLab