MarkerSelector.h

Go to the documentation of this file.

#ifndef URDF2GRASPIT_MARKERSELECTOR_H
#define URDF2GRASPIT_MARKERSELECTOR_H
// Copyright Jennifer Buehler

#include <urdf_viewer/InventorViewer.h>

#include <Eigen/Geometry>

#include <vector>
#include <map>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

namespace urdf2graspit
{
namespace markerselector
{
class MarkerSelector: public urdf_viewer::InventorViewer
{
public:
    class Marker
    {
    public:
        Marker(int id): markerID(id) {}
        Marker(const Marker& m):
            linkName(m.linkName),
            visualNum(m.visualNum),
            coords(m.coords),
            normal(m.normal),
            markerID(m.markerID) {}

        ~Marker() {}


        friend std::ostream& operator<<(std::ostream& o, const Marker& m)
        {
            o << m.linkName << "; visual " << m.visualNum << "; Coords " << m.coords
              << "; normal " << m.normal;
            return o;
        }

        void setCoords(const double x, const double y, const double z)
        {
            coords = Eigen::Vector3d(x, y, z);
        }
        void setNormal(const double x, const double y, const double z)
        {
            normal = Eigen::Vector3d(x, y, z);
        }

        static std::string toSoBaseName(const std::string& name);

        // coordinates in link reference frame
        Eigen::Vector3d coords;

        // normal
        Eigen::Vector3d normal;

        // name of the link the marker is on.
        // Note that this will be the name of the link in the Coin naming
        // convention, which may mean that some invalid characters of the
        // original link name (from URDF) may have been replaced by '_'.
        // See also SoBase::setName() for more details.
        // You may use the helper function toSoBaseName() to transform
        // a name just as SoBase::setName() would.
        std::string linkName;
        // name of the visual the marker is on
        int visualNum;

        // a globally unique ID for this marker
        int markerID;
    };

    typedef std::map<std::string, std::vector<Marker> > MarkerMap;


    explicit MarkerSelector(float _marker_size, bool _faces_ccw):
        urdf_viewer::InventorViewer(_faces_ccw),
        marker_size(_marker_size) {}

    MarkerSelector(const MarkerSelector& o):
        marker_size(o.marker_size), markers(o.markers) {}
    ~MarkerSelector() {}

    bool writeResults(const std::string& outputFile);

    MarkerMap getMarkers();

    std::string toString();

protected:
    virtual void onClickModel(const SoPickedPoint * pickPoint);
    virtual void onMouseBtnClick(SoEventCallback *pNode){}

private:
    // helper function to sort markers
    static bool sortMarker(const Marker& i, const Marker& j);

    static bool writeToFile(const std::string& content, const std::string& filename);

    std::vector<Marker> markers;

    // map for all the nodes associated to the marker cylinder nodes, sorted by ID
    typedef std::map<int, SoNode*> MarkerNodeMap;

    MarkerNodeMap markerParentNodes;

    // the size of the points displayed where marker is put
    float marker_size;
};

}  //  namespace markerselector
}  //  namespace urdf2graspit
#endif   // URDF2GRASPIT_MARKERSELECTOR_H