From b82a5d98028bc44752d0d8ea65ffb2f6d75310a8 Mon Sep 17 00:00:00 2001 From: Dima Garbuzov Date: Fri, 28 May 2021 23:48:44 +0000 Subject: [PATCH] Publish 16-bit images and avoid rviz bug on noetic (MR !2092) A mesa update has triggered a bug in rviz on intel systems[0]. Doesn't seem like it'll get fixed soon, so this changes the default point display type to "flat squares." It doesn't look great close-up, but works reasonably well for visualizing sample data and room-sized scenes. * Also rearrange the rviz window config a bit to be more useful * Update published images to have 16-bit dpeth * Use the usual method ot fit range values into 16 bits * Rename intensity/ambient topics and variables in the img_node * Add reflectivity image output to the img_node [0] https://github.com/ros-visualization/rviz/issues/1508 Approved-by: Chris Bayruns Approved-by: Pavlo Bashmakov --- ouster_ros/src/img_node.cpp | 152 +++++++++++++++++------------------- ouster_ros/viz.rviz | 91 ++++++++++++--------- 2 files changed, 129 insertions(+), 114 deletions(-) diff --git a/ouster_ros/src/img_node.cpp b/ouster_ros/src/img_node.cpp index 4ce7bf56..108e4a73 100644 --- a/ouster_ros/src/img_node.cpp +++ b/ouster_ros/src/img_node.cpp @@ -1,8 +1,8 @@ /** * @file - * @brief Example node to visualize range, ambient and intensity images + * @brief Example node to visualize range, near ir and signal images * - * Publishes ~/range_image, ~/ambient_image, and ~/intensity_image. Please bear + * Publishes ~/range_image, ~/nearir_image, and ~/signal_image. Please bear * in mind that there is rounding/clamping to display 8 bit images. For computer * vision applications, use higher bit depth values in /os_cloud_node/points */ @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -31,11 +32,21 @@ namespace sensor = ouster::sensor; namespace viz = ouster::viz; -using pixel_type = uint8_t; -constexpr size_t bit_depth = 8 * sizeof(pixel_type); +using pixel_type = uint16_t; const size_t pixel_value_max = std::numeric_limits::max(); -constexpr double range_multiplier = - 1.0 / 200.0; // assuming 200 m range typical + +sensor_msgs::ImagePtr make_image_msg(size_t H, size_t W, + const ros::Time& stamp) { + sensor_msgs::ImagePtr msg{new sensor_msgs::Image{}}; + msg->width = W; + msg->height = H; + msg->step = W * sizeof(pixel_type); + msg->encoding = sensor_msgs::image_encodings::MONO16; + msg->data.resize(W * H * sizeof(pixel_type)); + msg->header.stamp = stamp; + + return msg; +} int main(int argc, char** argv) { ros::init(argc, argv, "img_node"); @@ -57,98 +68,81 @@ int main(int argc, char** argv) { ros::Publisher range_image_pub = nh.advertise("range_image", 100); - ros::Publisher ambient_image_pub = - nh.advertise("ambient_image", 100); - ros::Publisher intensity_image_pub = - nh.advertise("intensity_image", 100); + ros::Publisher nearir_image_pub = + nh.advertise("nearir_image", 100); + ros::Publisher signal_image_pub = + nh.advertise("signal_image", 100); + ros::Publisher reflec_image_pub = + nh.advertise("reflec_image", 100); ouster_ros::Cloud cloud{}; - viz::AutoExposure ambient_ae, intensity_ae; - viz::BeamUniformityCorrector ambient_buc; + viz::AutoExposure nearir_ae, signal_ae, reflec_ae; + viz::BeamUniformityCorrector nearir_buc; - std::stringstream encoding_ss; - encoding_ss << "mono" << bit_depth; - std::string encoding = encoding_ss.str(); + ouster::img_t nearir_image_eigen(H, W); + ouster::img_t signal_image_eigen(H, W); + ouster::img_t reflec_image_eigen(H, W); auto cloud_handler = [&](const sensor_msgs::PointCloud2::ConstPtr& m) { pcl::fromROSMsg(*m, cloud); - sensor_msgs::Image range_image; - sensor_msgs::Image ambient_image; - sensor_msgs::Image intensity_image; - - range_image.width = W; - range_image.height = H; - range_image.step = W; - range_image.encoding = encoding; - range_image.data.resize(W * H * bit_depth / - (8 * sizeof(*range_image.data.data()))); - range_image.header.stamp = m->header.stamp; - - ambient_image.width = W; - ambient_image.height = H; - ambient_image.step = W; - ambient_image.encoding = encoding; - ambient_image.data.resize(W * H * bit_depth / - (8 * sizeof(*ambient_image.data.data()))); - ambient_image.header.stamp = m->header.stamp; - - intensity_image.width = W; - intensity_image.height = H; - intensity_image.step = W; - intensity_image.encoding = encoding; - intensity_image.data.resize(W * H * bit_depth / - (8 * sizeof(*intensity_image.data.data()))); - intensity_image.header.stamp = m->header.stamp; - - ouster::img_t ambient_image_eigen(H, W); - ouster::img_t intensity_image_eigen(H, W); - + auto range_image = make_image_msg(H, W, m->header.stamp); + auto nearir_image = make_image_msg(H, W, m->header.stamp); + auto signal_image = make_image_msg(H, W, m->header.stamp); + auto reflec_image = make_image_msg(H, W, m->header.stamp); + + // views into message data + auto range_image_map = Eigen::Map>( + (pixel_type*)range_image->data.data(), H, W); + auto nearir_image_map = Eigen::Map>( + (pixel_type*)nearir_image->data.data(), H, W); + auto signal_image_map = Eigen::Map>( + (pixel_type*)signal_image->data.data(), H, W); + auto reflec_image_map = Eigen::Map>( + (pixel_type*)reflec_image->data.data(), H, W); + + // copy data out of Cloud message, with destaggering for (size_t u = 0; u < H; u++) { for (size_t v = 0; v < W; v++) { const size_t vv = (v + W - px_offset[u]) % W; - const size_t index = u * W + vv; - const auto& pt = cloud[index]; - - if (pt.range == 0) { - reinterpret_cast( - range_image.data.data())[u * W + v] = 0; - } else { - reinterpret_cast( - range_image.data.data())[u * W + v] = - pixel_value_max - - std::min(std::round(pt.range * range_multiplier), - static_cast(pixel_value_max)); - } - ambient_image_eigen(u, v) = pt.ambient; - intensity_image_eigen(u, v) = pt.intensity; - } - } + const auto& pt = cloud[u * W + vv]; - ambient_buc(ambient_image_eigen); - ambient_ae(ambient_image_eigen); - intensity_ae(intensity_image_eigen); - ambient_image_eigen = ambient_image_eigen.sqrt(); - intensity_image_eigen = intensity_image_eigen.sqrt(); - for (size_t u = 0; u < H; u++) { - for (size_t v = 0; v < W; v++) { - reinterpret_cast( - ambient_image.data.data())[u * W + v] = - ambient_image_eigen(u, v) * pixel_value_max; - reinterpret_cast( - intensity_image.data.data())[u * W + v] = - intensity_image_eigen(u, v) * pixel_value_max; + // 16 bit img: use 4mm resolution and throw out returns > 260m + auto r = (pt.range + 0b10) >> 2; + range_image_map(u, v) = r > pixel_value_max ? 0 : r; + + nearir_image_eigen(u, v) = pt.ambient; + signal_image_eigen(u, v) = pt.intensity; + reflec_image_eigen(u, v) = pt.reflectivity; } } + // image processing + nearir_buc(nearir_image_eigen); + nearir_ae(nearir_image_eigen); + signal_ae(signal_image_eigen); + reflec_ae(reflec_image_eigen); + nearir_image_eigen = nearir_image_eigen.sqrt(); + signal_image_eigen = signal_image_eigen.sqrt(); + + // copy data into image messages + nearir_image_map = + (nearir_image_eigen * pixel_value_max).cast(); + signal_image_map = + (signal_image_eigen * pixel_value_max).cast(); + reflec_image_map = + (reflec_image_eigen * pixel_value_max).cast(); + + // publish range_image_pub.publish(range_image); - ambient_image_pub.publish(ambient_image); - intensity_image_pub.publish(intensity_image); + nearir_image_pub.publish(nearir_image); + signal_image_pub.publish(signal_image); + reflec_image_pub.publish(reflec_image); }; auto pc_sub = - nh.subscribe("points", 500, cloud_handler); + nh.subscribe("points", 100, cloud_handler); ros::spin(); return EXIT_SUCCESS; diff --git a/ouster_ros/viz.rviz b/ouster_ros/viz.rviz index d001a450..763a7e95 100644 --- a/ouster_ros/viz.rviz +++ b/ouster_ros/viz.rviz @@ -8,8 +8,8 @@ Panels: - /Status1 - /PointCloud21 - /TF1/Frames1 - Splitter Ratio: 0.5 - Tree Height: 573 + Splitter Ratio: 0.5654885768890381 + Tree Height: 756 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -18,7 +18,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -29,6 +29,10 @@ Panels: Name: Time SyncMode: 0 SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: false +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -36,9 +40,9 @@ Visualization Manager: Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 - Enabled: true + Enabled: false Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -49,73 +53,85 @@ Visualization Manager: Plane: XY Plane Cell Count: 10 Reference Frame: - Value: true + Value: false - Alpha: 1 - Autocompute Intensity Bounds: true + Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z - Channel Name: intensity + Channel Name: z Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true - Invert Rainbow: false + Invert Rainbow: true Max Color: 255; 255; 255 - Max Intensity: 1724 + Max Intensity: 15 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: -10 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true - Size (Pixels): 3 - Size (m): 0.00999999978 - Style: Points + Size (Pixels): 2 + Size (m): 0.029999999329447746 + Style: Flat Squares Topic: /os_cloud_node/points Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/Image - Enabled: true - Image Topic: /img_node/ambient_image + Enabled: false + Image Topic: /img_node/nearir_image Max Value: 1 Median window: 5 Min Value: 0 - Name: ambient + Name: near ir Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Class: rviz/Image Enabled: true - Image Topic: /img_node/intensity_image + Image Topic: /img_node/signal_image Max Value: 1 Median window: 5 Min Value: 0 - Name: intensity + Name: signal Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/Image - Enabled: true - Image Topic: /img_node/range_image + Enabled: false + Image Topic: /img_node/reflec_image Max Value: 1 Median window: 5 Min Value: 0 + Name: reflectivity + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /img_node/range_image + Max Value: 1 + Median window: 50 + Min Value: 0 Name: range Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Class: rviz/TF Enabled: true Frame Timeout: 15 @@ -155,7 +171,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -165,9 +184,9 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10.2322998 + Distance: 45.77256774902344 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false @@ -176,22 +195,22 @@ Visualization Manager: Y: 0 Z: 0 Focal Shape Fixed Size: false - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.595398128 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7153978943824768 Target Frame: Value: Orbit (rviz) - Yaw: 2.49040008 + Yaw: 4.228582382202148 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1051 + Height: 1080 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000003170000052ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000005f0000000180000000000000000fb0000000800480065006c007000000002540000046e000000000000000000000001000002a8000003c7fc0200000008fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00720061006e00670065010000003a000000620000001600fffffffb000000120069006e00740065006e007300690074007901000000a2000000730000001600fffffffb0000000e0061006d006200690065006e0074010000011b000000690000001600fffffffb000000100044006900730070006c006100790073010000018a00000277000000c600fffffffb000000100044006900730070006c0061007900730100000189000002d10000000000000000fb0000000a00560069006500770073000000036d000002060000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200001001000000a7fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000082b00000043fc0100000001fc000000000000082b0000000000fffffffa000000010200000002fb0000000800540069006d00650000000000ffffffff0000003600fffffffb0000000800540069006d006501000006c90000003a0000000000000000000004d2000003c700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -202,10 +221,12 @@ Window Geometry: collapsed: false Width: 1920 X: 0 - Y: 29 - ambient: - collapsed: false - intensity: + Y: 1151 + near ir: collapsed: false range: collapsed: false + reflectivity: + collapsed: false + signal: + collapsed: false