ArVizDrawer.cpp

Go to the documentation of this file.

#include "ArVizDrawer.h"
 
#include <algorithm>
#include <string>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/MathTools.h>
 
#include "RobotAPI/components/ArViz/Client/Client.h"
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
#include "RobotAPI/components/ArViz/Client/elements/Path.h"
#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
 
#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
 
#include "FeatureExtractor.h"
#include "conversions/eigen.h"
#include "conversions/pcl_eigen.h"
#include "laser_scanner_conversion.h"
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
 
    void
    ArVizDrawer::draw(const std::vector<Features>& features,
                      const std::string& frame,
                      const Eigen::Isometry3f& globalSensorPose)
    {
        if (parameters.drawCircles)
        {
            drawCircles(features, frame, globalSensorPose);
        }
 
        if (parameters.drawConvexHulls)
        {
            drawConvexHulls(features, frame, globalSensorPose);
        }
 
        if (parameters.drawEllipsoids)
        {
            drawEllipsoids(features, frame, globalSensorPose);
        }
 
        if (parameters.drawChains)
        {
            drawChains(features, frame, globalSensorPose);
        }
    }
 
    void
    ArVizDrawer::draw(const armem::laser_scans::LaserScanStamped& msg,
                      const Eigen::Isometry3f& globalSensorPose,
                      const simox::Color& color)
    {
        auto layer = arviz.layer("points_" + msg.header.frame);
 
        const auto pointCloud = conversions::eigen2pcl(toCartesian<Eigen::Vector3f>(msg.data));
 
        layer.add(viz::PointCloud("points_" + std::to_string(layer.size()))
                      .pointCloud(pointCloud, viz::Color(color))
                      .pointSizeInPixels(5)
                      .pose(globalSensorPose));
        arviz.commit(layer);
    }
 
    void
    ArVizDrawer::draw(const std::string& layerName,
                      const Circle& circle,
                      const Eigen::Isometry3f& robotGlobalPose,
                      const simox::Color& color)
    {
        auto layer = arviz.layer(layerName);
 
        drawCircle(layer, circle, robotGlobalPose, color);
        arviz.commit(layer);
    }
 
    void
    ArVizDrawer::drawCircle(viz::Layer& layer,
                            const Circle& circle,
                            const Eigen::Isometry3f& globalSensorPose,
                            const simox::Color& color)
    {
 
        const Eigen::Vector3f position =
            globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), 10.F);
 
        layer.add(viz::Ellipsoid("circle_" + std::to_string(layer.size()))
                      .axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
                      .position(position)
                      .color(color));
    }
 
    void
    ArVizDrawer::drawCircles(const std::vector<Features>& features,
                             const std::string& frame,
                             const Eigen::Isometry3f& globalSensorPose)
    {
        auto layer = arviz.layer("circles_" + frame);
 
        std::for_each(features.begin(),
                      features.end(),
                      [&](const Features& f)
                      {
                          if (not f.circle)
                          {
                              return;
                          }
                          drawCircle(
                              layer, *f.circle, globalSensorPose, simox::Color::red(200, 100));
                      });
 
        arviz.commit(layer);
    }
 
    void
    ArVizDrawer::drawConvexHulls(const std::vector<Features>& features,
                                 const std::string& frame,
                                 const Eigen::Isometry3f& globalSensorPose)
    {
        auto layer = arviz.layer("convex_hulls_" + frame);
 
        std::for_each(features.begin(),
                      features.end(),
                      [&](const Features& f)
                      {
                          if (not f.convexHull)
                          {
                              return;
                          }
                          drawConvexHull(
                              layer, *f.convexHull, globalSensorPose, simox::Color::red(100, 80));
                      });
 
        arviz.commit(layer);
    }
 
    void
    ArVizDrawer::draw(const std::string& layerName,
                      const VirtualRobot::MathTools::ConvexHull2D& robotHull,
                      const Eigen::Isometry3f& robotGlobalPose,
                      const simox::Color& color)
    {
        auto layer = arviz.layer(layerName);
 
        drawConvexHull(layer, robotHull, robotGlobalPose, color);
        arviz.commit(layer);
    }
 
    void
    ArVizDrawer::drawConvexHull(viz::Layer& layer,
                                const VirtualRobot::MathTools::ConvexHull2D& hull,
                                const Eigen::Isometry3f& globalSensorPose,
                                const simox::Color& color)
    {
        const auto points = conversions::to3D(hull.vertices);
 
        layer.add(viz::Polygon("convex_hull_" + std::to_string(layer.size()))
                      .points(points)
                      .color(color)
                      .pose(globalSensorPose));
    }
 
    void
    ArVizDrawer::drawEllipsoids(const std::vector<Features>& features,
                                const std::string& frame,
                                const Eigen::Isometry3f& globalSensorPose)
    {
        auto layer = arviz.layer("ellipsoids_" + frame);
 
        std::for_each(features.begin(),
                      features.end(),
                      [&](const Features& f)
                      {
                          if (not f.ellipsoid)
                          {
                              return;
                          }
                          drawEllipsoid(layer, *f.ellipsoid, globalSensorPose);
                      });
 
        arviz.commit(layer);
    }
 
    void
    ArVizDrawer::drawEllipsoid(viz::Layer& layer,
                               const Ellipsoid& ellipsoid,
                               const Eigen::Isometry3f& globalSensorPose)
    {
 
        const Eigen::Isometry3f pose = globalSensorPose * ellipsoid.pose;
 
        layer.add(viz::Ellipsoid("ellipsoid_" + std::to_string(layer.size()))
                      .axisLengths(conversions::to3D(ellipsoid.radii))
                      .pose(pose)
                      .color(simox::Color(255, 102, 0, 128))); // orange, but a bit more shiny
    }
 
    void
    ArVizDrawer::drawChains(const std::vector<Features>& features,
                            const std::string& frame,
                            const Eigen::Isometry3f& globalSensorPose)
    {
        auto layer = arviz.layer("chains_" + frame);
 
        std::for_each(features.begin(),
                      features.end(),
                      [&](const Features& f)
                      {
                          if (not f.chain)
                          {
                              return;
                          }
                          drawChain(layer, *f.chain, globalSensorPose);
 
                          // const auto ellipsoids = f.linesAsEllipsoids(50);
                          // for (const auto& ellipsoid : ellipsoids)
                          // {
                          //     drawEllipsoid(layer, ellipsoid, globalSensorPose);
                          // }
                      });
 
        arviz.commit(layer);
    }
 
    void
    ArVizDrawer::drawChain(viz::Layer& layer,
                           const Points& chain,
                           const Eigen::Isometry3f& globalSensorPose)
    {
        if (chain.size() < 2)
        {
            return;
        }
 
        const auto cloud = conversions::to3D(chain);
 
        layer.add(viz::Path("chain_" + std::to_string(layer.size()))
                      .points(cloud)
                      .width(7.F)
                      .color(viz::Color::blue())
                      .pose(globalSensorPose));
    }
} // namespace armarx::navigation::components::laser_scanner_feature_extraction