From 760884b6f67c8a39f817a23a35006e648c26fbef Mon Sep 17 00:00:00 2001
From: David Tiede <david.tiede@mailbox.tu-dresden.de>
Date: Wed, 12 May 2021 18:13:04 +0200
Subject: [PATCH] Update algorithm
---
.../src/add_safety.cpp | 24 +++++++------------
1 file changed, 9 insertions(+), 15 deletions(-)
diff --git a/safety_planning_request_adapters/src/add_safety.cpp b/safety_planning_request_adapters/src/add_safety.cpp
index 242134c..2b3089e 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
{
- 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;
};
};
--
GitLab