[ROS] how to draw a marker on rviz (node, link, text)

 

 

 

1. 노드 (위 그림의 초록색 원 찍기)

node.type = visualization_msgs::Marker::SPHERE; 일때

가장 쉽게 사용이 가능하다. ros의 visualization_msgs/Marker를 참고한다.

struct Point
{
float x,
float y,
float z
};

int main()
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::MarkerArray>("marker/node", 1);
  
  Point p;
  std::vector<Point> vec_point;
  for(int i=0; i<10; i++)
  {
    p.x = i;
    p.y = i;
    p.z = i;
    vec_point.push_back(p);
  }
  visualization_msgs::MarkerArray node_arr;
  for (size_t i = 0; i < vec_point.size(); i++){
        Point o_node = vec_point[i];

        visualization_msgs::Marker node;
        node.header.frame_id = "/map"; // map frame 기준
        node.header.stamp = ros::Time::now();
        node.type = visualization_msgs::Marker::SPHERE;
        node.id = i;
        node.action = visualization_msgs::Marker::ADD;
        node.pose.orientation.w = 1.0;
        node.pose.position.x =  o_node.x; //노드의 x 좌표
        node.pose.position.y =  o_node.y; //노드의 y 좌표
        // Points are green
        node.color.g = 0.5;
        node.color.a = 1.0;
        node.scale.x = 1.0;
        node.scale.y = 1.0;     
        node_arr.markers.push_back(node);
  }
        marker_pub .publish(node_arr);
}

주의할 점

 

- node의 id를 다르게 해줘야함

- frame_id 기준의 position 위치 기준

- node.color.a 의 값을 꼭 1로 넣어줘야함

- node.pose.orientation.w 의 값을 꼭 넣어줘야함

- node.scale.x, y값을 꼭 넣어줘야함 ! (type 이 SPHERE일때)

 

 

 

 

 

 

2. 노드와 노드 사이의 링크 (위 그림의 초록색 화살표)

struct Point
{
float x,
float y,
float z
};

int main()
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::MarkerArray>("marker/node", 1);
  
  Point p;
  std::vector<Point> vec_point;
  for(int i=0; i<10; i++)
  {
    p.x = i;
    p.y = i;
    p.z = i;
    vec_point.push_back(p);
  }
  visualization_msgs::MarkerArray node_link_arr;
  for (size_t i = 0; i < vec_point.size(); i++){ // 대충 포인트끼리 잇기 위한 for문
    for (size_t j = i+1; j < vec_point.size(); j++)
    {
      visualization_msgs::Marker node_link;
      node_link.header.frame_id = "/map";
      node_link.header.stamp = ros::Time::now();
      node_link.id = i*10+j;
      node_link.action = visualization_msgs::Marker::ADD;
      node_link.pose.orientation.w = 1.0;
      // Points are green
      node_link.color.g = 1.0f;
      node_link.color.a = 1.0;
      node_link.scale.x = 0.1;
      node_link.scale.y = 0.4;
      node_link.type = visualization_msgs::Marker::ARROW;

      geometry_msgs::Point start_p, end_p;
      start_p.x = vec_point[i].x;
      start_p.y = vec_point[i].y;
      node_link.points.push_back(start_p);
      
      end_p.x = vec_point[j].x;
      end_p.y = vec_point[j].y;
      node_link.points.push_back(end_p);

      node_link_arr.markers.push_back(node_link);
    }
  }
  marker_pub .publish(node_link_arr);
}

 

주의할 점

 

- node의 id를 다르게 해줘야함

- frame_id 기준의 position 위치 기준

- node_link.color.a 의 값을 꼭 1로 넣어줘야함

- node_link.pose.orientation.w 의 값을 꼭 넣어줘야함

- node_link의 position 값을 넣어줄 필요 없음

 

- node_link.scale.x : 화살표의 길이

- node_link.scale.y : 화살표 너비

- node_link.scale.z : 화살표 높이

 

- node_link의 points 벡터 배열의 길이는 무조건 0아니면 2가 되야함

- node_link의 points[0] : 화살표의 시작점

- node_link의 points[1] : 화살표의 끝점

 

 

 

 

 

 

3. 노드의 텍스트

struct Point
{
float x,
float y,
float z
};

int main()
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::MarkerArray>("marker/node", 1);
  
  Point p;
  std::vector<Point> vec_point;
  for(int i=0; i<10; i++)
  {
    p.x = i;
    p.y = i;
    p.z = i;
    vec_point.push_back(p);
  }
  visualization_msgs::MarkerArray node_arr;
  for (size_t i = 0; i < vec_point.size(); i++){
        Point o_node = vec_point[i];

        visualization_msgs::Marker node_name;
        node_name.header.frame_id = "/map"; // map frame 기준
        node_name.header.stamp = ros::Time::now();
        node_name.text = std::to_string(i);
        node_name.color.a = 1.0;
        node_name.scale.z = 1.0;
        node_name.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        node_name.id = i;
        node_name.action = visualization_msgs::Marker::ADD;
        node_name.pose.orientation.w = 1.0;
        node_name.pose.position.x =  o_node.x; //노드의 x 좌표
        node_name.pose.position.y =  o_node.y; //노드의 y 좌표

        node_arr.markers.push_back(node_name);
  }
        marker_pub .publish(node_arr);
}

주의할 점

 

- node_name의 id를 다르게 해줘야함

- frame_id 기준의 position 위치 기준

- node_name.color.a 의 값을 꼭 1로 넣어줘야함

- node_name.pose.orientation.w 의 값을 꼭 넣어줘야함

 

- node_name.scale.z : 글자 크기

- node_name.text : 원하는 텍스트 삽입

 

 

4. MarkerArray에 다 담아서 보내기

struct Point
{
float x,
float y,
float z
};

int main()
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::MarkerArray>("marker/node", 1);
  
  Point p;
  std::vector<Point> vec_point;
  for(int i=0; i<10; i++)
  {
    p.x = i;
    p.y = i;
    p.z = i;
    vec_point.push_back(p);
  }
  visualization_msgs::MarkerArray node_link_arr;
  for (size_t i = 0; i < vec_point.size(); i++){ // 대충 포인트끼리 잇기 위한 for문
        Point o_node = vec_point[i];

        visualization_msgs::Marker node;
        node.header.frame_id = "/map"; // map frame 기준
        node.header.stamp = ros::Time::now();
        node.type = visualization_msgs::Marker::SPHERE;
        node.id = i+1;
        node.action = visualization_msgs::Marker::ADD;
        node.pose.orientation.w = 1.0;
        node.pose.position.x =  o_node.x; //노드의 x 좌표
        node.pose.position.y =  o_node.y; //노드의 y 좌표
        // Points are green
        node.color.g = 0.5;
        node.color.a = 1.0;
        node.scale.x = 1.0;
        node.scale.y = 1.0;     
        node_arr.markers.push_back(node);

        visualization_msgs::Marker node_name;
        node_name.header.frame_id = "/map"; // map frame 기준
        node_name.header.stamp = ros::Time::now();
        node_name.text = std::to_string(i);
        node_name.color.a = 1.0;
        node_name.scale.z = 1.0;
        node_name.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        node_name.id = i*10+1;
        node_name.action = visualization_msgs::Marker::ADD;
        node_name.pose.orientation.w = 1.0;
        node_name.pose.position.x =  o_node.x; //노드의 x 좌표
        node_name.pose.position.y =  o_node.y; //노드의 y 좌표

        node_arr.markers.push_back(node_name);
    for (size_t j = i+1; j < vec_point.size(); j++)
    {
      visualization_msgs::Marker node_link;
      node_link.header.frame_id = "/map";
      node_link.header.stamp = ros::Time::now();
      node_link.id = i*100+j;
      node_link.action = visualization_msgs::Marker::ADD;
      node_link.pose.orientation.w = 1.0;
      // Points are green
      node_link.color.g = 1.0f;
      node_link.color.a = 1.0;
      node_link.scale.x = 0.1;
      node_link.scale.y = 0.4;
      node_link.type = visualization_msgs::Marker::ARROW;

      geometry_msgs::Point start_p, end_p;
      start_p.x = vec_point[i].x;
      start_p.y = vec_point[i].y;
      node_link.points.push_back(start_p);
      
      end_p.x = vec_point[j].x;
      end_p.y = vec_point[j].y;
      node_link.points.push_back(end_p);

      node_link_arr.markers.push_back(node_link);
    }
  }
  marker_pub .publish(node_link_arr);
}

 

주의할 점

 

- id를 각각 다르게 해줘야함