From 8caceb367803a5813e1d92b34cf66d40f08450c6 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 26 Jul 2022 09:23:13 +0200
Subject: [PATCH] TimedElasticBands: fixing build due to api changes
---
.../local_planning/TimedElasticBands.cpp | 21 ++++++++++++-------
.../local_planning/TimedElasticBands.h | 3 ++-
2 files changed, 15 insertions(+), 9 deletions(-)
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index c3fdb8ae..7ebf586f 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -8,7 +8,7 @@
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/local_planning/core.h>
#include <armarx/navigation/local_planning/ros_conversions.h>
-#include <teb_local_planner/extension/obstacles/EllipseObstacle.h>
+// #include <teb_local_planner/extension/obstacles/EllipseObstacle.h>
#include <teb_local_planner/homotopy_class_planner.h>
namespace armarx::navigation::loc_plan
@@ -92,14 +92,19 @@ namespace armarx::navigation::loc_plan
const teb_local_planner::PoseSE2 end = conv::toRos(target.target);
core::Twist currentVelocity = core::Twist::Zero();
- if (scene.platformVelocity.has_value())
- {
- currentVelocity = scene.platformVelocity.value();
- }
+
+ // FIXME: again, add to scene
+ // if (scene.platformVelocity.has_value())
+ // {
+ // currentVelocity = scene.platformVelocity.value();
+ // }
geometry_msgs::Twist velocity_start = conv::toRos(currentVelocity);
//TODO (SALt): fill obstacles
+ // TODO (SALt): implement
+ bool planToDest = true;
+
try
{
hcp_->plan(start, end, &velocity_start, !planToDest);
@@ -112,14 +117,14 @@ namespace armarx::navigation::loc_plan
ARMARX_VERBOSE << "Planned successfully (found " << hcp_->getTrajectoryContainer().size()
<< " Trajectories)";
- core::Trajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
+ core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
return {{.trajectory = best}};
}
TimedElasticBands::FindTargetResult
- TimedElasticBands::findTarget(const core::Pose& currentPose, const core::Trajectory& globalPath)
+ TimedElasticBands::findTarget(const core::Pose& currentPose, const core::GlobalTrajectory& globalPath)
{
float distance = std::numeric_limits<float>::max();
@@ -181,7 +186,7 @@ namespace armarx::navigation::loc_plan
}
}
- return {projection, globalPath.points().end()[0].waypoint.pose.translation(), true};
+ return {projection, globalPath.points().back().waypoint.pose.translation(), true};
}
} // namespace armarx::navigation::loc_plan
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 99b2ae44..0197b949 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -66,13 +66,14 @@ namespace armarx::navigation::loc_plan
void initConfig();
FindTargetResult findTarget(const core::Pose& currentPose,
- const core::Trajectory& globalPath);
+ const core::GlobalTrajectory& globalPath);
protected:
const Params params;
private:
const core::Scene& scene;
+
teb_local_planner::TebConfig cfg_;
teb_local_planner::ObstContainer teb_obstacles;
teb_local_planner::PoseSequence teb_globalPath;
--
GitLab