diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 37797077..bb635ccd 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -27,7 +27,10 @@ Changelog - RVIZ can't handle image resize - Can't handle points cloud resize properly (erroneous or corrupt PointCloud) - Doesn't detect and handle invalid configurations -* updated ouster_client to the release of ``20240425`` [v0.11.1]; changes listed below. +* Added a new parameter ``organized`` to request publishing unorganized point cloud +* Added a new parameter ``destagger`` to request publishing staggered point cloud +* Added two parameters ``min_range``, ``max_range`` to limit the lidar effective range +* Updated ouster_client to the release of ``20240425`` [v0.11.1]; changes listed below. ouster_client ------------- diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 631aa6ad..ec6127a8 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -160,6 +160,14 @@ inline bool check_token(const std::set& tokens, ouster::util::version parse_version(const std::string& fw_rev); +template +uint64_t ulround(T value) { + T rounded_value = std::round(value); + if (rounded_value < 0) return 0ULL; + if (rounded_value > ULLONG_MAX) return ULLONG_MAX; + return static_cast(rounded_value); +} + } // namespace impl } // namespace ouster_ros diff --git a/launch/common.launch b/launch/common.launch index 1ed5c7d7..8ab62420 100644 --- a/launch/common.launch +++ b/launch/common.launch @@ -41,6 +41,11 @@ xyzir }"/> + + + + + + + + + diff --git a/launch/driver.launch b/launch/driver.launch index cafe1bf7..96a7f8ab 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -5,14 +5,16 @@ - - - - - - + + + + diff --git a/launch/replay.launch b/launch/replay.launch index ed99213d..298a135f 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -12,8 +12,8 @@ - - + + + + + + @@ -93,6 +103,10 @@ + + + + diff --git a/launch/replay_pcap.launch b/launch/replay_pcap.launch index 9f02869f..d1ed6d6b 100644 --- a/launch/replay_pcap.launch +++ b/launch/replay_pcap.launch @@ -6,8 +6,8 @@ - - + + + + + + @@ -86,6 +96,10 @@ + + + + diff --git a/launch/sensor.launch b/launch/sensor.launch index 46ffeb1e..40464beb 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -5,14 +5,16 @@ - - - - + + + + + + @@ -137,6 +150,10 @@ + + + + diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index 963781cc..0b9e33bc 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -9,14 +9,16 @@ start client with active configuration of sensor"/> - - - - - - + + + + + + + + + + + output="screen" required="true" args="manager"/> @@ -108,6 +129,11 @@ + + + @@ -130,6 +156,10 @@ + + + + diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index bcd2b551..2b457dfe 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -47,14 +47,6 @@ uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { return y0 + (x - x0) * sign * (max_v - min_v) / (x1 - x0); } -template -uint64_t ulround(T value) { - T rounded_value = std::round(value); - if (rounded_value < 0) return 0ULL; - if (rounded_value > ULLONG_MAX) return ULLONG_MAX; - return static_cast(rounded_value); -} - } // namespace namespace ouster_ros { @@ -199,7 +191,7 @@ class LidarPacketHandler { last_scan_last_nonzero_idx, last_scan_last_nonzero_value, scan_width + curr_scan_first_nonzero_idx, curr_scan_first_nonzero_value, scan_width); - return ulround(interpolated_value); + return impl::ulround(interpolated_value); } uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, @@ -207,7 +199,7 @@ class LidarPacketHandler { double extrapolated_value = curr_scan_first_nonzero_value - scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; - return ulround(extrapolated_value); + return impl::ulround(extrapolated_value); } // compute_scan_ts_0 for first scan diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 975cfe63..2715b81f 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -164,8 +164,19 @@ class OusterCloud : public nodelet::Nodelet { auto point_type = pnh.param("point_type", std::string{"original"}); auto organized = pnh.param("organized", true); auto destagger = pnh.param("destagger", true); - uint32_t min_range = pnh.param("min_range", 0); - uint32_t max_range = pnh.param("max_range", 1000000); // 1000m + auto min_range_m = pnh.param("min_range", 0.0); + auto max_range_m = pnh.param("max_range", 1000.0); // 1000m + if (min_range_m < 0.0 || max_range_m < 0.0) { + NODELET_FATAL("min_range and max_range need to be positive"); + throw std::runtime_error("negative range limits!"); + } + if (min_range_m >= max_range_m) { + NODELET_FATAL("min_range can't be equal or exceed max_range"); + throw std::runtime_error("min_range equal to or exceeds max_range!"); + } + // convert to millimeters + uint64_t min_range = impl::ulround(min_range_m * 1000); + uint64_t max_range = impl::ulround(max_range_m * 1000); auto rows_step = pnh.param("rows_step", 1); processors.push_back( PointCloudProcessorFactory::create_point_cloud_processor( diff --git a/src/os_replay_nodelet.cpp b/src/os_replay_nodelet.cpp index d913039f..1a7f4a09 100644 --- a/src/os_replay_nodelet.cpp +++ b/src/os_replay_nodelet.cpp @@ -30,7 +30,7 @@ class OusterReplay : public OusterSensorNodeletBase { auto meta_file = getPrivateNodeHandle().param("metadata", std::string{}); if (!is_arg_set(meta_file)) { - NODELET_ERROR("Must specify metadata file in replay mode"); + NODELET_FATAL("Must specify metadata file in replay mode"); throw std::runtime_error("metadata no specificed"); } return meta_file; diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index bd6807d1..93770444 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -128,7 +128,7 @@ std::string OusterSensor::get_sensor_hostname() { auto hostname = nh.param("sensor_hostname", std::string{}); if (!is_arg_set(hostname)) { auto error_msg = "Must specify a sensor hostname"; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } @@ -316,7 +316,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { auto error_msg = "Invalid lidar port number! port value should be in the range " "[0, 65535]."; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } @@ -324,7 +324,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { auto error_msg = "Invalid imu port number! port value should be in the range " "[0, 65535]."; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } @@ -336,7 +336,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { if (!udp_profile_lidar) { auto error_msg = "Invalid udp profile lidar: " + udp_profile_lidar_arg; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } } @@ -347,7 +347,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); if (!lidar_mode) { auto error_msg = "Invalid lidar mode: " + lidar_mode_arg; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } } @@ -367,7 +367,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { if (!timestamp_mode) { auto error_msg = "Invalid timestamp mode: " + timestamp_mode_arg; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } } @@ -417,7 +417,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { azimuth_window_end < MIN_AZW || azimuth_window_end > MAX_AZW) { auto error_msg = "azimuth window values must be between " + to_string(MIN_AZW) + " and " + to_string(MAX_AZW); - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); }