SLAM从入门到精通（rviz自定义形状）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        rviz作为很好的上位机调试工具，它本身可以显示很多的传感器数据。比如说lidar、map、tf、camera、点云这些，在rviz上面显示都没有问题。但是有一些数据，我们其实是希望进行自定义显示的。以slam来讲，目前常见的slam就是激光slam和视觉slam。不管哪一种slam，环境的自然特征总没有人工设计的强特征来的稳定，激光slam的稳定特征就是反光柱，而视觉slam的稳定特征就是二维码。但是可惜的是，rviz本身并不支持反光柱的显示和二维码的显示，所以我们完全有必要通过自定义的方法来达成这一目的。

1、rviz的显示方法

        实现的基本方法也是通过编程来实现的。主要是通过visualization_msgs::Marker消息的方法来实现rviz的自定义显示。等Marker定义好了之后，就可以通过marker_pub发布出去。rviz收到这个发布的消息之后，接着就可以在图形界面上显示出来。

2、示例代码basic_shapes.cpp

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
 
int main( int argc, char** argv )
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Rate r(1);
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
 
  // Set our initial shape type to be a cube
  uint32_t shape = visualization_msgs::Marker::CUBE;
 
  while (ros::ok())
  {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "my_frame";
    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.id = 0;
 
    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;
 
    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;
 
    // 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.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.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.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;
 
    marker.lifetime = ros::Duration();
 
    // 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);
 
    // Cycle between different shapes
    switch (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;
    default:
      break;
 
    }
 
    r.sleep();
  }
}

        代码的内容比较简单，就是定义显示一种形状。当然，这种现实纯属于demo性质。实际应用的时候，我们一般会设置固定的形状和大小。并且在状态发生改变的时候，这种形状的颜色，有可能发生改变。

        需要注意的是，很多网上demo中frame_id都修改成了/my_frame，这是不对的。正确的做法应该是my_frame，没有前面的/。不然rviz有可能显示不出来效果。

3、添加编译脚本

add_executable(basic_shapes src/basic_shapes.cpp)
target_link_libraries(basic_shapes ${catkin_LIBRARIES})
add_dependencies(basic_shapes beginner_tutorials_generate_messages_cpp)

4、编译方法

        编译比较简单，就是在workspace的顶层输入catkin_make即可。

5、开始测试

        测试的方法也不复杂。主要过程分成三步来做。第一步，输入roscore；第二步，直接输入rosrun beginner_tutorials basic_shapes；第三步，输入rosrun rviz rviz。把fixed frame设置成my_frame之后，再添加一个Marker，基本上就可以看到我们想看的效果了。