From 7f6fc1b4004dbd11ed9e740bf82ec93497523bab Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 25 Jul 2022 14:42:28 +0200
Subject: [PATCH] TrajectoryFollowingController: if trajectory is empty: don't
move
---
.../trajectory_control/TrajectoryFollowingController.cpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 17431d8e..2ae7f289 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -131,6 +131,14 @@ namespace armarx::navigation::traj_ctrl
{
ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
+ if (trajectory.points().empty())
+ {
+ ARMARX_INFO << "Trajectory is empty.";
+ return TrajectoryControllerResult{
+ .twist = core::Twist::Zero(),
+ .dropPoint = {.waypoint = {.pose = core::Pose::Identity()}, .velocity = 0}};
+ }
+
const core::Pose currentPose(robot->getGlobalPose());
const auto projectedPose = trajectory.getProjection(
--
GitLab