rvizを使う

今回はrvizを使ってみます。
今までにも何度かrvizはでてきました。
主に自律移動の可視化（センサ情報、自己位置など）に利用してきました。

今回は自分のオリジナルデータを表示する方法をやります。

以下のチュートリアルをやります。
http://www.ros.org/wiki/rviz/Tutorials/Markers:%20Basic%20Shapes

visualization_msgs/Marker というメッセージを利用して表示する方法です。

規定のshapeから選択して表示する方法になります。
以下のような矢印、キューブ、線などから選択することになります。

以下では１つのオブジェクトを表示して、それが１秒毎に形状の種類を変化させるサンプルをつくります。

それではいつも通りパッケージを作りましょう。


$ roscreate-pkg using_markers roscpp visualization_msgs

で、いつものように以下のコードをsrc/basic_shapes.cppとして保存してください。

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

int main( int argc, char** argv )
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Rate r(1);
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);

  // Set our initial shape type to be a cube
  uint32_t shape = visualization_msgs::Marker::CUBE;

  while (ros::ok())
  {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/my_frame";
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "basic_shapes";
    marker.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;

    // Set the marker action.  Options are ADD and DELETE
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration();

    // Publish the marker
    marker_pub.publish(marker);

    // Cycle between different shapes
    switch (shape)
    {
    case visualization_msgs::Marker::CUBE:
      shape = visualization_msgs::Marker::SPHERE;
      break;
    case visualization_msgs::Marker::SPHERE:
      shape = visualization_msgs::Marker::ARROW;
      break;
    case visualization_msgs::Marker::ARROW:
      shape = visualization_msgs::Marker::CYLINDER;
      break;
    case visualization_msgs::Marker::CYLINDER:
      shape = visualization_msgs::Marker::CUBE;
      break;
    }

    r.sleep();
  }
}

ざっと、上から眺めていくと大体わかると思いますが、

  uint32_t shape = visualization_msgs::Marker::CUBE;

や

marker.action = visualization_msgs::Marker::ADD;

のところでメッセージの定数が利用されていますね。
これまであまり使ってきませんでした。

http://www.ros.org/wiki/msg
の2.2にあるように.msgファイルで

constanttype1 CONSTANTNAME1=constantvalue1
constanttype2 CONSTANTNAME2=constantvalue2

のようにすると定数が使えます。

あとは気になったのは↓でした。

    marker.lifetime = ros::Duration();

lifetimeを設定すると自動でその時間経過すると削除されるようです。
ros::Duration()　（つまり０秒）にすると自動で削除されません。

あとはここまで勉強してきた人ならさらっと理解できると思います。

そしたらCMakeList.txtに以下を追記して、

rosbuild_add_executable(basic_shapes src/basic_shapes.cpp)

$ rosmake
で依存関係を考慮してmakeしましょう。

そうしたら
$ roscore
$ rosrun rviz rviz
でrvizを立ち上げます。

.Global Optionsの
Fixed FrameとTarget Frameを/my_frameにして、
Addボタンを押してMarkerを追加します。

すると、Marker Topic はvisualization_marker
になっているので、このサンプルと同じです。

なのでこれだけで表示されます。

という感じで次々変わっていきます。

点群や、線などもpointsを使えば書けそうです。