From c3be1654a48fd700d41bb6a67ac9d26ee3b3ea5b Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 20 Jul 2023 09:51:07 -0700 Subject: [PATCH] Rename nodelets_os namesapce to ouster_ros + Rename nodelets_os.xml to ouster_ros_nodelet.xml + Wrap all classes with ouster_ros + always default no_bond + Fix Dockerfile build formatting --- CMakeLists.txt | 12 +++--- Dockerfile | 9 +++-- include/ouster_ros/os_ros.h | 34 ++++++++--------- include/ouster_ros/os_sensor_nodelet_base.h | 4 +- launch/common.launch | 4 +- launch/driver.launch | 6 +-- launch/record.launch | 4 +- launch/replay.launch | 4 +- launch/sensor.launch | 6 +-- launch/sensor_mtp.launch | 6 +-- nodelets_os.xml | 27 -------------- ouster_ros_nodelets.xml | 27 ++++++++++++++ package.xml | 2 +- src/image_processor.h | 27 ++++++++------ src/imu_packet_handler.h | 13 ++++--- src/laser_scan_processor.h | 12 +++--- src/lidar_packet_handler.h | 18 +++++---- src/os_cloud_nodelet.cpp | 18 ++++----- src/os_driver_nodelet.cpp | 21 +++++------ src/os_image_nodelet.cpp | 18 ++++----- src/os_replay_nodelet.cpp | 12 +++--- src/os_ros.cpp | 18 ++++----- src/os_sensor_nodelet.cpp | 9 ++--- src/os_sensor_nodelet.h | 7 +--- src/os_sensor_nodelet_base.cpp | 13 ++++--- src/os_transforms_broadcaster.h | 14 +++---- src/point_cloud_processor.h | 13 ++++--- src/thread_safe_ring_buffer.h | 41 +++++++++++---------- 28 files changed, 201 insertions(+), 198 deletions(-) delete mode 100644 nodelets_os.xml create mode 100644 ouster_ros_nodelets.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index fc2c605b..f64c0770 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -77,15 +77,15 @@ target_link_libraries(ouster_ros PUBLIC ${catkin_LIBRARIES} ouster_build pcl_com add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp) # ==== Executables ==== -add_library(nodelets_os +add_library(${PROJECT_NAME}_nodelets src/os_sensor_nodelet_base.cpp src/os_sensor_nodelet.cpp src/os_replay_nodelet.cpp src/os_cloud_nodelet.cpp src/os_image_nodelet.cpp src/os_driver_nodelet.cpp) -target_link_libraries(nodelets_os ouster_ros ${catkin_LIBRARIES}) -add_dependencies(nodelets_os ${PROJECT_NAME}_gencpp) +target_link_libraries(${PROJECT_NAME}_nodelets ouster_ros ${catkin_LIBRARIES}) +add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp) # ==== Test ==== if(CATKIN_ENABLE_TESTING) @@ -96,8 +96,8 @@ endif() # ==== Install ==== install( TARGETS - ouster_ros - nodelets_os + ${PROJECT_NAME} + ${PROJECT_NAME}_nodelets ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -111,7 +111,7 @@ install( install( FILES LICENSE - nodelets_os.xml + ${PROJECT_NAME}_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/Dockerfile b/Dockerfile index fc35c64c..34811a05 100644 --- a/Dockerfile +++ b/Dockerfile @@ -54,11 +54,12 @@ FROM build-env SHELL ["/bin/bash", "-c"] ENV CXXFLAGS="-Werror -Wno-deprecated-declarations" -RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make \ - -DCMAKE_BUILD_TYPE=Release --make-args tests \ - && /opt/ros/$ROS_DISTRO/env.sh catkin_make install +RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make \ +-DCMAKE_BUILD_TYPE=Release \ +&& /opt/ros/$ROS_DISTRO/env.sh catkin_make install -RUN source ./devel/setup.bash && rosrun ouster_ros ouster_ros_test +RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make --make-args tests \ +&& source ./devel/setup.bash && rosrun ouster_ros ouster_ros_test # Entrypoint for running Ouster ros: # diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 604785c6..7f888177 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -17,8 +17,8 @@ #include #include #include -#include #include +#include #include #include @@ -40,13 +40,13 @@ using ns = std::chrono::nanoseconds; bool is_legacy_lidar_profile(const sensor::sensor_info& info); /** - * Gets the number of point cloud returns that this sensor_info object represents + * Gets the number of point cloud returns that this sensor_info object + * represents * @param[in] info sensor_info * @return number of returns */ int get_n_returns(const sensor::sensor_info& info); - /** * Gets the number beams based on supplied sensor_info * @param[in] info sensor_info @@ -71,9 +71,9 @@ std::string topic_for_return(const std::string& topic_base, int return_idx); * @return ROS sensor message with fields populated from the packet */ sensor_msgs::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, - const ros::Time& timestamp, - const std::string& frame, - const uint8_t* buf); + const ros::Time& timestamp, + const std::string& frame, + const uint8_t* buf); /** * Parse an imu packet message into a ROS imu message @@ -84,9 +84,9 @@ sensor_msgs::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, * @return ROS sensor message with fields populated from the packet */ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, - const ros::Time& timestamp, - const std::string& frame, - const sensor::packet_format& pf); + const ros::Time& timestamp, + const std::string& frame, + const sensor::packet_format& pf); /** * Populate a PCL point cloud from a LidarScan. @@ -132,8 +132,8 @@ void scan_to_cloud_f(ouster::PointsF& points, * @return a ROS message containing the point cloud */ sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, - const ros::Time& timestamp, - const std::string& frame); + const ros::Time& timestamp, + const std::string& frame); /** * Convert transformation matrix return by sensor to ROS transform @@ -148,7 +148,6 @@ geometry_msgs::TransformStamped transform_to_tf_msg( const ouster::mat4d& mat, const std::string& frame, const std::string& child_frame, ros::Time timestamp); - /** * Convert transformation matrix return by sensor to ROS transform * @param[in] ls lidar scan object @@ -159,12 +158,9 @@ geometry_msgs::TransformStamped transform_to_tf_msg( * @return ROS message suitable for publishing as a LaserScan */ sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg( - const ouster::LidarScan& ls, - const ros::Time& timestamp, - const std::string &frame, - const ouster::sensor::lidar_mode lidar_mode, - const uint16_t ring, - const int return_index); + const ouster::LidarScan& ls, const ros::Time& timestamp, + const std::string& frame, const ouster::sensor::lidar_mode lidar_mode, + const uint16_t ring, const int return_index); namespace impl { sensor::ChanField suitable_return(sensor::ChanField input_field, bool second); @@ -191,6 +187,6 @@ inline ouster::img_t get_or_fill_zero(sensor::ChanField field, ros::Time ts_to_ros_time(uint64_t ts); -} // namespace impl +} // namespace impl } // namespace ouster_ros diff --git a/include/ouster_ros/os_sensor_nodelet_base.h b/include/ouster_ros/os_sensor_nodelet_base.h index 8dc7822c..44925f10 100644 --- a/include/ouster_ros/os_sensor_nodelet_base.h +++ b/include/ouster_ros/os_sensor_nodelet_base.h @@ -12,7 +12,7 @@ #include -namespace nodelets_os { +namespace ouster_ros { class OusterSensorNodeletBase : public nodelet::Nodelet { protected: @@ -40,4 +40,4 @@ class OusterSensorNodeletBase : public nodelet::Nodelet { ros::Publisher metadata_pub; }; -} // namespace nodelets_os \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/launch/common.launch b/launch/common.launch index 3579fc64..70c3bc9b 100644 --- a/launch/common.launch +++ b/launch/common.launch @@ -34,7 +34,7 @@ + args="load ouster_ros/OusterCloud os_nodelet_mgr $(arg _no_bond)"> @@ -53,7 +53,7 @@ + args="load ouster_ros/OusterImage os_nodelet_mgr $(arg _no_bond)"> diff --git a/launch/driver.launch b/launch/driver.launch index e1010bd1..c3b4f633 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -36,7 +36,7 @@ doc="sets name of choice for the os_lidar tf frame, value can not be empty"/> - @@ -44,7 +44,7 @@ - + @@ -63,7 +63,7 @@ + args="load ouster_ros/OusterDriver os_nodelet_mgr $(arg _no_bond)"> diff --git a/launch/record.launch b/launch/record.launch index 533f3d19..df445780 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -37,7 +37,7 @@ doc="sets name of choice for the os_lidar tf frame, value can not be empty"/> - @@ -67,7 +67,7 @@ + args="load ouster_ros/OusterSensor os_nodelet_mgr"> diff --git a/launch/replay.launch b/launch/replay.launch index 869ba23d..7f2080fb 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -17,7 +17,7 @@ doc="sets name of choice for the os_lidar tf frame, value can not be empty"/> - @@ -49,7 +49,7 @@ + args="load ouster_ros/OusterReplay os_nodelet_mgr"> diff --git a/launch/sensor.launch b/launch/sensor.launch index 961aef6d..1d252b01 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -36,7 +36,7 @@ doc="sets name of choice for the os_lidar tf frame, value can not be empty"/> - @@ -44,7 +44,7 @@ - + @@ -70,7 +70,7 @@ + args="load ouster_ros/OusterSensor os_nodelet_mgr $(arg _no_bond)"> diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index 527c538f..39545c15 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -40,7 +40,7 @@ doc="sets name of choice for the os_lidar tf frame, value can not be empty"/> - @@ -48,7 +48,7 @@ - + @@ -74,7 +74,7 @@ + args="load ouster_ros/OusterSensor os_nodelet_mgr $(arg _no_bond)"> diff --git a/nodelets_os.xml b/nodelets_os.xml deleted file mode 100644 index bde49a70..00000000 --- a/nodelets_os.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - A nodelet that connects to an Ouster sensor and publish incoming imu and lidar packets to other ros nodes. - - - - - A nodelet that can load up existing Ouster recordings and replay them. - - - - - A nodelet that process incoming Ouster lidar packets and publishes a corresponding point cloud. - - - - - A nodelet that processes Ouster point clouds and publish them as depth images. - - - - - A nodelet that combines the capabilities of OusterSensor, OusterCloud and OusterImage into a single nodelet. - - - diff --git a/ouster_ros_nodelets.xml b/ouster_ros_nodelets.xml new file mode 100644 index 00000000..cb7314aa --- /dev/null +++ b/ouster_ros_nodelets.xml @@ -0,0 +1,27 @@ + + + + A nodelet that connects to an Ouster sensor and publish incoming imu and lidar packets to other ros nodes. + + + + + A nodelet that can load up existing Ouster recordings and replay them. + + + + + A nodelet that process incoming Ouster lidar packets and publishes a corresponding point cloud. + + + + + A nodelet that processes Ouster point clouds and publish them as depth images. + + + + + A nodelet that combines the capabilities of OusterSensor, OusterCloud and OusterImage into a single nodelet. + + + diff --git a/package.xml b/package.xml index 2a447544..ae550b51 100644 --- a/package.xml +++ b/package.xml @@ -35,6 +35,6 @@ gtest - + diff --git a/src/image_processor.h b/src/image_processor.h index 09daf7eb..e3eefd8f 100644 --- a/src/image_processor.h +++ b/src/image_processor.h @@ -8,7 +8,7 @@ #pragma once -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -21,6 +21,7 @@ namespace sensor = ouster::sensor; namespace viz = ouster::viz; +namespace ouster_ros { class ImageProcessor { public: using OutputType = @@ -91,18 +92,18 @@ class ImageProcessor { ouster::img_t range = lidar_scan.field(range_channel); - ouster::img_t reflectivity = ouster_ros::impl::get_or_fill_zero( - ouster_ros::impl::suitable_return(sensor::ChanField::REFLECTIVITY, !first), + ouster::img_t reflectivity = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::REFLECTIVITY, !first), lidar_scan); - ouster::img_t signal = ouster_ros::impl::get_or_fill_zero( - ouster_ros::impl::suitable_return(sensor::ChanField::SIGNAL, !first), + ouster::img_t signal = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::SIGNAL, !first), lidar_scan); // TODO: note that near_ir will be processed twice for DUAL return // sensor - ouster::img_t near_ir = ouster_ros::impl::get_or_fill_zero( - ouster_ros::impl::suitable_return(sensor::ChanField::NEAR_IR, !first), + ouster::img_t near_ir = impl::get_or_fill_zero( + impl::suitable_return(sensor::ChanField::NEAR_IR, !first), lidar_scan); uint32_t H = info_.format.pixels_per_column; @@ -110,23 +111,23 @@ class ImageProcessor { // views into message data auto range_image_map = Eigen::Map>( - (pixel_type*)image_msgs[ouster_ros::impl::suitable_return( + (pixel_type*)image_msgs[impl::suitable_return( sensor::ChanField::RANGE, !first)] ->data.data(), H, W); auto signal_image_map = Eigen::Map>( - (pixel_type*)image_msgs[ouster_ros::impl::suitable_return( + (pixel_type*)image_msgs[impl::suitable_return( sensor::ChanField::SIGNAL, !first)] ->data.data(), H, W); auto reflec_image_map = Eigen::Map>( (pixel_type*) - image_msgs[ouster_ros::impl::suitable_return( + image_msgs[impl::suitable_return( sensor::ChanField::REFLECTIVITY, !first)] ->data.data(), H, W); auto nearir_image_map = Eigen::Map>( - (pixel_type*)image_msgs[ouster_ros::impl::suitable_return( + (pixel_type*)image_msgs[impl::suitable_return( sensor::ChanField::NEAR_IR, !first)] ->data.data(), H, W); @@ -147,7 +148,7 @@ class ImageProcessor { for (size_t v = 0; v < W; v++) { const size_t vv = (v + W - px_offset[u]) % W; const size_t idx = u * W + vv; - // TODO: re-examine this truncation later + // TODO: re-examine this truncation later // 16 bit img: use 4mm resolution and throw out returns > 260m auto r = (rg[idx] + 0b10) >> 2; range_image_map(u, v) = r > pixel_value_max ? 0 : r; @@ -193,3 +194,5 @@ class ImageProcessor { viz::AutoExposure nearir_ae, signal_ae, reflec_ae; viz::BeamUniformityCorrector nearir_buc; }; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h index 76f23cb8..2217ea6e 100644 --- a/src/imu_packet_handler.h +++ b/src/imu_packet_handler.h @@ -8,12 +8,14 @@ #pragma once -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" // clang-format on +namespace ouster_ros { + class ImuPacketHandler { public: using HandlerOutput = sensor_msgs::Imu; @@ -30,11 +32,12 @@ class ImuPacketHandler { Timestamper{[](const uint8_t* /*imu_buf*/) { return ros::Time::now(); }} : Timestamper{[pf](const uint8_t* imu_buf) { - return ouster_ros::impl::ts_to_ros_time(pf.imu_gyro_ts(imu_buf)); }}; + return impl::ts_to_ros_time(pf.imu_gyro_ts(imu_buf)); }}; // clang-format on return [&pf, &frame, timestamper](const uint8_t* imu_buf) { - return ouster_ros::packet_to_imu_msg(pf, timestamper(imu_buf), - frame, imu_buf); + return packet_to_imu_msg(pf, timestamper(imu_buf), frame, imu_buf); }; } -}; \ No newline at end of file +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h index 985402ba..ccd184e7 100644 --- a/src/laser_scan_processor.h +++ b/src/laser_scan_processor.h @@ -8,19 +8,17 @@ #pragma once -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" // clang-format on -using ouster_ros::get_n_returns; -using ouster_ros::lidar_scan_to_laser_scan_msg; +namespace ouster_ros { class LaserScanProcessor { public: - using OutputType = - std::vector>; + using OutputType = std::vector>; using PostProcessingFn = std::function; public: @@ -64,4 +62,6 @@ class LaserScanProcessor { uint16_t ring_; OutputType scan_msgs; PostProcessingFn post_processing_fn; -}; \ No newline at end of file +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index aba03064..a9c006e0 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -8,7 +8,7 @@ #pragma once -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -77,8 +77,10 @@ inline bool check_token(const std::set& tokens, namespace sensor = ouster::sensor; -using LidarScanProcessor = std::function; +using LidarScanProcessor = + std::function; + +namespace ouster_ros { class LidarPacketHandler { using LidarPacketAccumlator = std::function; @@ -221,8 +223,8 @@ class LidarPacketHandler { } ros::Time extrapolate_frame_ts(const sensor::packet_format& pf, - const uint8_t* lidar_buf, - const ros::Time current_time) { + const uint8_t* lidar_buf, + const ros::Time current_time) { auto curr_scan_first_arrived_idx = packet_col_index(pf, lidar_buf); auto delta_time = ros::Duration( 0, @@ -235,7 +237,7 @@ class LidarPacketHandler { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); lidar_scan_estimated_msg_ts = - ouster_ros::impl::ts_to_ros_time(lidar_scan_estimated_ts); + impl::ts_to_ros_time(lidar_scan_estimated_ts); return true; } @@ -284,4 +286,6 @@ class LidarPacketHandler { std::vector lidar_scan_handlers; LidarPacketAccumlator lidar_packet_accumlator; -}; \ No newline at end of file +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 99cd7012..b6ea552d 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -6,7 +6,7 @@ * @brief A nodelet to publish point clouds and imu topics */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -33,9 +33,8 @@ #include "laser_scan_processor.h" namespace sensor = ouster::sensor; -using ouster_ros::PacketMsg; -namespace nodelets_os { +namespace ouster_ros { class OusterCloud : public nodelet::Nodelet { public: @@ -119,7 +118,7 @@ class OusterCloud : public nodelet::Nodelet { lidar_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { lidar_pubs[i] = nh.advertise( - ouster_ros::topic_for_return("points", i), 10); + topic_for_return("points", i), 10); } processors.push_back(PointCloudProcessor::create( @@ -138,12 +137,11 @@ class OusterCloud : public nodelet::Nodelet { scan_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { scan_pubs[i] = nh.advertise( - ouster_ros::topic_for_return("scan", i), 10); + topic_for_return("scan", i), 10); } // TODO: avoid duplication in os_cloud_node - int beams_count = - static_cast(ouster_ros::get_beams_count(info)); + int beams_count = static_cast(get_beams_count(info)); int scan_ring = pnh.param("scan_ring", 0); scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1); if (scan_ring != pnh.param("scan_ring", 0)) { @@ -186,7 +184,7 @@ class OusterCloud : public nodelet::Nodelet { std::vector lidar_pubs; std::vector scan_pubs; - ouster_ros::OusterTransformsBroadcaster tf_bcast; + OusterTransformsBroadcaster tf_bcast; ImuPacketHandler::HandlerType imu_packet_handler; LidarPacketHandler::HandlerType lidar_packet_handler; @@ -195,6 +193,6 @@ class OusterCloud : public nodelet::Nodelet { ros::Time last_msg_ts; }; -} // namespace nodelets_os +} // namespace ouster_ros -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterCloud, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterCloud, nodelet::Nodelet) diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index d0e130ac..65d15074 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -7,7 +7,7 @@ * into a single ROS nodelet */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -27,7 +27,7 @@ namespace sensor = ouster::sensor; -namespace nodelets_os { +namespace ouster_ros { class OusterDriver : public OusterSensor { public: @@ -48,7 +48,8 @@ class OusterDriver : public OusterSensor { virtual void create_publishers() override { auto& pnh = getPrivateNodeHandle(); - auto proc_mask = pnh.param("proc_mask", std::string{"IMU|IMG|PCL|SCAN"}); + auto proc_mask = + pnh.param("proc_mask", std::string{"IMU|IMG|PCL|SCAN"}); auto tokens = parse_tokens(proc_mask, '|'); auto timestamp_mode_arg = pnh.param("timestamp_mode", std::string{}); @@ -69,7 +70,7 @@ class OusterDriver : public OusterSensor { lidar_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { lidar_pubs[i] = nh.advertise( - ouster_ros::topic_for_return("points", i), 10); + topic_for_return("points", i), 10); } processors.push_back(PointCloudProcessor::create( @@ -86,12 +87,11 @@ class OusterDriver : public OusterSensor { scan_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { scan_pubs[i] = nh.advertise( - ouster_ros::topic_for_return("scan", i), 10); + topic_for_return("scan", i), 10); } // TODO: avoid duplication in os_cloud_node - int beams_count = - static_cast(ouster_ros::get_beams_count(info)); + int beams_count = static_cast(get_beams_count(info)); int scan_ring = pnh.param("scan_ring", 0); scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1); if (scan_ring != pnh.param("scan_ring", 0)) { @@ -114,7 +114,6 @@ class OusterDriver : public OusterSensor { })); } - if (check_token(tokens, "IMG")) { const std::map channel_field_topic_map_1{ @@ -170,12 +169,12 @@ class OusterDriver : public OusterSensor { std::vector scan_pubs; std::map image_pubs; - ouster_ros::OusterTransformsBroadcaster tf_bcast; + OusterTransformsBroadcaster tf_bcast; ImuPacketHandler::HandlerType imu_packet_handler; LidarPacketHandler::HandlerType lidar_packet_handler; }; -} // namespace nodelets_os +} // namespace ouster_ros -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterDriver, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterDriver, nodelet::Nodelet) diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp index 93442b3a..4b5bd7a3 100644 --- a/src/os_image_nodelet.cpp +++ b/src/os_image_nodelet.cpp @@ -11,7 +11,7 @@ * vision applications, use higher bit depth values in /os_cloud_node/points */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -40,7 +40,7 @@ namespace viz = ouster::viz; using pixel_type = uint16_t; const size_t pixel_value_max = std::numeric_limits::max(); -namespace nodelets_os { +namespace ouster_ros { class OusterImage : public nodelet::Nodelet { private: @@ -58,7 +58,7 @@ class OusterImage : public nodelet::Nodelet { NODELET_INFO("OusterImage: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); create_cloud_object(); - const int n_returns = ouster_ros::get_n_returns(info); + const int n_returns = get_n_returns(info); create_publishers(n_returns); create_subscriptions(n_returns); } @@ -77,15 +77,15 @@ class OusterImage : public nodelet::Nodelet { ros::Publisher a_pub; for (int i = 0; i < n_returns; ++i) { a_pub = nh.advertise( - ouster_ros::topic_for_return("range_image", i), 100); + topic_for_return("range_image", i), 100); range_image_pubs.push_back(a_pub); a_pub = nh.advertise( - ouster_ros::topic_for_return("signal_image", i), 100); + topic_for_return("signal_image", i), 100); signal_image_pubs.push_back(a_pub); a_pub = nh.advertise( - ouster_ros::topic_for_return("reflec_image", i), 100); + topic_for_return("reflec_image", i), 100); reflec_image_pubs.push_back(a_pub); } } @@ -95,7 +95,7 @@ class OusterImage : public nodelet::Nodelet { pc_subs.resize(n_returns); for (int i = 0; i < n_returns; ++i) { pc_subs[i] = nh.subscribe( - ouster_ros::topic_for_return("points", i), 100, + topic_for_return("points", i), 100, [this, i](const sensor_msgs::PointCloud2::ConstPtr msg) { point_cloud_handler(msg, i); }); @@ -206,6 +206,6 @@ class OusterImage : public nodelet::Nodelet { viz::BeamUniformityCorrector nearir_buc; }; -} // namespace nodelets_os +} // namespace ouster_ros -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterImage, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterImage, nodelet::Nodelet) diff --git a/src/os_replay_nodelet.cpp b/src/os_replay_nodelet.cpp index b61b70b3..3cb2e9a3 100644 --- a/src/os_replay_nodelet.cpp +++ b/src/os_replay_nodelet.cpp @@ -9,13 +9,11 @@ #include -#include - #include "ouster_ros/os_sensor_nodelet_base.h" namespace sensor = ouster::sensor; -namespace nodelets_os { +namespace ouster_ros { class OusterReplay : public OusterSensorNodeletBase { private: @@ -29,7 +27,8 @@ class OusterReplay : public OusterSensorNodeletBase { } std::string get_meta_file() const { - auto meta_file = getPrivateNodeHandle().param("metadata", std::string{}); + auto meta_file = + getPrivateNodeHandle().param("metadata", std::string{}); if (!is_arg_set(meta_file)) { NODELET_ERROR("Must specify metadata file in replay mode"); throw std::runtime_error("metadata no specificed"); @@ -48,9 +47,8 @@ class OusterReplay : public OusterSensorNodeletBase { "Error when running in replay mode: " << e.what()); } } - }; -} // namespace nodelets_os +} // namespace ouster_ros -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterReplay, nodelet::Nodelet) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterReplay, nodelet::Nodelet) \ No newline at end of file diff --git a/src/os_ros.cpp b/src/os_ros.cpp index e0e57892..1a8d394e 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -48,9 +48,9 @@ std::string topic_for_return(const std::string& base, int idx) { } sensor_msgs::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, - const ros::Time& timestamp, - const std::string& frame, - const uint8_t* buf) { + const ros::Time& timestamp, + const std::string& frame, + const uint8_t* buf) { sensor_msgs::Imu m; m.header.stamp = timestamp; m.header.frame_id = frame; @@ -84,9 +84,9 @@ sensor_msgs::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, } sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, - const ros::Time& timestamp, - const std::string& frame, - const sensor::packet_format& pf) { + const ros::Time& timestamp, + const std::string& frame, + const sensor::packet_format& pf) { return packet_to_imu_msg(pf, timestamp, frame, pm.buf.data()); } @@ -118,7 +118,7 @@ ros::Time ts_to_ros_time(uint64_t ts) { return t; } -} +} // namespace impl template @@ -260,8 +260,8 @@ void scan_to_cloud_f(ouster::PointsF& points, } sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, - const ros::Time& timestamp, - const std::string& frame) { + const ros::Time& timestamp, + const std::string& frame) { sensor_msgs::PointCloud2 msg{}; pcl::toROSMsg(cloud, msg); msg.header.frame_id = frame; diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index c8b32faa..b7233bc9 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -27,7 +27,7 @@ using nonstd::optional; using namespace std::chrono_literals; using namespace std::string_literals; -namespace nodelets_os { +namespace ouster_ros { void OusterSensor::onInit() { sensor_hostname = get_sensor_hostname(); @@ -474,8 +474,7 @@ void OusterSensor::handle_lidar_packet(sensor::client& cli, bool success = sensor::read_lidar_packet(cli, buffer, pf); if (success) { read_lidar_packet_errors = 0; - if (!ouster_ros::is_legacy_lidar_profile(info) && - init_id_changed(pf, buffer)) { + if (!is_legacy_lidar_profile(info) && init_id_changed(pf, buffer)) { // TODO: short circut reset if no breaking changes occured? NODELET_WARN("sensor init_id has changed! reactivating.."); reactivate_sensor(); @@ -616,6 +615,6 @@ void OusterSensor::reactivate_sensor(bool /*init_id_reset*/) { "sensor reactivate is invoked but sensor it is not implemented"); } -} // namespace nodelets_os +} // namespace ouster_ros -PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterSensor, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterSensor, nodelet::Nodelet) diff --git a/src/os_sensor_nodelet.h b/src/os_sensor_nodelet.h index e35aeb17..6e636ca8 100644 --- a/src/os_sensor_nodelet.h +++ b/src/os_sensor_nodelet.h @@ -25,11 +25,8 @@ #include "thread_safe_ring_buffer.h" namespace sensor = ouster::sensor; -using ouster_ros::GetConfig; -using ouster_ros::SetConfig; -using ouster_ros::PacketMsg; -namespace nodelets_os { +namespace ouster_ros { class OusterSensor : public OusterSensorNodeletBase { private: @@ -156,4 +153,4 @@ class OusterSensor : public OusterSensorNodeletBase { int read_imu_packet_errors = 0; }; -} // namespace nodelets_os +} // namespace ouster_ros diff --git a/src/os_sensor_nodelet_base.cpp b/src/os_sensor_nodelet_base.cpp index b741dc2e..b4681c62 100644 --- a/src/os_sensor_nodelet_base.cpp +++ b/src/os_sensor_nodelet_base.cpp @@ -16,9 +16,8 @@ #include "ouster_ros/GetMetadata.h" namespace sensor = ouster::sensor; -using ouster_ros::GetMetadata; -namespace nodelets_os { +namespace ouster_ros { void OusterSensorNodeletBase::create_get_metadata_service() { auto& nh = getNodeHandle(); @@ -44,7 +43,8 @@ void OusterSensorNodeletBase::publish_metadata() { metadata_pub.publish(metadata_msg); } -void OusterSensorNodeletBase::display_lidar_info(const sensor::sensor_info& info) { +void OusterSensorNodeletBase::display_lidar_info( + const sensor::sensor_info& info) { auto lidar_profile = info.format.udp_profile_lidar; NODELET_INFO_STREAM( "ouster client version: " @@ -55,7 +55,8 @@ void OusterSensorNodeletBase::display_lidar_info(const sensor::sensor_info& info << "lidar udp profile: " << sensor::to_string(lidar_profile)); } -std::string OusterSensorNodeletBase::read_text_file(const std::string& text_file) { +std::string OusterSensorNodeletBase::read_text_file( + const std::string& text_file) { std::ifstream ifs{}; ifs.open(text_file); if (ifs.fail()) return {}; @@ -65,7 +66,7 @@ std::string OusterSensorNodeletBase::read_text_file(const std::string& text_file } bool OusterSensorNodeletBase::write_text_to_file(const std::string& file_path, - const std::string& text) { + const std::string& text) { std::ofstream ofs(file_path); if (!ofs.is_open()) return false; ofs << text << std::endl; @@ -73,4 +74,4 @@ bool OusterSensorNodeletBase::write_text_to_file(const std::string& file_path, return true; } -} // namespace nodelets_os +} // namespace ouster_ros diff --git a/src/os_transforms_broadcaster.h b/src/os_transforms_broadcaster.h index ee03fe68..a3fea44b 100644 --- a/src/os_transforms_broadcaster.h +++ b/src/os_transforms_broadcaster.h @@ -23,7 +23,7 @@ class OusterTransformsBroadcaster { return arg.find_first_not_of(' ') != std::string::npos; } - std::string getName() const { return node_name; } + const std::string& getName() const { return node_name; } public: void parse_parameters(ros::NodeHandle& pnh) { @@ -41,8 +41,8 @@ class OusterTransformsBroadcaster { // validate point_cloud_frame if (!is_arg_set(point_cloud_frame)) { - point_cloud_frame = - sensor_frame; // for ROS1 we'd still use sensor_frame + // for ROS1 we'd still default to sensor_frame + point_cloud_frame = sensor_frame; } else if (point_cloud_frame != sensor_frame && point_cloud_frame != lidar_frame) { NODELET_WARN( @@ -73,17 +73,17 @@ class OusterTransformsBroadcaster { void broadcast_transforms(const sensor::sensor_info& info) { auto now = ros::Time::now(); - static_tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + static_tf_bcast.sendTransform(transform_to_tf_msg( info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now)); - static_tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + static_tf_bcast.sendTransform(transform_to_tf_msg( info.imu_to_sensor_transform, sensor_frame, imu_frame, now)); } void broadcast_transforms(const sensor::sensor_info& info, const ros::Time& ts) { - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + tf_bcast.sendTransform(transform_to_tf_msg( info.lidar_to_sensor_transform, sensor_frame, lidar_frame, ts)); - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + tf_bcast.sendTransform(transform_to_tf_msg( info.imu_to_sensor_transform, sensor_frame, imu_frame, ts)); } diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h index 6763170c..52d8ff2e 100644 --- a/src/point_cloud_processor.h +++ b/src/point_cloud_processor.h @@ -16,12 +16,11 @@ #include "lidar_packet_handler.h" -using ouster_ros::get_n_returns; +namespace ouster_ros { class PointCloudProcessor { public: - using OutputType = - std::vector>; + using OutputType = std::vector>; using PostProcessingFn = std::function; public: @@ -62,8 +61,8 @@ class PointCloudProcessor { void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const ros::Time& msg_ts) { for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { - ouster_ros::scan_to_cloud_f(points, lut_direction, lut_offset, - scan_ts, lidar_scan, cloud, i); + scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, + lidar_scan, cloud, i); pcl_toROSMsg(cloud, *pc_msgs[i]); pc_msgs[i]->header.stamp = msg_ts; pc_msgs[i]->header.frame_id = frame; @@ -101,4 +100,6 @@ class PointCloudProcessor { OutputType pc_msgs; PostProcessingFn post_processing_fn; -}; \ No newline at end of file +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/thread_safe_ring_buffer.h b/src/thread_safe_ring_buffer.h index 170600a4..26b5c88d 100644 --- a/src/thread_safe_ring_buffer.h +++ b/src/thread_safe_ring_buffer.h @@ -8,21 +8,22 @@ #pragma once -#include -#include -#include #include +#include +#include /** * @class ThreadSafeRingBuffer thread safe ring buffer. */ class ThreadSafeRingBuffer { -public: - ThreadSafeRingBuffer(size_t item_size_, size_t items_count_) : - buffer(item_size_ * items_count_), item_size(item_size_), - max_items_count(items_count_), active_items_count(0), - write_idx(0), read_idx(0) { - } + public: + ThreadSafeRingBuffer(size_t item_size_, size_t items_count_) + : buffer(item_size_ * items_count_), + item_size(item_size_), + max_items_count(items_count_), + active_items_count(0), + write_idx(0), + read_idx(0) {} /** * Gets the maximum number of items that this ring buffer can hold. @@ -32,7 +33,7 @@ class ThreadSafeRingBuffer { /** * Gets the number of item that currently occupy the ring buffer. This * number would vary between 0 and the capacity(). - * + * * @remarks * if returned value was 0 or the value was equal to the buffer capacity(), * this does not guarantee that a subsequent call to read() or write() @@ -45,7 +46,7 @@ class ThreadSafeRingBuffer { /** * Checks if the ring buffer is empty. - * + * * @remarks * if empty() returns true this does not guarantee that calling the write() * operation directly right after wouldn't block the calling thread. @@ -57,7 +58,7 @@ class ThreadSafeRingBuffer { /** * Checks if the ring buffer is full. - * + * * @remarks * if full() returns true this does not guarantee that calling the read() * operation directly right after wouldn't block the calling thread. @@ -68,20 +69,20 @@ class ThreadSafeRingBuffer { } /** - * Writes to the buffer safely, the method will keep blocking until the there - * is a space available within the buffer. + * Writes to the buffer safely, the method will keep blocking until the + * there is a space available within the buffer. */ template void write(BufferWriteFn&& buffer_write) { std::unique_lock lock(mutex); - fullCondition.wait(lock, [this] { return active_items_count < capacity(); }); + fullCondition.wait(lock, + [this] { return active_items_count < capacity(); }); buffer_write(&buffer[write_idx * item_size]); write_idx = (write_idx + 1) % capacity(); ++active_items_count; emptyCondition.notify_one(); } - /** * Writes to the buffer safely, if there is not space left then this method * will overite the last item. @@ -118,9 +119,11 @@ class ThreadSafeRingBuffer { * inaccessible the method will timeout and buffer_read gets a nullptr. */ template - void read_timeout(BufferReadFn&& buffer_read, std::chrono::seconds timeout) { + void read_timeout(BufferReadFn&& buffer_read, + std::chrono::seconds timeout) { std::unique_lock lock(mutex); - if (emptyCondition.wait_for(lock, timeout, [this] { return active_items_count > 0; })) { + if (emptyCondition.wait_for( + lock, timeout, [this] { return active_items_count > 0; })) { buffer_read(&buffer[read_idx * item_size]); read_idx = (read_idx + 1) % capacity(); --active_items_count; @@ -130,7 +133,7 @@ class ThreadSafeRingBuffer { } } -private: + private: std::vector buffer; size_t item_size; size_t max_items_count;