~/pose/info topic doesn't provide all the link poses

asked 2020-06-05 22:33:14 -0600

I have written a visual plugin that needs to subscribe to the poses of various links in my world. However when I subscribe to the ~/pose/info topic, I only get the poses of 14 links and 2 models (even though my world contains 5 models and 30 links). I also tried the ~/poses/local/info. Here is the Load, Update, and Callback functions I have written:-

void ModelLight::Load(rendering::VisualPtr visual_, sdf::ElementPtr sdf_) {
    if (sdf_->HasElement("led_color")) led_color_ = sdf_->Get<ignition::math::Color>("led_color");
    else led_color_ = ignition::math::Color(1,0,0,0);
    model_visual_ = visual_;
    std::cout << "Plugin Loaded" << std::endl;
    node_ = transport::NodePtr(new transport::Node);
    node_->Init();
    pose_sub_ = node_->Subscribe("~/pose/info", &ModelLight::infoCallback, this);

    updateConnection = event::Events::ConnectPreRender(std::bind(&ModelLight::OnUpdate, this));  
}

void ModelLight::OnUpdate() {
    model_visual_->SetEmissive(led_color_);
}

void ModelLight::infoCallback(ConstPosesStampedPtr &msg) {
    size_ = msg.get()->pose_size();
    if (pose_names_ == NULL) pose_names_ = new std::string [size_];
    std::cout << msg.get()->pose().size() << std::endl;
    for (int i=0; i<size_;i++) {
        pose_names_[i] = std::string(msg->pose(i).name()); 
    }
}

The output of the cout statement is 18 in the very beginning, then immediately becomes 16 as the world loads. Can somebody explain what is going on, and how to fix this?

edit retag flag offensive close merge delete