3131#include < utility>
3232
3333#include " rclcpp/rclcpp.hpp"
34- #include " sensor_msgs/msg/point_cloud .hpp"
35- #include " sensor_msgs/msg/channel_float32 .hpp"
34+ #include " sensor_msgs/msg/point_cloud2 .hpp"
35+ #include " sensor_msgs/point_cloud2_iterator .hpp"
3636#include " nav2_voxel_grid/voxel_grid.hpp"
3737#include " nav2_msgs/msg/voxel_grid.hpp"
3838#include " nav2_util/execution_timer.hpp"
@@ -70,8 +70,63 @@ V_Cell g_unknown;
7070
7171rclcpp::Node::SharedPtr g_node;
7272
73- rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr pub_marked;
74- rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr pub_unknown;
73+ rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_marked;
74+ rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_unknown;
75+
76+ /* *
77+ * @brief An helper function to fill pointcloud2 of both the marked and unknown points from voxel_grid
78+ * @param cloud PointCloud2 Ptr which needs to be filled
79+ * @param num_channels Represents the total number of points that are going to be filled
80+ * @param header Carries the header information that needs to be assigned to PointCloud2 header
81+ * @param g_cells contains the x, y, z values that needs to be added to the PointCloud2
82+ */
83+ void pointCloud2Helper (
84+ std::unique_ptr<sensor_msgs::msg::PointCloud2> & cloud,
85+ uint32_t num_channels,
86+ std_msgs::msg::Header header,
87+ V_Cell & g_cells)
88+ {
89+ cloud->header = header;
90+ cloud->width = num_channels;
91+ cloud->height = 1 ;
92+ cloud->is_dense = true ;
93+ cloud->is_bigendian = false ;
94+ sensor_msgs::PointCloud2Modifier modifier (*cloud);
95+
96+ modifier.setPointCloud2Fields (
97+ 6 , " x" 1 , sensor_msgs::msg::PointField::FLOAT32,
98+ " y" 1 , sensor_msgs::msg::PointField::FLOAT32,
99+ " z" 1 , sensor_msgs::msg::PointField::FLOAT32,
100+ " r" 1 , sensor_msgs::msg::PointField::UINT8,
101+ " g" 1 , sensor_msgs::msg::PointField::UINT8,
102+ " b" 1 , sensor_msgs::msg::PointField::UINT8);
103+
104+ sensor_msgs::PointCloud2Iterator<float > iter_x (*cloud, " x"
105+ sensor_msgs::PointCloud2Iterator<float > iter_y (*cloud, " y"
106+ sensor_msgs::PointCloud2Iterator<float > iter_z (*cloud, " z"
107+
108+ sensor_msgs::PointCloud2Iterator<uint8_t > iter_r (*cloud, " r"
109+ sensor_msgs::PointCloud2Iterator<uint8_t > iter_g (*cloud, " g"
110+ sensor_msgs::PointCloud2Iterator<uint8_t > iter_b (*cloud, " b"
111+
112+ for (uint32_t i = 0 ; i < num_channels; ++i) {
113+ Cell & c = g_cells[i];
114+ // assigning value to the point cloud2's iterator
115+ *iter_x = c.x ;
116+ *iter_y = c.y ;
117+ *iter_z = c.z ;
118+ *iter_r = g_colors_r[c.status ] * 255.0 ;
119+ *iter_g = g_colors_g[c.status ] * 255.0 ;
120+ *iter_b = g_colors_b[c.status ] * 255.0 ;
121+
122+ ++iter_x;
123+ ++iter_y;
124+ ++iter_z;
125+ ++iter_r;
126+ ++iter_g;
127+ ++iter_b;
128+ }
129+ }
75130
76131void voxelCallback (const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
77132{
@@ -133,65 +188,19 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
133188 }
134189 }
135190
136- {
137- auto cloud = std::make_unique<sensor_msgs::msg::PointCloud>();
138- cloud->points .resize (num_marked);
139- cloud->channels .resize (1 );
140- cloud->channels [0 ].values .resize (num_marked);
141- cloud->channels [0 ].name = " rgb"
142- cloud->header .frame_id = frame_id;
143- cloud->header .stamp = stamp;
144-
145- sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels [0 ];
146- for (uint32_t i = 0 ; i < num_marked; ++i) {
147- geometry_msgs::msg::Point32 & p = cloud->points [i];
148- float & cval = chan.values [i];
149- Cell & c = g_marked[i];
150-
151- p.x = c.x ;
152- p.y = c.y ;
153- p.z = c.z ;
154-
155- uint32_t r = g_colors_r[c.status ] * 255.0 ;
156- uint32_t g = g_colors_g[c.status ] * 255.0 ;
157- uint32_t b = g_colors_b[c.status ] * 255.0 ;
158- // uint32_t a = g_colors_a[c.status] * 255.0;
159-
160- uint32_t col = (r << 16 ) | (g << 8 ) | b;
161- memcpy (&cval, &col, sizeof col);
162- }
191+ std_msgs::msg::Header pcl_header;
192+ pcl_header.frame_id = frame_id;
193+ pcl_header.stamp = stamp;
163194
195+ {
196+ auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
197+ pointCloud2Helper (cloud, num_marked, pcl_header, g_marked);
164198 pub_marked->publish (std::move (cloud));
165199 }
166200
167201 {
168- auto cloud = std::make_unique<sensor_msgs::msg::PointCloud>();
169- cloud->points .resize (num_unknown);
170- cloud->channels .resize (1 );
171- cloud->channels [0 ].values .resize (num_unknown);
172- cloud->channels [0 ].name = " rgb"
173- cloud->header .frame_id = frame_id;
174- cloud->header .stamp = stamp;
175-
176- sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels [0 ];
177- for (uint32_t i = 0 ; i < num_unknown; ++i) {
178- geometry_msgs::msg::Point32 & p = cloud->points [i];
179- float & cval = chan.values [i];
180- Cell & c = g_unknown[i];
181-
182- p.x = c.x ;
183- p.y = c.y ;
184- p.z = c.z ;
185-
186- uint32_t r = g_colors_r[c.status ] * 255.0 ;
187- uint32_t g = g_colors_g[c.status ] * 255.0 ;
188- uint32_t b = g_colors_b[c.status ] * 255.0 ;
189- // uint32_t a = g_colors_a[c.status] * 255.0;
190-
191- uint32_t col = (r << 16 ) | (g << 8 ) | b;
192- memcpy (&cval, &col, sizeof col);
193- }
194-
202+ auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
203+ pointCloud2Helper (cloud, num_unknown, pcl_header, g_unknown);
195204 pub_unknown->publish (std::move (cloud));
196205 }
197206
@@ -208,9 +217,9 @@ int main(int argc, char ** argv)
208217
209218 RCLCPP_DEBUG (g_node->get_logger (), " Starting up costmap_2d_cloud"
210219
211- pub_marked = g_node->create_publisher <sensor_msgs::msg::PointCloud >(
220+ pub_marked = g_node->create_publisher <sensor_msgs::msg::PointCloud2 >(
212221 " voxel_marked_cloud" 1 );
213- pub_unknown = g_node->create_publisher <sensor_msgs::msg::PointCloud >(
222+ pub_unknown = g_node->create_publisher <sensor_msgs::msg::PointCloud2 >(
214223 " voxel_unknown_cloud" 1 );
215224 auto sub = g_node->create_subscription <nav2_msgs::msg::VoxelGrid>(
216225 " voxel_grid" rclcpp::SystemDefaultsQoS (), voxelCallback);
0 commit comments