HKUST-Aerial-Robotics / Teach-Repeat-Replan

Teach-Repeat-Replan: A Complete and Robust System for Aggressive Flight in Complex Environments
GNU General Public License v3.0
962 stars 252 forks source link

椭球,多面体消息类型的定义 #61

Closed JiangxinkeTao closed 2 years ago

JiangxinkeTao commented 2 years ago

高飞老师您好,我想使用您代码中的多面体显示插件,但是其中的含义有部分不是很明确,另外椭球的消息类型含义不是很明确,不能添加一下注释信息吗? 另外就是多面体,我按照数据类型编写代码,调用插件,并不能在RVIZ中显示,

下面是我编写的代码,用于测试多面体的显示,本例子中显示的是四面体,但是并不能得到预期效果

#include <iostream>
#include <ros/ros.h>

#include "decomp_ros_msgs/Polyhedron.h"
#include "decomp_ros_msgs/PolyhedronArray.h"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "test_poly");
    ros::NodeHandle nh;
    ros::Publisher poly_pub;
    ros::Publisher polydre_pub;

    poly_pub = nh.advertise<decomp_ros_msgs::Polyhedron>("poly",1,true);
    polydre_pub = nh.advertise<decomp_ros_msgs::PolyhedronArray>("polydre",1,true);

    ros::Rate loop_rate(10);

    decomp_ros_msgs::Polyhedron poly;
    geometry_msgs::Point p1;
    p1.x=1.0;
    p1.y=0.0;
    p1.z=0.0;
    poly.points.push_back(p1);

    geometry_msgs::Point p2;
    p2.x=0.0;
    p2.y=1.0;
    p2.z=0.0;
    poly.points.push_back(p2);

    geometry_msgs::Point p3;
    p3.x=-1.0;
    p3.y= 0.0;
    p3.z= 0.0;
    poly.points.push_back(p3);

    geometry_msgs::Point p4;
    p4.x=0.0;
    p4.y=0.0;
    p4.z=1.0;
    // poly.points.push_back(p4);

    geometry_msgs::Point np;
    np.x = 0;
    np.y = 1;
    np.z = 2;
    decomp_ros_msgs::Polyhedron mp1;
    mp1.points.push_back(p1);
    mp1.points.push_back(p2);
    mp1.points.push_back(p3);
    mp1.normals.push_back(np);
    mp1.normals.push_back(np);
    mp1.normals.push_back(np);

    decomp_ros_msgs::Polyhedron mp2;
    mp2.points.push_back(p2);
    mp2.points.push_back(p3);
    mp2.points.push_back(p4);
    mp2.normals.push_back(np);
    mp2.normals.push_back(np);
    mp2.normals.push_back(np);

    decomp_ros_msgs::Polyhedron mp3;
    mp3.points.push_back(p3);
    mp3.points.push_back(p4);
    mp3.points.push_back(p1);
    mp3.normals.push_back(np);
    mp3.normals.push_back(np);
    mp3.normals.push_back(np);

    decomp_ros_msgs::Polyhedron mp4;
    mp4.points.push_back(p4);
    mp4.points.push_back(p1);
    mp4.points.push_back(p2);
    mp4.normals.push_back(np);
    mp4.normals.push_back(np);
    mp4.normals.push_back(np);

    decomp_ros_msgs::PolyhedronArray mpoly;
    mpoly.header.frame_id = "/map";
    mpoly.polyhedrons.push_back(mp1);
    mpoly.ids.push_back(0);
    mpoly.polyhedrons.push_back(mp2);
    mpoly.ids.push_back(1);
    mpoly.polyhedrons.push_back(mp3);
    mpoly.ids.push_back(2);
    mpoly.polyhedrons.push_back(mp4);
    mpoly.ids.push_back(3);

    bool tb_f = false;
    while (ros::ok())
    {
        ROS_ERROR_STREAM_THROTTLE(5,"polygon ... ");
        if(!tb_f)
        {
            poly_pub.publish(poly);
            polydre_pub.publish(mpoly);
            tb_f = true;
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

但是效果并不是很好,插件能够被加载,数据也能够echo 但是不能在RVIZ中显示,按照你README文件操作还是不能够显示

Screenshot from 2022-07-23 14-37-00

诚盼您的回复!!!

bigsuperZZZX commented 2 years ago

建议人工仔细对比原始代码发送出来的多面体topic和你的代码的topic

JiangxinkeTao commented 2 years ago

好的,谢谢你,周鑫同学,我已经解决这个问题了,我没注意你们这个包是调用别人的原因,不好意思,我已经解决这个问题了,忘记关闭这个issue。