diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index a537ffac..bab4451a 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -86,4 +86,10 @@ ouster/os_driver: # - ouster_ros/os_point.h # - ouster_ros/sensor_point_types.h # - ouster_ros/common_point_types.h. - point_type: original \ No newline at end of file + point_type: original + # azimuth window start[optional]: values range [0, 360000] millidegrees + azimuth_window_start: 0 + # azimuth_window_end[optional]: values range [0, 360000] millidegrees + azimuth_window_end: 360000 + # persist_config[optional]: request the sensor to persist settings + persist_config: false diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml index 063774f4..76999cfa 100644 --- a/ouster-ros/config/os_sensor_cloud_image_params.yaml +++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml @@ -15,6 +15,9 @@ ouster/os_sensor: lidar_port: 0 imu_port: 0 use_system_default_qos: False + azimuth_window_start: 0 + azimuth_window_end: 360000 + persist_config: false ouster/os_cloud: ros__parameters: sensor_frame: os_sensor diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 400eec98..46f60ca4 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -22,6 +22,7 @@ using ouster_sensor_msgs::srv::SetConfig; using namespace std::chrono_literals; using namespace std::string_literals; +using std::to_string; namespace ouster_ros { @@ -532,9 +533,9 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { persist_config = get_parameter("persist_config").as_bool(); if (persist_config && (lidar_port == 0 || imu_port == 0)) { - RCLCPP_WARN("When using persist_config it is recommended to not " - "use 0 for port values as this currently will trigger sensor reinit " - "event each time"); + RCLCPP_WARN(get_logger(), "When using persist_config it is recommended " + " to not use 0 for port values as this currently will trigger sensor " + " reinit event each time"); } config.udp_profile_lidar = udp_profile_lidar; @@ -553,7 +554,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); - RCLCPP_ERROR_STREAM(error_msg); + RCLCPP_ERROR_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } @@ -599,7 +600,7 @@ uint8_t OusterSensor::compose_config_flags( } if (persist_config) { - RCLCPP_INFO("Configuration will be persisted"); + RCLCPP_INFO(get_logger(), "Configuration will be persisted"); config_flags |= ouster::sensor::CONFIG_PERSIST; }