ROS-4-Visualization-Rviz

1.简单的几何模型（boxes,spheres,cylinder,arrow)
1.1使用 visualization_msgs/Marker类型的消息来发送基本的立体形状。我们借助工具Marker Display 来将数据在rviz中显示，这样rviz就不需要对数据进行解释。
1.2执行以下代码，创建using_markers package

catkin_create_pkg using_markers roscpp visualization_msgs

之后在src文件夹下面建立basic_shapes.cpp文件，粘贴以下代码：

#include 
#include 

int main( int argc, char** argv )
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Rate r(1);
  //变量marker_pub 在visualization_marker话题上发布数据，数据类型为visualization_msgs::Marker
  ros::Publisher marker_pub = n.advertise("visualization_marker", 1);
//用整数类型变量shape来记录Marker shape,初始化为CUBE
  uint32_t shape = visualization_msgs::Marker::CUBE;

//设置循环，不断改变shape的类型
  while (ros::ok())
  {
    visualization_msgs::Marker marker;   建立要发布的变量marker,它的类型是visualization_msgs::Marker
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/my_frame";            //marker的坐标系
    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的命名空间
    marker.id = 0;                                                 //marker的id

    // 指定我们要发布marker的type为shape类型，
    marker.type = shape;                              

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD; //marker的动作为添加一个marker

    // 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的位姿
    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的大小
    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的颜色
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration(); //设定marker的持续时间

    // Publish the marker
    while (marker_pub.getNumSubscribers() < 1)
    {
      if (!ros::ok())
      {
        return 0;
      }
      ROS_WARN_ONCE("Please create a subscriber to the marker");
      sleep(1);
    }
    marker_pub.publish(marker);       //发布marker消息

    // Cycle between different shapes
    switch (shape)                     //调换到下一个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();
  }
}

之后在CMakeLists.txt中添加如下代码

add_executable(basic_shapes src/basic_shapes.cpp)  //前一个是生成可执行文件名，与.cpp文件名相同
target_link_libraries(basic_shapes ${catkin_LIBRARIES})  //link到生成的可执行文件

之后进行编译catkin_make
1.3 试验代码
由于我们需要发布markers,首先我们要设置好Rviz以便后面来观察这些markers,输入以下代码：

rosrun using_markers basic_shapes //注意先打开roscore客户端，这时会提示我们没有订阅者
rosmake rviz//打开另一个terminal，运行这两行代码
rosrun rviz rviz 

Rviz使用手册
更多的shape
打开之后可能没有东西，我们在打开的Rviz界面设定fixed frame 为刚才的/my_frame，添加Markers的显示界面，将topic设定为visualization_marker

2.在发送简单的几何体到Rviz的基础上，我们需要学习发送点,线段，直线到Rviz上，借助同样的工具，发布marker数据到话题上。point将一个点放到要添加的位置上，line_strip将每个点作为顶点，将这些点连接起来形成一条直线，line_list创建彼此之间不连接的单独线段。
2.1与之前创建几何体一样，粘贴以下代码到points_and_lines.cpp文件中

#include 
#include 

#include 

int main( int argc, char** argv )
{
  ros::init(argc, argv, "points_and_lines");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise("visualization_marker", 10);

  ros::Rate r(30);

  float f = 0.0;
  while (ros::ok())
  {

    visualization_msgs::Marker points, line_strip, line_list;  //设置要发布的marker,分别命名为points,line_strip,line_list
    points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
    points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
    points.ns = line_strip.ns = line_list.ns = "points_and_lines";
    points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;
//设置位姿，由于位置和x,y,z的orientation默认为0，所以只需要设置w的值


    points.id = 0;
    line_strip.id = 1;
    line_list.id = 2;



    points.type = visualization_msgs::Marker::POINTS;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_list.type = visualization_msgs::Marker::LINE_LIST;



    // POINTS markers use x and y scale for width/height respectively，设置点的大小
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
    line_strip.scale.x = 0.1;    //设置将点连接时，连接线的宽度
    line_list.scale.x = 0.1;    //设置连接点的线段时，线段的宽度



    // Points are green
    points.color.g = 1.0f;
    points.color.a = 1.0;

    // Line strip is blue
    line_strip.color.b = 1.0;
    line_strip.color.a = 1.0;

    // Line list is red
    line_list.color.r = 1.0;
    line_list.color.a = 1.0;



    // Create the vertices for the points and lines
    for (uint32_t i = 0; i < 100; ++i)
    {
      float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
      float z = 5 * cos(f + i / 100.0f * 2 * M_PI);

      geometry_msgs::Point p;  //计算每个点的位置，将生成的每个点利用visualization_msgs::Marker类所共有的points.push_back()函数插入到要发布的三个变量中。
      p.x = (int32_t)i - 50;
      p.y = y;
      p.z = z;

      points.points.push_back(p);
      line_strip.points.push_back(p);

      // The line list needs two points for each line   由于line_list为线段，所以每次存储两个点，z坐标差1，总的点数为points的两倍。
      line_list.points.push_back(p);
      p.z += 1.0;
      line_list.points.push_back(p);
    }


    marker_pub.publish(points);
    marker_pub.publish(line_strip);
    marker_pub.publish(line_list);

    r.sleep();

    f += 0.04;
  }
}

之后在CMakeLists.txt中添加以下代码：

add_executable(points_and_lines src/points_and_lines.cpp)
target_link_libraries(points_and_lines ${catkin_LIBRARIES})

$ catkin_make

2.2试验代码

$ rosrun rviz rviz &
$ rosrun using_markers points_and_lines

结果大致如下：

3.Interactive marker
为了使用户能够更灵活地控制marker,而不是根据固定的marker代码来控制，我们使用了Interactive Marker.用户通过点击这些controls块，或者通过context menu 来控制每个marker.
交互式的marker的消息类型为visualization_msgs/InteractiveMarker ，controls块负责他的可视化面板，我们可以通过创建一个Interactive Marker Server类的node，来提供多个interactive marker并对他们进行管理，该Node负责与Client(Rviz)进行连接，从而确保我们所做的改变都能被Rviz所接收到。
首先学习如何使用interactive marker,interactive marker发布的消息包含很多参数，这些参数主要在文件visualization_msgs/InteractiveMarker,visualization_msgs/InteractiveMarkerControl and visualization_msgs/InteractiveMarkerFeedback中。创建context menu通常使用MenuHandler interface,从而能够避免处理基础信息，我们要学习五个部分，simple_marker,basic_controls,menu,pong,cube.每个部分用到的源代码在这里代码
3.1 首先学习如何运行这五个部分的程序
首先运行basic_controls该interactive marker server,然后在另一个terminal打开rviz

rosrun interactive_marker_tutorials basic_controls

rosrun rviz rviz

打开之后，1.选择fixed frame为/base_link2.在Display面板下Add一个interactive markers3.设置update topic为/basic_controls/update4.选择上面的Interact选项，这将激活之前设置的interactive elements，并将箭头形状和环形的control块可视化，右键三维模型还可以显示出context menu.5.添加Grid.
3.2 simple_marker
simple_marker是interactive marker server较为简单的server,他的代码如下：

#include 

#include 

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::init(argc, argv, "simple_marker");

  // create an interactive marker server on the topic namespace simple_marker
  interactive_markers::InteractiveMarkerServer server("simple_marker");  //建立server,并命名为simple_server

  // 为该server建立第一个 interactive marker
  visualization_msgs::InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  int_marker.header.stamp=ros::Time::now();
  int_marker.name = "my_marker";
  int_marker.description = "Simple 1-DOF Control";

  // 创建一个 box marker，并在下面将他包含在一个InteractiveMarkerControl中
  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;

  // create a non-interactive control which contains the box
  visualization_msgs::InteractiveMarkerControl box_control;
  box_control.always_visible = true;
  box_control.markers.push_back( box_marker );

  // 把InteractiveMarkerControl包含在Interactive Marker中
  int_marker.controls.push_back( box_control );

  // create a control which will move the box
  // this control does not contain any markers,
  // which will cause RViz to insert two arrows
  //创建另一个InteractiveMarkerControl,也把他放在Interactive Marker中
  visualization_msgs::InteractiveMarkerControl rotate_control;
  rotate_control.name = "move_x";
  rotate_control.interaction_mode =
      visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;

  // add the control to the interactive marker
  int_marker.controls.push_back(rotate_control);

  // add the interactive marker to our collection &
  // tell the server to call processFeedback() when feedback arrives for it
  将我们设定的这个InteractiveMarker插入到sever中，当有来自Client（Rviz)的操作时，调用processFeedback函数
  server.insert(int_marker, &processFeedback);
//仅仅调用insert并不能使Client收到信息，只会使当前的InteractiveMarker处在waiting list中
  // 'commit' changes and send to all clients
  server.applyChanges();  //向所有客户端发送更改的InteractiveMarker

  // start the ROS main loop
  ros::spin();
}

3.3 basic_controls
basic_controls主要是使用不同的方法来操作一系列的InteractiveMarkers。
3.3.1 simple 6-DOF control
使用六个箭头控制box的移动方向，使用ring 控制块控制旋转；

Marker makeBox( InteractiveMarker &msg )  //制作marker大小和颜色的函数
{
  Marker marker;

  marker.type = Marker::CUBE;
  marker.scale.x = msg.scale * 0.45;
  marker.scale.y = msg.scale * 0.45;
  marker.scale.z = msg.scale * 0.45;
  marker.color.r = 0.5;
  marker.color.g = 0.5;
  marker.color.b = 0.5;
  marker.color.a = 1.0;

  return marker;
}

InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )//制作一个InteractiveMarker
{
  InteractiveMarkerControl control;
  control.always_visible = true;
  control.markers.push_back( makeBox(msg) );
  msg.controls.push_back( control );

  return msg.controls.back();
}

3.3.2 simple 6-DOF control（fixed orientation)
与上面唯一不同的是，controls的方向将保持固定，与被控制的frame的方向无关；

void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector3& position, bool show_6dof )  //根据上面的函数，制作6DOF的Marker
{
  InteractiveMarker int_marker;     //生成第一个InteractiveMarker
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "simple_6dof";
  int_marker.description = "Simple 6-DOF Control";

  // insert a box
  makeBoxControl(int_marker);   //调用函数制造一个InteractiveMarkerControl
  int_marker.controls[0].interaction_mode = interaction_mode;//设置交互Marker control下面的interaction_mode为一个变量 

  InteractiveMarkerControl control;

  if ( fixed )   如果为fixed，设置交互控制的orientation_mode为Fixed
  {
    int_marker.description += "\n(fixed orientation)";
    control.orientation_mode = InteractiveMarkerControl::FIXED;
  }

  if (interaction_mode != visualization_msgs::InteractiveMarkerControl::NONE)
  {
      std::string mode_text;
      if( interaction_mode == visualization_msgs::InteractiveMarkerControl::MOVE_3D )         mode_text = "MOVE_3D";
      if( interaction_mode == visualization_msgs::InteractiveMarkerControl::ROTATE_3D )       mode_text = "ROTATE_3D";
      if( interaction_mode == visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D )  mode_text = "MOVE_ROTATE_3D";
      int_marker.name += "_" + mode_text;
      int_marker.description = std::string("3D Control") + (show_6dof ? " + 6-DOF controls" : "") + "\n" + mode_text;
  }

  if(show_6dof)
  {
    control.orientation.w = 1;    //设置坐标系正方向，建立X轴旋转和移动的interaction_mode
    control.orientation.x = 1;
    control.orientation.y = 0;
    control.orientation.z = 0;
    control.name = "rotate_x";
    control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_x";
    control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);

    control.orientation.w = 1; //建立Y轴旋转和移动的interaction_mode
    control.orientation.x = 0;
    control.orientation.y = 1;
    control.orientation.z = 0;
    control.name = "rotate_z";
    control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_z";
    control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);

    control.orientation.w = 1;//建立Z轴旋转和移动的interaction_mode
    control.orientation.x = 0;
    control.orientation.y = 0;
    control.orientation.z = 1;
    control.name = "rotate_y";
    control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_y";
    control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);
  }

  server->insert(int_marker);//将这个InteractiveMarker插入到server中
  server->setCallback(int_marker.name, &processFeedback);//在收到Client(Rviz)的反馈之后，将int_marker的名字传给Rviz,并调用processFeedback函数
  if (interaction_mode != visualization_msgs::InteractiveMarkerControl::NONE)
    menu_handler.apply( *server, int_marker.name );
}

上面两个6自由度的主要区别在于orientation_mode不同，一个是fixed,另一个是使用默认值inherit。而且这些control中都没有插入新的marker，只有一个最初的灰色方块，所以会生成围绕方块的箭头和圆环。默认3D Controls也是使用这些函数所构建的。
3.3.3 任意旋转轴的6DOF

void makeRandomDofMarker( const tf::Vector3& position )//引用旋转轴的位置变量
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "6dof_random_axes";
  int_marker.description = "6-DOF\n(Arbitrary Axes)";

  makeBoxControl(int_marker);//调用函数制造一个InteractiveMarkerControl

  InteractiveMarkerControl control;

  for ( int i=0; i<3; i++ )
  {
    control.orientation.w = rand(-1,1); //给四元数指定任意值决定orientation
    control.orientation.x = rand(-1,1);
    control.orientation.y = rand(-1,1);
    control.orientation.z = rand(-1,1);
    control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;//指定移动的mode
    int_marker.controls.push_back(control);
    control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;//指定旋转的mode
    int_marker.controls.push_back(control);
  }

  server->insert(int_marker);  将该interactiveMarker插入到server中
  server->setCallback(int_marker.name, &processFeedback);在收到Rviz的反馈之后使用name来指定名称，并调用processFeedback函数
}

3.3.4 View-Facing-6DOF
在旋转摄像机到任意一个平面后都能在该平面内作平面移动和旋转，

void makeViewFacingMarker( const tf::Vector3& position )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "view_facing";
  int_marker.description = "View Facing 6-DOF";

  InteractiveMarkerControl control;

  // 建立在摄像机平面旋转的control
  control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  control.orientation.w = 1;
  control.name = "rotate";

  int_marker.controls.push_back(control);
//建立在摄像机平面移动的control
  // create a box in the center which should not be view facing,
  // but move in the camera plane.
  control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
  control.independent_marker_orientation = true;
  control.name = "move";

  control.markers.push_back( makeBox(int_marker) );//将最初创建的marker作为使用的marker
  control.always_visible = true;

  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
}

3.3.5 Quadrocopter
建立一个不能Y，X轴旋转的marker

void makeQuadrocopterMarker( const tf::Vector3& position )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "quadrocopter";
  int_marker.description = "Quadrocopter";

  makeBoxControl(int_marker);

  InteractiveMarkerControl control;

  control.orientation.w = 1;  //建立沿Z轴旋转的Control
  control.orientation.x = 0;
  control.orientation.y = 1;
  control.orientation.z = 0;
  control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
  int_marker.controls.push_back(control);
  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
}

3.3.6 Chess Piece
建立一个自动归位到Grid网格的方块，原理是在Rviz外面运行有basic_controls server,当我们在拖动方块时，将会发送position信息给server，然后server将新的position信息发给Rviz。一旦我们停止拖动，Rviz将应用Update Position.

void makeChessPieceMarker( const tf::Vector3& position )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "chess_piece";
  int_marker.description = "Chess Piece\n(2D Move + Alignment)";

  InteractiveMarkerControl control;

  control.orientation.w = 1;
  control.orientation.x = 0;
  control.orientation.y = 1;
  control.orientation.z = 0;
  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
  int_marker.controls.push_back(control);

  // make a box which also moves in the plane
  control.markers.push_back( makeBox(int_marker) );
  control.always_visible = true;
  int_marker.controls.push_back(control);

  // we want to use our special callback function
  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);

  // set different callback for POSE_UPDATE feedback
  //当有关于int_marker.name方块的操作时，会调用alignMarker函数，并把新的update传输给Rviz
  server->setCallback(int_marker.name, &alignMarker, visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE );
}

void alignMarker( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
  geometry_msgs::Pose pose = feedback->pose; //将当前反馈的position消息进行更新

  pose.position.x = round(pose.position.x-0.5)+0.5;
  pose.position.y = round(pose.position.y-0.5)+0.5;

  ROS_INFO_STREAM( feedback->marker_name << ":"  //在屏幕输出位置的更新信息
      << " aligning position = "
      << feedback->pose.position.x
      << ", " << feedback->pose.position.y
      << ", " << feedback->pose.position.z
      << " to "
      << pose.position.x
      << ", " << pose.position.y
      << ", " << pose.position.z );

  server->setPose( feedback->marker_name, pose );  对反馈的marker，server指定其pose
  server->applyChanges(); 应用更新，从waiting list投入使用
}

3.3.7 Pan/Tilt
方块只沿Z轴和Y轴旋转,不在平面内移动

void makePanTiltMarker( const tf::Vector3& position )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "pan_tilt";
  int_marker.description = "Pan / Tilt";

  makeBoxControl(int_marker);

  InteractiveMarkerControl control;

  control.orientation.w = 1;
  control.orientation.x = 0;
  control.orientation.y = 1;
  control.orientation.z = 0;
  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  control.orientation_mode = InteractiveMarkerControl::FIXED;
  int_marker.controls.push_back(control);

  control.orientation.w = 1;
  control.orientation.x = 0;
  control.orientation.y = 0;
  control.orientation.z = 1;
  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  control.orientation_mode = InteractiveMarkerControl::INHERIT;
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
}

3.3.8 Context Menu
创建一个simple_static menu类型的interactive marker，从而在点击可视化的方块时会有相应的选项操作

void makeMenuMarker( const tf::Vector3& position )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "context_menu";
  int_marker.description = "Context Menu\n(Right Click)";

  InteractiveMarkerControl control;

  control.interaction_mode = InteractiveMarkerControl::MENU;  //interaction_mode为menu
  control.name = "menu_only_control";

  Marker marker = makeBox( int_marker ); //创建marker
  control.markers.push_back( marker );  //将创建的marker包含在control下
  control.always_visible = true;
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
  menu_handler.apply( *server, int_marker.name );
}

3.3.9 Button
这种方块与Menu类似，只是他表明鼠标左键点击方块是实现交互的方式

void makeButtonMarker( const tf::Vector3& position )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "button";
  int_marker.description = "Button\n(Left Click)";

  InteractiveMarkerControl control;

  control.interaction_mode = InteractiveMarkerControl::BUTTON;
  control.name = "button_control";

  Marker marker = makeBox( int_marker );
  control.markers.push_back( marker );
  control.always_visible = true;
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
}

3.3.10 Marker attached to moving frame
方块在相对于当前世界坐标系上下移动，当对方块进行旋转控制时，仍然在保持上下移动，这种interactive marker的Header 的时间戳标签必须是ros::Time(0),从而保证rviz能够收到最及时的tf 信息来转换方块所对应的坐标系信息

void makeMovingMarker( const tf::Vector3& position )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "moving_frame";  //marker建立在一个移动坐标系基础上
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "moving";
  int_marker.description = "Marker Attached to a\nMoving Frame";

  InteractiveMarkerControl control;

  control.orientation.w = 1;
  control.orientation.x = 1;
  control.orientation.y = 0;
  control.orientation.z = 0;
  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  int_marker.controls.push_back(control);

  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
  control.always_visible = true;
  control.markers.push_back( makeBox(int_marker) );
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
}

3.3.11 The surrounding code
针对上面的许多interactive marker,我们建立一个interactiveMarkerServer节点文件来处理这些信息，注意在文件末尾要使用applyChanges来保证对interactive marker所作的改动能够及时生效并被Client所接收使用。
建立的server和Menu interactive marker

boost::shared_ptr server;
interactive_markers::MenuHandler menu_handler;

主函数

int main(int argc, char** argv)
{
  ros::init(argc, argv, "basic_controls");
  ros::NodeHandle n;
 更新base_link和moving_frame之间的tf 变换，tf 变换通过frameCallback函数运行
  // create a timer to update the published transforms
  ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);

  server.reset( new interactive_markers::InteractiveMarkerServer("basic_controls","",false) );

  ros::Duration(0.1).sleep();

  menu_handler.insert( "First Entry", &processFeedback );
  menu_handler.insert( "Second Entry", &processFeedback );
  interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Submenu" );//将做好的一级目录的menu复制给sub_menu_handle，继续对二级目录进行操作
  menu_handler.insert( sub_menu_handle, "First Entry", &processFeedback );
  menu_handler.insert( sub_menu_handle, "Second Entry", &processFeedback );

  tf::Vector3 position;
  position = tf::Vector3(-3, 3, 0);
  make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::NONE, position, true );
  position = tf::Vector3( 0, 3, 0);
  make6DofMarker( true, visualization_msgs::InteractiveMarkerControl::NONE, position, true );
  position = tf::Vector3( 3, 3, 0);
  makeRandomDofMarker( position );
  position = tf::Vector3(-3, 0, 0);
  make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::ROTATE_3D, position, false );
  position = tf::Vector3( 0, 0, 0);
  make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D, position, true );
  position = tf::Vector3( 3, 0, 0);
  make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::MOVE_3D, position, false );
  position = tf::Vector3(-3,-3, 0);
  makeViewFacingMarker( position );
  position = tf::Vector3( 0,-3, 0);
  makeQuadrocopterMarker( position );
  position = tf::Vector3( 3,-3, 0);
  makeChessPieceMarker( position );
  position = tf::Vector3(-3,-6, 0);
  makePanTiltMarker( position );
  position = tf::Vector3( 0,-6, 0);
  makeMovingMarker( position );
  position = tf::Vector3( 3,-6, 0);
  makeMenuMarker( position );
  position = tf::Vector3( 0,-9, 0);
  makeButtonMarker( position );

  server->applyChanges();

  ros::spin();

  server.reset();
}

frameCallback函数的代码如下：

void frameCallback(const ros::TimerEvent&)
{
  static uint32_t counter = 0;

  static tf::TransformBroadcaster br;

  tf::Transform t;

  ros::Time time = ros::Time::now();

  t.setOrigin(tf::Vector3(0.0, 0.0, sin(float(counter)/140.0) * 2.0));
  t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
  br.sendTransform(tf::StampedTransform(t, time, "base_link", "moving_frame"));

  t.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
  t.setRotation(tf::createQuaternionFromRPY(0.0, float(counter)/140.0, 0.0));
  br.sendTransform(tf::StampedTransform(t, time, "base_link", "rotating_frame"));

  counter++;
}

processFeedBack函数用来输出结果到显示器，当Rviz的反馈抵达时.

void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
  std::ostringstream s;
  s << "Feedback from marker '" << feedback->marker_name << "' "
      << " / control '" << feedback->control_name << "'";

  std::ostringstream mouse_point_ss;
  if( feedback->mouse_point_valid )
  {
    mouse_point_ss << " at " << feedback->mouse_point.x
                   << ", " << feedback->mouse_point.y
                   << ", " << feedback->mouse_point.z
                   << " in frame " << feedback->header.frame_id;
  }

  switch ( feedback->event_type )
  {
    case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
      ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." );
      break;

    case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
      ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." );
      break;

    case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
      ROS_INFO_STREAM( s.str() << ": pose changed"
          << "\nposition = "
          << feedback->pose.position.x
          << ", " << feedback->pose.position.y
          << ", " << feedback->pose.position.z
          << "\norientation = "
          << feedback->pose.orientation.w
          << ", " << feedback->pose.orientation.x
          << ", " << feedback->pose.orientation.y
          << ", " << feedback->pose.orientation.z
          << "\nframe: " << feedback->header.frame_id
          << " time: " << feedback->header.stamp.sec << "sec, "
          << feedback->header.stamp.nsec << " nsec" );
      break;

    case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
      ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." );
      break;

    case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
      ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." );
      break;
  }

  server->applyChanges();
}

4.使用Rviz来在3D环境下进行渲染
3D渲染会让场景看起来有深度，为了能够实现这一功能，需要有一个支持四缓冲立体声的graphics card,可能还需要一个有立体功能的monitor或者120HZ的monitor。在LINUX操作系统上，支持这一显卡，关于monitor我们有两个选项：1、Special 3D monitor 2、3D vision glasses
4.1 Special 3D monitor
获得3DVision眼镜附带的特殊显示器，内置3DVision红外发射器。您需要使用DVI双链路电缆将显示器直接连接到显卡。 （单链路DVI无法工作，显示器端口到DVI不起作用.HDMI不能工作在120Hz。）因为显示器配有有源3DVision眼镜，内置红外发射器，因此您不需要单独的红外发射器并且你的显卡上不需要“3 spin mini-din”连接器。
4.2 3D vision glasses
使用有源眼镜和红外发射器（或带有无线电发射器的3DVision-Pro眼镜）获取3DVision套件。 您还需要一台刷新率至少为100Hz（120Hz更好）的显示器。在Linux上，您需要将IR（或无线电）发射器直接连接到显卡。 插入USB的发射器在Linux上不起作用（它们可能在Mac上工作，但我还没试过）。 要直接连接到显卡，请使用3DVision套件随附的“3 spin mini-din to 1/8”立体声电缆。
4.3 设置X来支持stereo
首先确定你正在使用 NVIDIA binary drivers，运行以下命令：

  sudo apt-get install nvidia-settings
  nvidia-settings

之后运行：

  nvidia-settings