ROS2: visualization_msgs/Markerを試す

海洋ロボコンをやってた人です。
今回は、ROS 2を用いたPCL+Realsenseの3次元点群を用いて、visualization_msgs/Markerを試してみたので、その備忘録としてまとめました。

モチベーションはGrowing Neural Gas(GNG) を実装する際に使いたかったためです。

自分自身の備忘録としていますが、本学の後輩への引き継ぎとなればとも思っています。

また、何か間違っている箇所等あれば、お気軽にご教授いただけると嬉しく思います。

本記事のプログラム

この記事で扱うプログラムのコードはGithubよりクローンしてご活用ください。

https://github.com/tasada038/ros2_rs_pcl

ディレクトリは「src/visualization_marker」です。

ROS2: visualization_msgs/Markerとは

はじめに、ROS 2 visualization_msgs/Markerとはwiki.ros.orgによると様々なシェイプを可視化できるマーカーになります。

visualization_msgs/Marker または visualization_msgs/MarkerArray メッセージで、さまざまなプリミティブ シェイプをプログラムで 3D ビューに追加できる。
引用：　http://wiki.ros.org/rviz/DisplayTypes/Marker

ROS/ROS 2 で予め用意されたシェイプ以外に、独自でシェイプ等を作成する際に役立てます。

visualization_msgs/Markerは、以下のようにMessageが定義されており、uint8型で各シェイプの形状を変更することができます。
また、定義したシェイプの位置(pose)、スケール(scale)、カラー(color)も自在に変更することができます。

http://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/Marker.html

さらに、シェイプを配列として持つvisualization_msgs/MarkerArrayを使用することで、一つのtopicで複数のシェイプを送信することが可能です。

http://docs.ros.org/en/noetic/api/visualization_msgs/html/msg/MarkerArray.html

本記事では、MarkerArrayを使用して、4種類のシェイプを試してみたので、そのサンプルを紹介します。

SPHEPE

SPHEPEでは球を表示できます。
scaleの値を異なる値とすると、球体ではなく楕円になります。
そのため、球体を表示させる場合はscale.x=scale.y=scale.zとする必要があります。
marker.typeは「SPHERE=2」です。

rspcl_marker.cpp

        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = "camera_color_optical_frame";
        marker.header.stamp = this->now();
        marker.ns = "sphepe";
        marker.id = id++;
        marker.type = visualization_msgs::msg::Marker::SPHERE;
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.pose.position.x = geometry_points[i].x;
        marker.pose.position.y = geometry_points[i].y;
        marker.pose.position.z = geometry_points[i].z;
        marker.scale.x = marker.scale.y = marker.scale.z = 0.02;
        marker.color.r = 0.0;
        marker.color.g = 1.0;
        marker.color.b = 1.0;
        marker.color.a = 1.0;
        marker.lifetime = rclcpp::Duration(0.0);

ARROW

ARROWは矢印のシェイプを表示できます。
scale.xは矢印(線)の長さ、scale.yは矢印(ヘッド)の幅、scale.zは矢印(ヘッド)の高さです。
矢印を明確に表示する場合は、scale.x > scale.y , scale.zとなるのが自然かと思います。
※　矢印の線の長さが矢印のヘッドよりも長い方が「→」として綺麗に表示されるため

marker.typeは「ARROW=0」です。

また、ARROW等の向きは、pose.orientation.x/y/z/wの単位クォータニオン(unit quaternion)で表しており

$$||q|| = \sqrt{{q_x}^2+{q_y}^2+{q_z}^2+{q_w}^2}$$

を満たす必要があると思います。

rspcl_marker.cpp

        // ARROW_r
        visualization_msgs::msg::Marker arrow_r;
        arrow_r.header.frame_id = "camera_color_optical_frame";
        arrow_r.header.stamp = this->now();
        arrow_r.ns = "arrow";
        arrow_r.id = id++;
        arrow_r.type = visualization_msgs::msg::Marker::ARROW;
        arrow_r.action = visualization_msgs::msg::Marker::ADD;
        arrow_r.pose.position.x = geometry_points[i].x;
        arrow_r.pose.position.y = geometry_points[i].y;
        arrow_r.pose.position.z = geometry_points[i].z;
        arrow_r.pose.orientation.x = 0.0;
        arrow_r.pose.orientation.y = 0.0;
        arrow_r.pose.orientation.z = 0.0;
        arrow_r.pose.orientation.w = 1.0;
        arrow_r.scale.x = 0.1;
        arrow_r.scale.y = arrow_r.scale.z = 0.01;
        arrow_r.color.r = 1.0;
        arrow_r.color.g = 0.0;
        arrow_r.color.b = 0.0;
        arrow_r.color.a = 1.0;
        arrow_r.lifetime = rclcpp::Duration(0.0);

        // ARROW_g
        visualization_msgs::msg::Marker arrow_g;
        arrow_g.header.frame_id = "camera_color_optical_frame";
        arrow_g.header.stamp = this->now();
        arrow_g.ns = "arrow";
        arrow_g.id = id++;
        arrow_g.type = visualization_msgs::msg::Marker::ARROW;
        arrow_g.action = visualization_msgs::msg::Marker::ADD;
        arrow_g.pose.position.x = geometry_points[i].x;
        arrow_g.pose.position.y = geometry_points[i].y;
        arrow_g.pose.position.z = geometry_points[i].z;
        arrow_g.pose.orientation.x = 0.0;
        arrow_g.pose.orientation.y = -0.707;
        arrow_g.pose.orientation.z = 0.0;
        arrow_g.pose.orientation.w = 0.707;
        arrow_g.scale.x = 0.1;
        arrow_g.scale.y = arrow_g.scale.z = 0.01;
        arrow_g.color.r = 0.0;
        arrow_g.color.g = 1.0;
        arrow_g.color.b = 0.0;
        arrow_g.color.a = 1.0;
        arrow_g.lifetime = rclcpp::Duration(0.0);

View-Oriented Text

TEXT_VIEW_FACINGはテキストを表示できます。
テキストのスケールはscale.zのみで表現し、この値を大きくすると、フォントサイズが増加します。
marker.typeは「TEXT_VIEW_FACING=9」です。

rspcl_marker.cpp

        // TEXT
        visualization_msgs::msg::Marker text;
        text.header.frame_id = "camera_color_optical_frame";
        text.header.stamp = this->now();
        text.ns = "text";
        text.id = id++;
        text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
        text.action = visualization_msgs::msg::Marker::ADD;
        text.pose.position.x = geometry_points[i].x+0.02;
        text.pose.position.y = geometry_points[i].y+0.02;
        text.pose.position.z = geometry_points[i].z-0.02;
        text.scale.z = 0.02;
        text.text = std::to_string(text.pose.position.x) + "," \
        + std::to_string(text.pose.position.y) + "," \
        + std::to_string(text.pose.position.z);
        text.color.r = 1.0;
        text.color.g = 1.0;
        text.color.b = 1.0;
        text.color.a = 1.0;
        text.lifetime = rclcpp::Duration(0.0);

LINE_STRIP

LINE_STRIPは始点と終点間を繋ぐ線を表示できます。
スケールはscale.xのみを使用し、線の幅を変更することができます。
marker.typeは「LINE_STRIP=4」です。

rspcl_marker.cpp

        // LINE STRIP
        visualization_msgs::msg::Marker connection;
        connection.header.frame_id = "camera_color_optical_frame";
        connection.header.stamp = this->now();
        connection.ns = "line_strip";
        connection.id = id++;
        connection.type = visualization_msgs::msg::Marker::LINE_STRIP;
        connection.action = visualization_msgs::msg::Marker::ADD;
        connection.scale.x = 0.005;
        connection.color.r = 0.0;
        connection.color.g = 1.0;
        connection.color.b = 0.0;
        connection.color.a = 1.0;
        connection.lifetime = rclcpp::Duration(0.0);
        start_line.x = geometry_points[i].x;
        start_line.y = geometry_points[i].y;
        start_line.z = geometry_points[i].z;
        goal_line.x = geometry_points[i-1].x;
        goal_line.y = geometry_points[i-1].y;
        goal_line.z = geometry_points[i-1].z;
        connection.points.push_back(start_line);
        connection.points.push_back(goal_line);

TRIANGLE_LIST

TRIANGLE_LISTは3点間のPointを塗りつぶして三角面を描画します。
marker.typeは「TRIANGLE_LIST=11」です。

rspcl_marker.cpp

      geometry_msgs::msg::Point point_1;
      geometry_msgs::msg::Point point_2;
      geometry_msgs::msg::Point point_3;
      for(int i=0; i<1; i++){
        visualization_msgs::msg::Marker triangle;
        triangle.header.frame_id = "camera_color_optical_frame";
        triangle.header.stamp = this->now();
        triangle.ns = "triangle_list";
        triangle.id = i;
        triangle.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
        triangle.action = visualization_msgs::msg::Marker::MODIFY;
        triangle.scale.x = triangle.scale.y = triangle.scale.z = 1;
        triangle.color.r = 0.0;
        triangle.color.g = 1.0;
        triangle.color.b = 0.0;
        triangle.color.a = 1.0;
        // triangle.lifetime = rclcpp::Duration(0.0);

        point_1.x = geometry_points[i].x;
        point_1.y = geometry_points[i].y;
        point_1.z = geometry_points[i].z;
        point_2.x = geometry_points[i+30].x;
        point_2.y = geometry_points[i+30].y;
        point_2.z = geometry_points[i+30].z;
        point_3.x = geometry_points[i+31].x;
        point_3.y = geometry_points[i+31].y;
        point_3.z = geometry_points[i+31].z;
        triangle.points.push_back(point_1);
        triangle.points.push_back(point_2);
        triangle.points.push_back(point_3);
        markers.markers.push_back(triangle);  
      }

Mesh Resource

Rvizでサポートしている、.stl, .dae等を表現できます。
Meshを使用する場合は、以下をプログラムに追記する必要があります。

mesh_marker.mesh_resource = "package://rviz_collada_marker/nil_link_mesh.dae"
mesh_marker.mesh_use_embedded_materials = True

marker.typeは「MESH_RESOURCE=10」です。

こちらは試せていませんが、garemonさんのプログラムを参考にしてみると良いと思います。

https://github.com/garaemon/rviz_collada_marker/blob/master/marker_mesh_sample.py

以上、ご参考にしていただければ幸いです。