hack, please investigate

CentralLabFacilities · Jul 9, 2024 · 1b3081f · 1b3081f
1 parent abbe0b7
commit 1b3081f
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
@@ -166,6 +166,14 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory);
 		exec_traj.controller_names_ = sub_traj.execution_info.controller_names;
 
+		for (int i = 1; i < exec_traj.trajectory_->getWayPointCount(); i++) {
+			auto from_prev = exec_traj.trajectory_->getWayPointDurationFromPrevious(i);
+			if(from_prev < 0.0001) {
+				ROS_ERROR_NAMED("ExecuteTaskSolution", "Found broken waypoint? fixing...");
+				exec_traj.trajectory_->setWayPointDurationFromPrevious(i, 0.0001);
+			}
+		}
+
 		/* TODO add action feedback and markers */
 		exec_traj.effect_on_success_ = [this,
 		                                &scene_diff = const_cast<::moveit_msgs::PlanningScene&>(sub_traj.scene_diff),