Merge branch 'feature/teb_local_planner' into 'dev'

Add TEB to CMake config

See merge request ArmarX/skills/navigation!27

CMakeLists.txt

@@ -32,6 +32,9 @@ armarx_find_package(PUBLIC VTK QUIET)
 armarx_find_package(PUBLIC SemanticObjectRelations QUIET)
 armarx_find_package(PUBLIC OpenCV QUIET)  # Required as RobotAPI is a legacy project.
 armarx_find_package(PUBLIC range-v3 QUIET)
+# human aware navigation
+armarx_find_package(PUBLIC teb_local_planner QUIET)
+armarx_find_package(PUBLIC teb_extension QUIET)
 
 add_subdirectory(etc)
 

source/armarx/navigation/local_planning/CMakeLists.txt

 
 armarx_add_library(local_planning
-    DEPENDENCIES
+    DEPENDENCIES_PUBLIC
         ArmarXCoreInterfaces
         ArmarXCore
         armarx_navigation::core
+        armarx_navigation::conversions
+        # teb_extension::obstacles
+    DEPENDENCIES_PRIVATE
+        range-v3::range-v3
+    DEPENDENCIES_LEGACY
+        teb_local_planner
     SOURCES 
         ./LocalPlanner.cpp
         ./TimedElasticBands.cpp
+        ./ros_conversions.cpp
     HEADERS
         ./LocalPlanner.h
         ./TimedElasticBands.h
+        ./ros_conversions.h
         core.h
 )

source/armarx/navigation/local_planning/TimedElasticBands.cpp

 #include "TimedElasticBands.h"
 
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/core/Trajectory.h"
 #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/homotopy_class_planner.h>
 
 namespace armarx::navigation::loc_plan
 {
@@ -29,14 +39,100 @@ namespace armarx::navigation::loc_plan
     // TimedElasticBands
 
     TimedElasticBands::TimedElasticBands(const Params& params, const core::Scene& ctx) :
-        LocalPlanner(ctx), params(params)
+        LocalPlanner(ctx), params(params), scene(ctx)
+    {
+        //TODO (SALt): init configuration, robot footprint
+        initConfig();
+        auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(0.2);
+
+        hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
+        hcp_->initialize(
+            cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);
+
+        ros::Time::init(); // we have to init time before we can use the planner
+    }
+
+    void
+    TimedElasticBands::initConfig()
     {
+        cfg_.robot.max_vel_x = 1.0;
+        cfg_.robot.max_vel_x_backwards = 1.0;
+        cfg_.robot.max_vel_y = 1.0;
+        cfg_.robot.max_vel_theta = 1.0;
+        cfg_.robot.acc_lim_x = 1.0;
+        cfg_.robot.acc_lim_y = 1.0;
+        cfg_.robot.acc_lim_theta = 1.0;
+
+        cfg_.optim.weight_max_vel_x = 1.0;
+        cfg_.optim.weight_max_vel_y = 1.0;
+        cfg_.optim.weight_max_vel_theta = 1.0;
+        cfg_.optim.weight_acc_lim_x = 1.0;
+        cfg_.optim.weight_acc_lim_y = 1.0;
+        cfg_.optim.weight_acc_lim_theta = 1.0;
+
+        cfg_.optim.weight_optimaltime = 0.6;
+        cfg_.optim.weight_shortest_path = 0.6;
+
+        cfg_.hcp.selection_cost_hysteresis = 1.0;
+        cfg_.hcp.switching_blocking_period = 0;
+
+        cfg_.pse.weight_global_path = 0.25;
     }
 
     std::optional<LocalPlannerResult>
     TimedElasticBands::plan(const core::GlobalTrajectory& goal)
     {
-        // TODO implement
-        return {};
+        const teb_local_planner::TimedElasticBand globalPath = conv::toRos(goal);
+        teb_globalPath = globalPath.poses();
+        hcp_->setGlobalPath(&teb_globalPath);
+
+        const core::Pose currentPose{scene.robot->getGlobalPose()};
+
+        const teb_local_planner::PoseSE2 start = conv::toRos(currentPose);
+
+        const FindTargetResult target = findTarget(currentPose, goal);
+        const teb_local_planner::PoseSE2 end = conv::toRos(target.target);
+
+        core::Twist currentVelocity = core::Twist::Zero();
+        
+        // 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);
+        }
+        catch (std::exception& e)
+        {
+            ARMARX_DEBUG << "Caugth exception while planning: " << e.what();
+            return {};
+        }
+        ARMARX_VERBOSE << "Planned successfully (found " << hcp_->getTrajectoryContainer().size()
+                       << " Trajectories)";
+
+        core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
+
+        return {{.trajectory = best}};
     }
+
+
+    TimedElasticBands::FindTargetResult
+    TimedElasticBands::findTarget(const core::Pose& currentPose, const core::GlobalTrajectory& globalPath)
+    {
+
+        const core::Projection projection = globalPath.getProjection(
+            currentPose.translation(), core::VelocityInterpolation::LinearInterpolation);
+
+        return {conv::to2D(projection.projection.waypoint.pose), conv::to2D(globalPath.points().back().waypoint.pose), true};
+    }
+
 } // namespace armarx::navigation::loc_plan

source/armarx/navigation/local_planning/TimedElasticBands.h

@@ -24,9 +24,12 @@
 
 #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
+#include "armarx/navigation/core/basic_types.h"
 #include <armarx/navigation/core/Trajectory.h>
 #include <armarx/navigation/local_planning/LocalPlanner.h>
 #include <armarx/navigation/local_planning/core.h>
+#include <teb_local_planner/homotopy_class_planner.h>
+#include <teb_local_planner/teb_config.h>
 
 namespace armarx::navigation::loc_plan
 {
@@ -54,9 +57,27 @@ namespace armarx::navigation::loc_plan
 
         std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
 
+    private:
+        struct FindTargetResult
+        {
+            core::Pose2D projection;
+            core::Pose2D target;
+            bool planToDestination;
+        };
+
+        void initConfig();
+        FindTargetResult findTarget(const core::Pose& currentPose,
+                                    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;
+        std::unique_ptr<teb_local_planner::HomotopyClassPlanner> hcp_{nullptr};
     };
 } // namespace armarx::navigation::loc_plan

source/armarx/navigation/local_planning/ros_conversions.cpp

+#include "ros_conversions.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/Duration.h"
+
+#include "armarx/navigation/core/Trajectory.h"
+#include "armarx/navigation/core/basic_types.h"
+#include <armarx/navigation/conversions/eigen.h>
+
+#include <range/v3/view/zip.hpp>
+
+namespace armarx::navigation::conv
+{
+    teb_local_planner::PoseSE2
+    toRos(const core::Pose& pose)
+    {
+        core::Pose2D pose2D = to2D(pose);
+        return toRos(pose2D);
+    }
+
+    teb_local_planner::PoseSE2
+    toRos(const core::Pose2D& pose)
+    {
+        Eigen::Vector2d pos = pose.translation().cast<double>();
+        double theta = Eigen::Rotation2Df(pose.linear()).angle();
+
+        return {pos, theta};
+    }
+
+
+    core::Pose
+    fromRos(const teb_local_planner::PoseSE2& pose)
+    {
+        Eigen::Vector2d pos = pose.position();
+        double theta = pose.theta();
+
+        core::Pose2D ret;
+        ret.translation() = pos.cast<float>();
+        ret.linear() = Eigen::Rotation2Df(theta).toRotationMatrix();
+
+        return to3D(ret);
+    }
+
+    geometry_msgs::Twist
+    toRos(const core::Twist& velocity)
+    {
+        geometry_msgs::Twist ret;
+        ret.linear.x = velocity.linear.x();
+        ret.linear.y = velocity.linear.y();
+        ret.angular.z = velocity.angular.z();
+
+        return ret;
+    }
+
+    teb_local_planner::TimedElasticBand
+    toRos(const core::GlobalTrajectory& trajectory)
+    {
+        teb_local_planner::TimedElasticBand teb;
+
+        bool first = true;
+        teb_local_planner::PoseSE2 lastPose;
+        for (const core::GlobalTrajectoryPoint& point : trajectory.points())
+        {
+            teb_local_planner::PoseSE2 pose = toRos(point.waypoint.pose);
+            teb.addPose(pose);
+
+            if (!first)
+            {
+                Eigen::Vector2d distance = pose.position() - lastPose.position();
+                double timeDiff = distance.norm() / point.velocity;
+                teb.addTimeDiff(timeDiff);
+
+                first = false;
+            }
+
+            lastPose = pose;
+        }
+
+        return teb;
+    }
+
+    core::LocalTrajectory
+    fromRos(const teb_local_planner::TimedElasticBand& teb)
+    {
+        core::LocalTrajectory trajectory;
+        trajectory.reserve(teb.poses().size());
+
+        // TODO this timestamp should be given via the argument list
+        DateTime timestamp = Clock::Now();
+
+        ARMARX_CHECK_EQUAL(teb.poses().size(), teb.timediffs().size());
+
+        for (const auto& [poseVertex, timediff]: ranges::views::zip(teb.poses(), teb.timediffs()))
+        {
+            const core::Pose pose = fromRos(poseVertex->pose());
+            const Duration dt = Duration::SecondsDouble(timediff->dt());
+            timestamp += dt;
+
+            trajectory.push_back(core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
+        }
+
+        return trajectory;
+    }
+
+
+} // namespace armarx::navigation::conv

source/armarx/navigation/local_planning/ros_conversions.h

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/core/types.h>
+#include <geometry_msgs/Twist.h>
+#include <teb_local_planner/pose_se2.h>
+#include <teb_local_planner/timed_elastic_band.h>
+
+namespace armarx::navigation::conv
+{
+
+    teb_local_planner::PoseSE2 toRos(const core::Pose& pose);
+    teb_local_planner::PoseSE2 toRos(const core::Pose2D& pose);
+
+    core::Pose fromRos(const teb_local_planner::PoseSE2& pose);
+
+    geometry_msgs::Twist toRos(const core::Twist& velocity);
+
+    teb_local_planner::TimedElasticBand toRos(const core::GlobalTrajectory& trajectory);
+
+    core::LocalTrajectory fromRos(const teb_local_planner::TimedElasticBand& teb);
+
+
+} // namespace armarx::navigation::conv