因为我想在代码里修改并保存urdf文件,参考2个开源代码,实现待更新
https://github.com/ilidar/urdf_editor
https://github.com/rhololkeolke/robot_editor

参考上面开源的代码,读写urdf使用如下:

读取urdf文件

#include <urdf/model.h>
#include <urdf_parser/urdf_parser.h>
#include <ros/package.h>

void  QNode::initUrdfFile(){

    std::string init_file = ros::package::getPath("navigation_ros")
            + "/urdf/backpack_2d.urdf";

    std::cout << "init_file : "
              << init_file << std::endl;

    if(!urdfModel->initFile(init_file)){
        std::cout << "urdf_file load fail" << std::endl;
    }
    else{
        std::cout << "urdf_file load success" << std::endl;
    }

    return;
}

写urdf文件

#include <urdf/model.h>
#include <urdf_parser/urdf_parser.h>
#include <ros/package.h>

bool  QNode::saveUrdfFile()
{
    auto& data = urdfModel->joints_["base_laser_link_joint"];

    data->parent_to_joint_origin_transform.position.x = cali_lidar_odom_x;
    data->parent_to_joint_origin_transform.position.y = cali_lidar_odom_y;
    data->parent_to_joint_origin_transform.rotation.setFromRPY(0,0,cali_lidar_odom_yaw);

    std::unique_ptr<TiXmlDocument> tiXmlDoc(urdf::exportURDF(*urdfModel));


    std::string final_file = ros::package::getPath("navigation_ros")
             + "/urdf/backpack_2d.urdf";

    //
    std::cout << "final_file : "
              << final_file << std::endl;

    return tiXmlDoc->SaveFile(final_file);
}
Logo

更多推荐