Corentin Chauvin-Hameau – 2019-2020
Coverage Path Planning for an underwater robot surveying a marine farm
rviz_visualisation.cpp
Go to the documentation of this file.
1 /**
2  * @file
3  *
4  * \brief Definition of common Rviz display functions
5  * \author Corentin Chauvin-Hameau
6  * \date 2019
7  */
8 
9 #include "rviz_visualisation.hpp"
10 #include <visualization_msgs/Marker.h>
11 #include <visualization_msgs/MarkerArray.h>
12 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
13 #include <tf2/LinearMath/Vector3.h>
14 #include <ros/ros.h>
15 
16 
17 namespace mfcpp {
18 
19 void fill_marker_header(visualization_msgs::Marker &marker,
20  const MarkerArgs &common_args)
21 {
22  marker.header.frame_id = common_args.frame_id;
23  marker.header.stamp = common_args.stamp;
24  marker.ns = common_args.ns;
25  marker.lifetime = common_args.duration;
26 }
27 
28 
29 visualization_msgs::Marker rviz_marker_spheres(float diameter,
30  const MarkerArgs &common_args)
31 {
32  visualization_msgs::Marker marker;
33  fill_marker_header(marker, common_args);
34  marker.color = common_args.color;
35  marker.scale.x = diameter;
36  marker.scale.y = diameter;
37  marker.scale.z = diameter;
38 
39  marker.type = visualization_msgs::Marker::SPHERE_LIST;
40  marker.action = visualization_msgs::Marker::ADD;
41 
42  return marker;
43 }
44 
45 
46 visualization_msgs::Marker rviz_marker_line(tf2::Vector3 p1, tf2::Vector3 p2,
47  float thickness, const MarkerArgs &common_args)
48 {
49  visualization_msgs::Marker marker;
50  fill_marker_header(marker, common_args);
51  marker.color = common_args.color;
52 
53  marker.type = visualization_msgs::Marker::LINE_LIST;
54  marker.action = visualization_msgs::Marker::ADD;
55 
56  geometry_msgs::Point p;
57  p.x = p1.getX();
58  p.y = p1.getY();
59  p.z = p1.getZ();
60  marker.points.emplace_back(p);
61  p.x = p2.getX();
62  p.y = p2.getY();
63  p.z = p2.getZ();
64  marker.points.emplace_back(p);
65 
66  marker.scale.x = thickness;
67 
68  return marker;
69 }
70 
71 
72 visualization_msgs::Marker rviz_marker_line(float thickness,
73  const MarkerArgs &common_args)
74 {
75  visualization_msgs::Marker marker;
76  fill_marker_header(marker, common_args);
77  marker.color = common_args.color;
78  marker.scale.x = thickness;
79 
80  marker.type = visualization_msgs::Marker::LINE_LIST;
81  marker.action = visualization_msgs::Marker::ADD;
82 
83  return marker;
84 }
85 
86 
87 visualization_msgs::Marker rviz_marker_cylinder(tf2::Vector3 p, float diameter,
88  float height, const MarkerArgs &common_args)
89 {
90  visualization_msgs::Marker marker;
91  fill_marker_header(marker, common_args);
92  marker.color = common_args.color;
93 
94  marker.type = visualization_msgs::Marker::CYLINDER;
95  marker.action = visualization_msgs::Marker::ADD;
96 
97  marker.pose.position.x = p.getX();
98  marker.pose.position.y = p.getY();
99  marker.pose.position.z = p.getZ();
100 
101  marker.scale.x = diameter;
102  marker.scale.y = diameter;
103  marker.scale.z = height;
104 
105  return marker;
106 }
107 
108 
109 visualization_msgs::Marker rviz_marker_rect(const MarkerArgs &common_args)
110 {
111  visualization_msgs::Marker marker;
112  fill_marker_header(marker, common_args);
113  marker.color = common_args.color;
114 
115  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
116  marker.action = visualization_msgs::Marker::ADD;
117 
118  marker.scale.x = 1;
119  marker.scale.y = 1;
120  marker.scale.z = 1;
121 
122  return marker;
123 }
124 
125 
126 visualization_msgs::Marker rviz_marker_triangles(const MarkerArgs &common_args)
127 {
128  visualization_msgs::Marker marker;
129  fill_marker_header(marker, common_args);
130  marker.color = common_args.color;
131 
132  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
133  marker.action = visualization_msgs::Marker::ADD;
134 
135  marker.scale.x = 1;
136  marker.scale.y = 1;
137  marker.scale.z = 1;
138 
139  return marker;
140 }
141 
142 
143 visualization_msgs::Marker rviz_marker_rect(tf2::Vector3 p1, tf2::Vector3 p2,
144  tf2::Vector3 p3, tf2::Vector3 p4, const MarkerArgs &common_args)
145 {
146  visualization_msgs::Marker marker;
147  fill_marker_header(marker, common_args);
148  marker.color = common_args.color;
149 
150  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
151  marker.action = visualization_msgs::Marker::ADD;
152 
153  marker.points.reserve(6);
154  geometry_msgs::Point point;
155 
156  marker.points.emplace_back(tf2::toMsg(p1, point)); // first triangle
157  marker.points.emplace_back(tf2::toMsg(p2, point));
158  marker.points.emplace_back(tf2::toMsg(p4, point));
159 
160  marker.points.emplace_back(tf2::toMsg(p2, point)); // second triangle
161  marker.points.emplace_back(tf2::toMsg(p3, point));
162  marker.points.emplace_back(tf2::toMsg(p4, point));
163 
164  std_msgs::ColorRGBA color;
165  color.r = 0.0;
166  color.g = 1.0;
167  color.b = 0.0;
168  color.a = 1.0;
169  marker.colors.resize(2, color);
170  marker.color.a = 1.0;
171  marker.scale.x = 1;
172  marker.scale.y = 1;
173  marker.scale.z = 1;
174 
175  return marker;
176 }
177 
178 
179 void pop_marker_ids(visualization_msgs::MarkerArray &array)
180 {
181  unsigned int n = array.markers.size();
182 
183  for (unsigned int i = 0; i < n; i++) {
184  array.markers[i].id = i;
185  }
186 }
187 
188 
189 } // namespace mfcpp
mfcpp
Definition: common.hpp:23
mfcpp::rviz_marker_line
visualization_msgs::Marker rviz_marker_line(float thickness, const MarkerArgs &common_args)
Creates a blank Rviz marker to display lines.
Definition: rviz_visualisation.cpp:72
mfcpp::MarkerArgs
Arguments used to create a Rviz maker.
Definition: rviz_visualisation.hpp:23
mfcpp::MarkerArgs::stamp
ros::Time stamp
Time stamp for the ROS message.
Definition: rviz_visualisation.hpp:25
mfcpp::pop_marker_ids
void pop_marker_ids(visualization_msgs::MarkerArray &array)
Populates id of all the markers in a marker array.
Definition: rviz_visualisation.cpp:179
mfcpp::MarkerArgs::frame_id
std::string frame_id
Frame in which position/orientation of the object is.
Definition: rviz_visualisation.hpp:26
rviz_visualisation.hpp
Declaration of common Rviz display functions.
mfcpp::rviz_marker_rect
visualization_msgs::Marker rviz_marker_rect(const MarkerArgs &common_args)
Creates a blank Rviz marker to display rectangles.
Definition: rviz_visualisation.cpp:109
mfcpp::rviz_marker_cylinder
visualization_msgs::Marker rviz_marker_cylinder(tf2::Vector3 p, float diameter, float height, const MarkerArgs &common_args)
Creates a Rviz marker to display a cylinder.
Definition: rviz_visualisation.cpp:87
mfcpp::fill_marker_header
void fill_marker_header(visualization_msgs::Marker &marker, const MarkerArgs &common_args)
Fill header of marker.
Definition: rviz_visualisation.cpp:19
keyboard_teleop.n
int n
Definition: keyboard_teleop.py:76
mfcpp::rviz_marker_triangles
visualization_msgs::Marker rviz_marker_triangles(const MarkerArgs &common_args)
Creates a blank Rviz marker to display triangles.
Definition: rviz_visualisation.cpp:126
mfcpp::MarkerArgs::duration
ros::Duration duration
Duration of the marker (in sec)
Definition: rviz_visualisation.hpp:28
mfcpp::rviz_marker_spheres
visualization_msgs::Marker rviz_marker_spheres(float thickness, const MarkerArgs &common_args)
Creates a blank Rviz marker to display spheres.
Definition: rviz_visualisation.cpp:29
mfcpp::MarkerArgs::ns
std::string ns
Namespace for the Rviz marker.
Definition: rviz_visualisation.hpp:27
mfcpp::MarkerArgs::color
std_msgs::ColorRGBA color
Color of the marker.
Definition: rviz_visualisation.hpp:29
Generated by   doxygen 1.8.11