ros中和RVIZ交互显示的InteractiveMarker简单例子

cpp 复制代码

#include <ros/ros.h>
#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <interactive_markers/interactive_marker_server.h>
#include <sstream>  // 包含 stringstream

// 回调函数，用于处理用户的互动反馈
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
  ROS_INFO_STREAM(feedback->marker_name << " is now at "
      << feedback->pose.position.x << ", " << feedback->pose.position.y
      << ", " << feedback->pose.position.z);
}

int main(int argc, char** argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "multiple_markers");
  ros::NodeHandle n;

  // 创建一个交互式标记服务器
  interactive_markers::InteractiveMarkerServer server("multiple_markers");

  // 定义要创建的方块数量
  const int num_cubes = 3;

  // 为每个方块创建交互式标记
  for (int i = 0; i < num_cubes; ++i) {
    // 创建一个交互式标记
    visualization_msgs::InteractiveMarker int_marker;
    int_marker.header.frame_id = "base_link";
    std::stringstream ss;
    ss << "cube_" << i;
    int_marker.name = ss.str();
    int_marker.description = "Simple 3-DOF Control";

    // 设置方块的位置
    int_marker.pose.position.x = i * 1.0;  // 沿X轴放置方块
    int_marker.pose.orientation.w = 1.0;

    // 创建一个灰色立方体作为可视化元素
    visualization_msgs::Marker box_marker;
    box_marker.type = visualization_msgs::Marker::CUBE;
    box_marker.scale.x = 0.45;
    box_marker.scale.y = 0.45;
    box_marker.scale.z = 0.45;
    box_marker.color.r = 0.5;
    box_marker.color.g = 0.5;
    box_marker.color.b = 0.5;
    box_marker.color.a = 1.0;

    // 创建一个总是可见的控制项，包含上面的立方体
    visualization_msgs::InteractiveMarkerControl box_control;
    box_control.always_visible = true;
    box_control.markers.push_back(box_marker);

    // 将控制项添加到交互式标记
    int_marker.controls.push_back(box_control);

    // 创建三个可以沿X、Y、Z轴移动的控制项
    for (int j = 0; j < 3; ++j) {
      visualization_msgs::InteractiveMarkerControl control;
      control.orientation.w = 1;
      control.orientation.x = 0;
      control.orientation.y = 0;
      control.orientation.z = 0;

      // 使用 stringstream 来格式化名称
      std::stringstream ss_control;
      ss_control << "move_" << j;
      control.name = ss_control.str();

      control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
      if (j == 0) {
        control.orientation.y = 1;
        control.orientation.w = 0;
      } else if (j == 1) {
        control.orientation.z = 1;
        control.orientation.w = 0;
      }
      int_marker.controls.push_back(control);
    }

    // 将交互式标记插入服务器，并设置回调函数
    server.insert(int_marker, &processFeedback);
  }

  // 应用更改
  server.applyChanges();

  // 进入主循环
  ros::spin();

  return 0;
}

visualization_msgs::InteractiveMarker int_marker; 的controls里都是InteractiveMarkerControl 对象，其中InteractiveMarkerControl 可以添加若干Marker（若干静态元素，默认是静态）如box_control，也添加可以是控制方式如：control。

通常：一个int_marker里添加若干个InteractiveMarkerControl，其中一个InteractiveMarkerControl添加Maker定义物体尺寸，其余若干个InteractiveMarkerControl表示对这个物体的控制。然后在将int_marker添加到交互服务server.insert(int_marker, &processFeedback);

若：int_marker里有多个Maker，则表示对这多个物体一起控制

若：想添加两个不同的物体独立控制，则向server.insert两个InteractiveMarker即可