2010年1月28日木曜日

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;
}


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

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 ()



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

0 件のコメント:

コメントを投稿