コード例 #1
0
    /**
     * \brief create a branch marker between two nodes and store it into marker array
     */
    virtual void createBranchMarker(const Chart &child, const Chart &parent)
    {
        if (!markers){
            ROS_WARN_THROTTLE(30,"[ExplorerBase::%s]\tNo marker array to update is provided, visualization is disabled.",__func__);
            return;
        }
        geometry_msgs::Point e;
        geometry_msgs::Point s;

        visualization_msgs::Marker branch;
        branch.header.frame_id = mark_frame;
        branch.header.stamp = ros::Time();
        branch.lifetime = ros::Duration(0.5);
        branch.ns = "Atlas Branches";
        //need to know the branch id, too bad branches don't have it.
        //Lets use Cantor pairing function: 0.5(a+b)(a+b+1)+b
        branch.id = 0.5*(child.getId() + parent.getId())*(child.getId()+parent.getId()+1) + parent.getId();
        branch.type = visualization_msgs::Marker::LINE_STRIP;
        branch.action = visualization_msgs::Marker::ADD;
        s.x = child.getCenter()[0];
        s.y = child.getCenter()[1];
        s.z = child.getCenter()[2];
        branch.points.push_back(s);
        e.x = parent.getCenter()[0];
        e.y = parent.getCenter()[1];
        e.z = parent.getCenter()[2];
        branch.points.push_back(e);
        branch.scale.x = 0.005;
        branch.color.a = 1.0;
        branch.color.r = 0.0;
        branch.color.g = 0.0;
        branch.color.b = 0.9;
        std::lock_guard<std::mutex> guard(*mtx_ptr);
        markers->markers.push_back(branch);
    }
コード例 #2
0
 /**
  * \brief create a marker from a chart and stores it into markers array
  */
 virtual void createNodeMarker(const Chart &c)
 {
     if (!markers){
         ROS_WARN_THROTTLE(30,"[ExplorerBase::%s]\tNo marker array to update is provided, visualization is disabled.",__func__);
         return;
     }
     visualization_msgs::Marker disc;
     disc.header.frame_id = mark_frame;
     disc.header.stamp = ros::Time();
     disc.lifetime = ros::Duration(0.5);
     disc.ns = "Atlas Nodes";
     disc.id = c.getId();
     disc.type = visualization_msgs::Marker::CYLINDER;
     disc.action = visualization_msgs::Marker::ADD;
     disc.scale.x = c.getRadius()*2;
     disc.scale.y = c.getRadius()*2;
     disc.scale.z = 0.001;
     disc.color.a = 0.75;
     disc.color.r = 0.3;
     disc.color.b = 0.35;
     disc.color.g = 0.5;
     Eigen::Matrix3d rot;
     rot.col(0) =  c.getTanBasisOne();
     rot.col(1) =  c.getTanBasisTwo();
     rot.col(2) =  c.getNormal();
     Eigen::Quaterniond q(rot);
     q.normalize();
     disc.pose.orientation.x = q.x();
     disc.pose.orientation.y = q.y();
     disc.pose.orientation.z = q.z();
     disc.pose.orientation.w = q.w();
     disc.pose.position.x = c.getCenter()[0];
     disc.pose.position.y = c.getCenter()[1];
     disc.pose.position.z = c.getCenter()[2];
     std::lock_guard<std::mutex> guard(*mtx_ptr);
     markers->markers.push_back(disc);
 }
コード例 #3
0
    virtual void createSamplesMarker(const Chart &c, const Eigen::Vector3d &projected)
    {
        if (c.samp_chosen < 0 || no_sample_markers)
            return;
        for (size_t i=0; i<c.samples.rows(); ++i)
        {
            visualization_msgs::Marker samp;
            samp.header.frame_id = mark_frame;
            samp.header.stamp = ros::Time();
            samp.lifetime = ros::Duration(0.5);
            samp.ns = "Atlas Node(" + std::to_string(c.getId()) + ") Samples";
            samp.id = i;
            samp.type = visualization_msgs::Marker::SPHERE;
            samp.action = visualization_msgs::Marker::ADD;
            samp.scale.x = 0.01;
            samp.scale.y = 0.01;
            samp.scale.z = 0.01;
            samp.color.a = 0.8;
            if (i==c.samp_chosen){
                //is winner
                samp.color.r = 0.6;
                samp.color.b = 0.0;
                samp.color.g = 0.0;
            }
            else{
                samp.color.r = 0.6;
                samp.color.b = 0.0;
                samp.color.g = 0.6;
            }
            samp.pose.position.x = c.samples(i,0);
            samp.pose.position.y = c.samples(i,1);
            samp.pose.position.z = c.samples(i,2);
            std::lock_guard<std::mutex> guard(*mtx_ptr);
            markers->markers.push_back(samp);
        }
        geometry_msgs::Point e;
        geometry_msgs::Point s;

        visualization_msgs::Marker proj;
        proj.header.frame_id = mark_frame;
        proj.header.stamp = ros::Time();
        proj.lifetime = ros::Duration(0.5);
        proj.ns = "Atlas Node(" + std::to_string(c.getId()) + ") Samples";
        proj.id = c.samples.rows();
        proj.type = visualization_msgs::Marker::ARROW;
        proj.action = visualization_msgs::Marker::ADD;
        s.x = c.samples(0,0);
        s.y = c.samples(0,1);
        s.z = c.samples(0,2);
        proj.points.push_back(s);
        e.x = projected[0];
        e.y = projected[1];
        e.z = projected[2];
        proj.points.push_back(e);
        proj.scale.x = 0.001;
        proj.scale.y = 0.01;
        proj.scale.z = 0.01;
        proj.color.a = 0.4;
        proj.color.r = 0.0;
        proj.color.g = 0.2;
        proj.color.b = 0.8;
        std::lock_guard<std::mutex> guard(*mtx_ptr);
        markers->markers.push_back(proj);
    }