From fd4655a24ff22df987b6eac5d5b368148e9954a9 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 27 Jul 2022 21:36:56 +0200
Subject: [PATCH] fix: convert trajectory
---
.../local_planning/ros_conversions.cpp | 22 +++++++++++++++----
1 file changed, 18 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 2742d26b..a591346c 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -7,6 +7,7 @@
#include "armarx/navigation/core/basic_types.h"
#include <armarx/navigation/conversions/eigen.h>
+#include <range/v3/view/drop.hpp>
#include <range/v3/view/zip.hpp>
namespace armarx::navigation::conv
@@ -82,23 +83,36 @@ namespace armarx::navigation::conv
core::LocalTrajectory
fromRos(const teb_local_planner::TimedElasticBand& teb)
{
+ const auto& tebPoses = teb.poses();
+
+ if(tebPoses.empty())
+ {
+ return {};
+ }
+
core::LocalTrajectory trajectory;
- trajectory.reserve(teb.poses().size());
+ trajectory.reserve(tebPoses.size());
// TODO this timestamp should be given via the argument list
DateTime timestamp = Clock::Now();
- ARMARX_CHECK_EQUAL(teb.poses().size(), teb.timediffs().size());
+ ARMARX_CHECK_EQUAL(tebPoses.size(), teb.timediffs().size()-1);
+
+ // add the first pose at this timestamp
+ trajectory.push_back(core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {fromRos(tebPoses.front()->pose())}});
- for (const auto& [poseVertex, timediff]: ranges::views::zip(teb.poses(), teb.timediffs()))
+ // all consecutive poses
+ for (const auto& [poseVertex, timediff]: ranges::views::zip(tebPoses | ranges::views::drop(1), teb.timediffs()))
{
const core::Pose pose = fromRos(poseVertex->pose());
const Duration dt = Duration::SecondsDouble(timediff->dt());
- timestamp += dt;
+ timestamp += dt;
trajectory.push_back(core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
}
+ ARMARX_CHECK_EQUAL(trajectory.size(), tebPoses.size());
+
return trajectory;
}
--
GitLab