From adbaed04e66b69712761dc8981f4179899357026 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Wed, 15 Jul 2020 13:51:49 +0200
Subject: [PATCH] fix treatment of non-looping trajectories
---
src/TimedPlanner.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index 91c3d37..6cc4cfb 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -114,7 +114,7 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
loadWaypoints();
- while (currentTrajectory.loop) {
+ do {
for (auto &waypoint : currentTrajectory.waypoints) {
moveit::planning_interface::MoveGroupInterface::Plan plan;
@@ -147,7 +147,9 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
"Unable to compute " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint);
}
}
- }
+ } while (currentTrajectory.loop);
+
+ currentTrajectory.waypoints.clear(); // the trajectory is completed and can be cleared
}
void TimedPlanner::newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {
--
GitLab