TebObstacleManager.cpp

Go to the documentation of this file.

#include "TebObstacleManager.h"
 
#include <SimoxUtility/algorithm/apply.hpp>
#include <VirtualRobot/BoundingBox.h>
 
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/local_planning/ros_conversions.h>
 
#include <teb_local_planner/extension/obstacles/EllipseObstacle.h>
#include <teb_local_planner/obstacles.h>
 
namespace armarx::navigation::local_planning
{
 
    void
    TebObstacleManager::clear()
    {
        container.clear();
        visualizationIndex = 0;
    }
 
    size_t
    TebObstacleManager::size()
    {
        return container.size();
    }
 
    void
    TebObstacleManager::addLaserScannerFeaturesObstacle(
        const memory::LaserScannerFeatures& features,
        viz::Layer* visLayer)
    {
 
        Eigen::Isometry3f global_T_frame = features.frameGlobalPose;
        for (const auto& feature : features.features)
        {
            std::vector<Eigen::Vector2f> convexHull =
                simox::alg::apply(feature.convexHull,
                                  [&](const Eigen::Vector2f& point)
                                  {
                                      Eigen::Vector3f point3d = conv::to3D(point);
                                      return conv::to2D(global_T_frame * point3d);
                                  });
 
            addPolygonObstacle(convexHull, visLayer);
        }
    }
 
    void
    TebObstacleManager::addPolygonObstacle(const Polygon& polygon, viz::Layer* visLayer)
    {
        auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
 
        for (const auto& vertex : polygon)
        {
            obst->pushBackVertex(conv::toRos2D(vertex));
        }
        obst->finalizePolygon();
        container.push_back(obst);
 
        // visualize bounding box if layer is available
        if (visLayer != nullptr)
        {
            visLayer->add(viz::Polygon("polygon_" + std::to_string(visualizationIndex++))
                              .points(conv::to3D(polygon))
                              .color(simox::Color::gray()));
        }
    }
 
    void
    TebObstacleManager::addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer)
    {
        const Eigen::Vector2f min = conv::to2D(bbox.getMin());
        const Eigen::Vector2f max = conv::to2D(bbox.getMax());
 
        Polygon poly{};
        poly.push_back(min);
        poly.emplace_back(min.x(), max.y());
        poly.push_back(max);
        poly.emplace_back(max.x(), min.y());
 
        addPolygonObstacle(poly, visLayer);
    }
 
    void
    TebObstacleManager::addHumanObstacle(const human::Human& human, viz::Layer* visLayer)
    {
        auto proxemicZones = proxemics.createProxemicZones(human);
 
        int i = 0;
        for (const auto& proxemicZone : proxemicZones)
        {
            auto pose = conv::toRos(proxemicZone.pose);
            auto shape = conv::toRos(proxemicZone.shape);
 
            auto obst = boost::make_shared<teb_local_planner::extension::EllipseObstacle>(
                pose.position(), pose.theta(), shape.a, shape.b);
 
            const auto& penalty = proxemicZone.penalty;
 
            obst->setPenaltyModel(conv::toRos(penalty));
 
            obst->setWeight(proxemicZone.weight);
            obst->setHomotopicRelevance(proxemicZone.homotopicRelevance);
 
            if (human.linearVelocity != Eigen::Vector2f::Zero())
            {
                obst->setCentroidVelocity(conv::toRos2D(human.linearVelocity));
            }
 
            container.push_back(obst);
 
            // visualize proxemic zone if layer is available
            if (visLayer != nullptr)
            {
                const Eigen::Vector3f axisLength(
                    proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
                const core::Pose pose3d = conv::to3D(proxemicZone.pose);
 
                visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))
                                  .pose(pose3d)
                                  .axisLengths(axisLength)
                                  .color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
            }
            i++;
        }
    }
 
} // namespace armarx::navigation::local_planning