Skip to content
Snippets Groups Projects
Commit adbaed04 authored by Johannes Mey's avatar Johannes Mey
Browse files

fix treatment of non-looping trajectories

parent 8078bebe
Branches
No related tags found
No related merge requests found
...@@ -114,7 +114,7 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning ...@@ -114,7 +114,7 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning
loadWaypoints(); loadWaypoints();
while (currentTrajectory.loop) { do {
for (auto &waypoint : currentTrajectory.waypoints) { for (auto &waypoint : currentTrajectory.waypoints) {
moveit::planning_interface::MoveGroupInterface::Plan plan; moveit::planning_interface::MoveGroupInterface::Plan plan;
...@@ -147,7 +147,9 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning ...@@ -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); "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) { void TimedPlanner::newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment