通过Marker 在rviz中加入 历史轨迹点

实现官网例程 rviz/Tutorials/Markers: Basic Shapes

官网地址

与其他显示方式不同，Markers 显示 可以在rviz中可视化数据，而rviz不用了解有关解释该数据的任何信息。 相反，原始对象通过visualisation_msgs / Marker消息发送到显示器，该消息使您可以显示箭头，框，球和线之类的内容。

将创建一个程序 发送一个新的maeker每秒，替换上一个用不同的形状。

创建功能包

catkin_create_pkg using_markers roscpp visualization_msgs

发送 Markers

直接 上 代码

#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;
    }

    r.sleep();
  }
}

修改 CMakeLists.txt

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

运行

rosrun using_markers basic_shapes

rosrun rviz rviz

一上来会没有 ，把 Fixed Frame 改成 my_frame

添加 Marker

就有了
一秒换一个 立体

ok 实现完了 看看代码

visualization_msgs/Marker Message 定义

visualization_msgs/Marker Message 定义 官网

例程代码解释

需 包含 visualization_msgs/Marker.h 消息定义文件

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

初始化ROS，创建 NodeHandle ，循环频率 及 发布 句柄 赋值，
往 visualization_marker topic 上发布 只能往这上面发 rviz 只订阅这个

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);

声明 要发布的形状变量 CUBE 还有其它的可以看 visualization_msgs::Marker msg定义

  uint32_t shape = visualization_msgs::Marker::CUBE;

创建一个 visualization_msgs::Marker 变量 叫 marker

赋值 marker的 内容 frame_id “/my_frame” stamp ros::Time::now()

  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();

给marker 设定 namespace命名空间 和id
这里要注意 ： 加入 命名空间和 id 全部一样的话 ，那么 新的就会覆盖旧的

    marker.ns = "basic_shapes";
    marker.id = 0;

给 marker 设定 type ，type就是 哪种 指定的marker ，在msg 里有 枚举 ， 正方体、球、圆柱等

 marker.type = shape;

给 marker 设定 action 这个是要对marker 做什么，ADD就是 加 DELETE就是减

 marker.action = visualization_msgs::Marker::ADD;

设定 maker 的 pose 位置和方向

    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;

指定maker 的比例。 对于基本形状，在所有方向上为1的比例表示一侧为1米。

    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

maker 的颜色

    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

指的是marker 的 存在时间
ros::Duration() 就是一直存在

    marker.lifetime = ros::Duration();

检测订阅数量 不足1个提示 ，，

    // 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);

更换形状

    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;
    }

延时 循环结束

    r.sleep();
  }