ROS中发布各种形状的visualization_msgs::Marker及rviz中的显示

最新推荐文章于 2025-02-27 21:33:55 发布

历尽千帆_SLAM

最新推荐文章于 2025-02-27 21:33:55 发布

阅读量5.9k

点赞数 5

分类专栏： ROS 文章标签： Marker rviz ROS

本文链接：https://blog.csdn.net/weixin_39458342/article/details/108798693

版权

ROS 专栏收录该内容

7 篇文章

订阅专栏

以下发布的是根据协方差矩阵求解的表征位置误差1 sigma范围的椭球,以及表征速度的箭头

    void publish(){
        visualization_msgs::MarkerArray ma;
        visualization_msgs::Marker m;
        visualization_msgs::Marker array;
        m.header.frame_id = "scan";
        m.id = 0;
        m.type = visualization_msgs::Marker::SPHERE;
        m.action = visualization_msgs::Marker::ADD;
        m.color.a = 0.5; // Don't forget to set the alpha!
        m.color.r = 0.0;
        m.color.g = 1.0;
        m.color.b = 0.0;
        m.pose.orientation.w = 1.0;
        // 发布球形
        for(const auto& object : dynamic_objects_){
            Eigen::Vector2d center = object->getPostPose();
            Eigen::Matrix2d covariance = object->getPostCov();
            Eigen::EigenSolver<Eigen::Matrix2d> solver(covariance);// 协方差矩阵为对角阵,对角线上的元素为方差,也是特征值
            Eigen::Matrix2d values = solver.pseudoEigenvalueMatrix();
            Eigen::Matrix2d vectors = solver.pseudoEigenvectors();
//            LOG(INFO) << "COV: " << covariance;
//            LOG(INFO) << values;
            // xyz分别代表,以圆球中心为原点,球的长宽高
            m.scale.x = std::sqrt(std::abs(values(0, 0)));// 1 sigma,边缘上的点的马氏距离恰好为1
            m.scale.y = std::sqrt(std::abs(values(1, 1)));
            m.scale.z = 0.01 ;
            // 圆球中心在世界坐标系中的位置
            m.pose.position.x = center(0);
            m.pose.position.y = center(1);
            // 圆球在世界坐标系中的姿态
            m.pose.orientation.x = 0.0;
            m.pose.orientation.y = 0.0;
            m.pose.orientation.z = std::atan2(vectors(1, 0), vectors(0, 0));
//            LOG(INFO) << "----------------" << m.scale.x << ", " << m.scale.y;
//            m.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
            ma.markers.push_back(m);
            m.id++;
        }
        array.header.frame_id = "scan";
        array.id = m.id;
        array.type = visualization_msgs::Marker::ARROW;
        array.action = visualization_msgs::Marker::ADD;
        array.color.a = 0.5; // Don't forget to set the alpha!
        array.color.r = 1.0;
        array.color.g = 0.0;
        array.color.b = 0.0;
        array.pose.orientation.w = 1.0;
        for(const auto& object : dynamic_objects_){
            Eigen::Vector2d velocity = object->getVelocity();
            Eigen::Vector2d pose = object->getPostPose();
            // 指定arrow的直径等大小
            array.scale.x = 0.05;// 柄直径
            array.scale.y = 0.1;// 箭头直径
            array.scale.z = 0;// 长度,由于已经指定了起始点/终止点,这里不能再指定长度!!!
            array.points.clear();
            geometry_msgs::Point p;// 箭头的在局部坐标系中的起始点/终止点
            p.x = 0.0;
            p.y = 0.0;
            array.points.push_back(p);
            p.x = velocity(0) * 10;
            p.y = velocity(1) * 10;
            array.points.push_back(p);
            // 局部坐标系在全局坐标系中的位置(也可以直接和array.points加到一起而这里默认为0)
            array.pose.position.x = pose(0);
            array.pose.position.y = pose(1);
            LOG(INFO) << "Velocity: " << velocity;
            ma.markers.push_back(array);
            array.id++;
        }
        // 清理掉掉上一帧多出来的
        int siz = ma.markers.size();
        if(siz < max_marker_count_){
            array.color.a = 0.0;
            for(int index = siz; index < max_marker_count_; ++index){
                ma.markers.push_back(array);
                array.id++;
            }
            array.color.a = 0.5;
        }else{
            max_marker_count_ = siz;
        }
        dynamic_sphere_pub_.publish(ma);
    }
};

参考:http://wiki.ros.org/rviz/DisplayTypes/Marker