add waypoint text marker

Author: zhangwanjie

rviz/edit.rviz

@@ -5,7 +5,6 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Map1/Topic1
-        - /InteractiveMarkers1
       Splitter Ratio: 0.49873417615890503
     Tree Height: 549
   - Class: rviz_common/Selection
@@ -74,7 +73,7 @@ Visualization Manager:
       Interactive Markers Namespace: /waypoint_markers
       Name: InteractiveMarkers
       Show Axes: false
-      Show Descriptions: true
+      Show Descriptions: false
       Show Visual Aids: false
       Value: true
   Enabled: true
@@ -108,33 +107,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 9.635456085205078
+      Distance: 9.749788284301758
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.09208789467811584
-        Y: -0.39830833673477173
-        Z: -0.6636285781860352
+        X: 0.1213555708527565
+        Y: 0.25752243399620056
+        Z: -1.2387741804122925
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.1403977870941162
+      Pitch: 1.0253981351852417
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.708583354949951
+      Yaw: 1.5735856294631958
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: true
-  Height: 846
+  Height: 1376
   Hide Left Dock: true
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000018d000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b0000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000018d000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000009ba000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -143,6 +142,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1200
-  X: 715
-  Y: 186
+  Width: 2490
+  X: 70
+  Y: 27

src/wp_edit_node.cpp

@@ -5,7 +5,7 @@
 #include <yaml-cpp/yaml.h>
 
 #include <rclcpp/rclcpp.hpp>
-#include <visualization_msgs/msg/marker.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 #include <visualization_msgs/msg/interactive_marker.hpp>
 #include <visualization_msgs/msg/interactive_marker_control.hpp>
 #include <interactive_markers/interactive_marker_server.hpp>
@@ -16,6 +16,7 @@
 std::shared_ptr<rclcpp::Node> node;
 static std::vector <wp_map_tools::msg::Waypoint> arWaypoint;
 static std::vector <wp_map_tools::msg::Waypoint> arCharger;
+static rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub;
 static visualization_msgs::msg::Marker text_marker;
 static interactive_markers::InteractiveMarkerServer* pWaypointServer = NULL;
 
@@ -161,12 +162,12 @@ void processWaypointFeedback(
   const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback,
   rclcpp::Logger logger)
 {
-  // std::ostringstream oss;
-  // oss << feedback->marker_name << " is now at " << feedback->pose.position.x << ", " <<
-  //   feedback->pose.position.y << ", " << feedback->pose.position.z;
-  // RCLCPP_WARN(logger, "%s", oss.str().c_str());
+    // std::ostringstream oss;
+    // oss << feedback->marker_name << " is now at " << feedback->pose.position.x << ", " <<
+    //   feedback->pose.position.y << ", " << feedback->pose.position.z;
+    // RCLCPP_WARN(logger, "%s", oss.str().c_str());
 
-  int nNumWP = arWaypoint.size();
+    int nNumWP = arWaypoint.size();
     for(int i=0; i<nNumWP ; i++ )
     {
         if(feedback->marker_name == arWaypoint[i].name)
@@ -182,18 +183,16 @@ void NewWaypointInterMarker(interactive_markers::InteractiveMarkerServer* inServ
 {
     visualization_msgs::msg::InteractiveMarker wp_itr_marker;
     visualization_msgs::msg::InteractiveMarkerControl wp_dis_ctrl;
-    visualization_msgs::msg::Marker wp_dis_marker;
     visualization_msgs::msg::InteractiveMarkerControl move_control;
     wp_itr_marker.header.stamp=node->get_clock()->now();
     wp_itr_marker.name = inName;
     wp_itr_marker.description = inName;
     wp_itr_marker.pose = InPose;
-
-    // 显示外形
     wp_itr_marker.header.frame_id = "map";
     wp_itr_marker.header.stamp=node->get_clock()->now();
 
-    // wp_dis_marker.ns = "waypoint_markers";
+    // 显示3D模型
+    visualization_msgs::msg::Marker wp_dis_marker;
     wp_dis_marker.action = visualization_msgs::msg::Marker::ADD;
     wp_dis_marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
     wp_dis_marker.mesh_resource = "package://wp_map_tools/meshes/waypoint.dae";
@@ -204,8 +203,20 @@ void NewWaypointInterMarker(interactive_markers::InteractiveMarkerServer* inServ
     wp_dis_marker.color.g = 0.0;
     wp_dis_marker.color.b = 1.0;
     wp_dis_marker.color.a = 1.0;
-
     wp_dis_ctrl.markers.push_back( wp_dis_marker );
+
+    // 显示航点名称文字
+    visualization_msgs::msg::Marker text_marker;
+    text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+    text_marker.scale.z = 0.3;
+    text_marker.color.r = 1;
+    text_marker.color.g = 1;
+    text_marker.color.b = 0;
+    text_marker.color.a = 1.0;
+    text_marker.text = inName;
+    text_marker.pose.position.z = 0.8;
+    wp_dis_ctrl.markers.push_back( text_marker );
+    
     wp_dis_ctrl.always_visible = true;
     wp_itr_marker.controls.push_back( wp_dis_ctrl );
 
@@ -294,7 +305,6 @@ void AddWayPointCallback(const wp_map_tools::msg::Waypoint::ConstPtr& wp)
     }
 }
 
-
 int main(int argc, char ** argv)
 {
     rclcpp::init(argc, argv);
@@ -318,6 +328,7 @@ int main(int argc, char ** argv)
     //服务和话题初始化
     auto add_waypoint_sub = node->create_subscription<wp_map_tools::msg::Waypoint>("waterplus/add_waypoint",10,AddWayPointCallback);
     auto service = node->create_service<wp_map_tools::srv::SaveWaypoints>("waterplus/save_waypoints", saveWaypoints);
+    marker_pub = node->create_publisher<visualization_msgs::msg::Marker>("text_marker", 100);
 
     RCLCPP_INFO(node->get_logger(), "wp_edit_node 初始化完毕");