grasp_candidate_drawer.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Dense>
 
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
 
#include "GraspCandidateHelper.h"
 
#include "box_to_grasp_candidates.h"
 
namespace armarx
{
    struct grasp_candidate_drawer
    {
    private:
        struct side_data
        {
            std::string name;
            std::string hand_model_package;
            std::string hand_model_path;
            Eigen::Matrix4f tcp_2_hroot;
        };
    public:
        grasp_candidate_drawer(armarx::RobotNameHelperPtr rnh,
                               const VirtualRobot::RobotPtr& robot);
        void reset_colors();
    public:
        void draw(const armarx::grasping::GraspCandidatePtr& gc, armarx::viz::Layer& l);
        void draw(const armarx::GraspCandidateHelper& gch,       armarx::viz::Layer& l);
        void draw(const armarx::grasping::GraspCandidatePtr& gc, armarx::viz::Layer& l, const armarx::viz::Color& color);
        void draw(const armarx::GraspCandidateHelper& gch,       armarx::viz::Layer& l, const armarx::viz::Color& color);
    public:
        void draw(const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
                  float ax_len = 300, float ax_width = 5,
                  const armarx::viz::Color& color_up    = {255, 0, 0},
                  const armarx::viz::Color& color_trans = {0, 255, 0});
        void draw(const std::string& name,
                  const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
                  float ax_len = 300, float ax_width = 5,
                  const armarx::viz::Color& color_up    = {255, 0, 0},
                  const armarx::viz::Color& color_trans = {0, 255, 0});
        void draw(const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
                  const Eigen::Matrix4f& gpose,
                  float ax_len = 300, float ax_width = 5,
                  const armarx::viz::Color& color_up    = {255, 0, 0},
                  const armarx::viz::Color& color_trans = {0, 255, 0});
        void draw(const std::string& name,
                  const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
                  const Eigen::Matrix4f& gpose,
                  float ax_len = 300, float ax_width = 5,
                  const armarx::viz::Color& color_up    = {255, 0, 0},
                  const armarx::viz::Color& color_trans = {0, 255, 0});
    private:
        const side_data& side(const armarx::GraspCandidateHelper& gch);
 
    public:
        armarx::RobotNameHelperPtr rnh;
        VirtualRobot::RobotPtr robot;
        float length_approach = 300;
        float width_approach = 5;
    private:
        std::map<std::string, side_data> _sides;
        std::size_t _color_offset = 0;
    };
}