kdl-parserを使ってurdfをパースする

URDFとは？

gazeboで使用されているロボットの物理モデルの表現形式、xmlの書式でロボットのパラメータが記述されている。
urdfのパーサーには以下のものがある。(kdl-parser,urdf_parser)
/robot_descriptionパラメータにはURDF形式の文字列が入力されており、robot_state_publisher等で参照されている。
今回は/robot_descriptionパラメータからデータを受け取りそれを元にロボットの物理パラメータを取得する。

kdl-parserを使用する準備

package.xml,CMakeLists.txtにkdl_parserを追加

package.xml

<package>
  <build_depend>kdl_parser</build_depend>
  <run_depend>kdl_parser</run_depend>
</package>

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  sensor_msgs
  std_msgs
  tf
  tf2
  roslib
  urdf
  #add kdl_parser to find_package
  kdl_parser
)

C++コードの追加

以下のようなC++コードを追加

load_robot_description.cpp

  std::string robot_description_text;
  //get robot_description parameter
  nh.getParam("/robot_description", robot_description_text);
  KDL::Tree robot_tree;
  //parse urdf by using kdl_parser
  kdl_parser::treeFromString(robot_description_text, robot_tree);
  std::map<std::string,KDL::TreeElement> robot_segment = robot_tree.getSegments();
  //access each data by using iterator
  for(auto itr = robot_segment.begin(); itr != robot_segment.end(); ++itr)
  {
    //get link name from std::map
    std::string link_name = itr->first;
    //get link parameters
    KDL::RigidBodyInertia rigid_body_inertia = itr->second.segment.getInertia();
    //get Mass of link
    double mass = rigid_body_inertia.getMass();
    //get Center of Gravity parameters
    KDL::Vector cog_point = rigid_body_inertia.getCOG();
    //get Center of Gravity position
    double cog_point_x = cog_point.x();
    double cog_point_y = cog_point.y();
    double cog_point_z = cog_point.z();
  }

nao_descriptionパッケージ内のURDFファイルをパースした結果は以下のとおりです。
ちゃんとリンクの名前が出力されていますね！！

CameraBottom_frame
CameraBottom_optical_frame
CameraTop_frame
CameraTop_optical_frame
ChestButton_frame
Head
HeadTouchFront_frame
HeadTouchMiddle_frame
HeadTouchRear_frame
ImuTorsoAccelerometer_frame
ImuTorsoGyrometer_frame
LAnklePitch
LBicep
LElbow
LFinger11_link
LFinger12_link
LFinger13_link
LFinger21_link
LFinger22_link
LFinger23_link
LFootBumperLeft_frame
LFootBumperRight_frame
LForeArm
LFsrFL_frame
LFsrFR_frame
LFsrRL_frame
LFsrRR_frame
LHandTouchBack_frame
LHandTouchLeft_frame
LHandTouchRight_frame
LHip
LInfraRed_frame
LPelvis
LShoulder
LSonar_frame
LThigh
LThumb1_link
LThumb2_link
LTibia
MicroFrontCenter_frame
MicroRearCenter_frame
MicroSurroundLeft_frame
MicroSurroundRight_frame
Neck
RAnklePitch
RBicep
RElbow
RFinger11_link
RFinger12_link
RFinger13_link
RFinger21_link
RFinger22_link
RFinger23_link
RFootBumperLeft_frame
RFootBumperRight_frame
RForeArm
RFsrFL_frame
RFsrFR_frame
RFsrRL_frame
RFsrRR_frame
RHandTouchBack_frame
RHandTouchLeft_frame
RHandTouchRight_frame
RHip
RInfraRed_frame
RPelvis
RShoulder
RSonar_frame
RThigh
RThumb1_link
RThumb2_link
RTibia
base_link
gaze
l_ankle
l_gripper
l_sole
l_wrist
r_ankle
r_gripper
r_sole
r_wrist
torso