ROS2: visualization_msgs/Markerを試す
海洋ロボコンをやってた人です。
今回は、ROS 2を用いたPCL+Realsenseの3次元点群を用いて、visualization_msgs/Markerを試してみたので、その備忘録としてまとめました。
モチベーションはGrowing Neural Gas(GNG) を実装する際に使いたかったためです。
自分自身の備忘録としていますが、本学の後輩への引き継ぎとなればとも思っています。
また、何か間違っている箇所等あれば、お気軽にご教授いただけると嬉しく思います。
本記事のプログラム
この記事で扱うプログラムのコードはGithubよりクローンしてご活用ください。
ディレクトリは「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)も自在に変更することができます。
さらに、シェイプを配列として持つvisualization_msgs/MarkerArrayを使用することで、一つのtopicで複数のシェイプを送信することが可能です。
本記事では、MarkerArrayを使用して、4種類のシェイプを試してみたので、そのサンプルを紹介します。
SPHEPE
SPHEPEでは球を表示できます。
scaleの値を異なる値とすると、球体ではなく楕円になります。
そのため、球体を表示させる場合はscale.x=scale.y=scale.zとする必要があります。
marker.typeは「SPHERE=2」です。
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)で表しており
を満たす必要があると思います。
// 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」です。
// 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」です。
// 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);
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さんのプログラムを参考にしてみると良いと思います。
以上、ご参考にしていただければ幸いです。
Discussion