URDFファイルをパースしてプログラムから読み込む

前回作ったURDFファイルをプログラムからパースします。

http://www.ros.org/wiki/urdf/Tutorials/Parse%20a%20urdf%20file

では、さっそくいつものように練習用パッケージを作ります。

$ roscd sandbox
 $ roscreate-pkg learning_urdf urdf
 $ roscd learning_urdf
 $ rosmake

また、前回作ったurdfファイルをカレントディレクトリに持ってきます。

そして以下をsrc/parser.cppとして保存しましょう。

#include <urdf/model.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_parser");
  if (argc != 2){
    ROS_ERROR("Need a urdf file as argument");
    return -1;
  }
  std::string urdf_file = argv[1];

  urdf::Model model;
  if (!model.initFile(urdf_file)){
    ROS_ERROR("Failed to parse urdf file");
    return -1;
  }
  ROS_INFO("Successfully parsed urdf file");
  return 0;
}

意味のある中身は以下の２行です。

urdf::Model model;
  if (!model.initFile(urdf_file)){

urdf::Model型のオブジェクトを、initFIle()メソッドでファイルから読み出して初期化しています。

成功するとtrueがかえります。

$ bin/parser my_urdf.xml

[ INFO] 1264608023.141038000: Successfully parsed urdf file

となり成功しました。

urdf::Modelクラスは以下のメソッドでリンクやジョイントにアクセスできます。

boost::shared_ptr< const Joint >	getChildJoint (const std::string &name) const
	get child Joint of a Link given name
boost::shared_ptr< const Link >	getChildLink (const std::string &name) const
	get child Link of a Link given name
boost::shared_ptr< const Joint >	getJoint (const std::string &name) const
boost::shared_ptr< const Link >	getLink (const std::string &name) const
void	getLinks (std::vector< boost::shared_ptr< Link > > &links) const
const std::string &	getName () const
boost::shared_ptr< const Joint >	getParentJoint (const std::string &name) const
	get parent Joint of a Link given name
boost::shared_ptr< const Link >	getParentLink (const std::string &name) const
	get parent Link of a Link given name
boost::shared_ptr< const Link >	getRoot (void) const
bool	initFile (const std::string &filename)
	Load Model given a filename.
bool	initString (const std::string &xmlstring)
	Load Model from a XML-string.
bool	initXml (TiXmlDocument *xml)
	Load Model from TiXMLDocument.
bool	initXml (TiXmlElement *xml)
	Load Model from TiXMLElement.
	Model ()

これでロボティクスっぽいことができそうですね。