From e812b4dfd240b519c02d8ab234f1b736dbc42933 Mon Sep 17 00:00:00 2001 From: Bernardo Taveira Date: Thu, 7 Apr 2022 17:06:12 +0200 Subject: [PATCH 1/7] add azimuth window parameter --- ros2_ouster/include/ros2_ouster/client/client.h | 1 + ros2_ouster/include/ros2_ouster/client/types.h | 8 ++++++++ .../include/ros2_ouster/interfaces/configuration.hpp | 1 + ros2_ouster/params/driver_config.yaml | 1 + ros2_ouster/src/client/client.cpp | 10 ++++++++++ ros2_ouster/src/client/types.cpp | 9 +++++++++ ros2_ouster/src/ouster_driver.cpp | 1 + ros2_ouster/src/sensor.cpp | 7 +++++++ 8 files changed, 38 insertions(+) diff --git a/ros2_ouster/include/ros2_ouster/client/client.h b/ros2_ouster/include/ros2_ouster/client/client.h index cf7f3ff..1e4fc73 100644 --- a/ros2_ouster/include/ros2_ouster/client/client.h +++ b/ros2_ouster/include/ros2_ouster/client/client.h @@ -56,6 +56,7 @@ namespace ouster { const std::string & udp_dest_host, lidar_mode mode = MODE_UNSPEC, timestamp_mode ts_mode = TIME_FROM_UNSPEC, + AzimuthWindow azimuth_window = {0, 360000}, int lidar_port = 0, int imu_port = 0, int timeout_sec = 60); diff --git a/ros2_ouster/include/ros2_ouster/client/types.h b/ros2_ouster/include/ros2_ouster/client/types.h index 961ae37..9e5c7c8 100644 --- a/ros2_ouster/include/ros2_ouster/client/types.h +++ b/ros2_ouster/include/ros2_ouster/client/types.h @@ -285,6 +285,14 @@ namespace ouster { */ optional nmea_baud_rate_of_string(const std::string& s); + /** + * Get azimuth window from string. + * + * @param string + * @return azimuth window corresponding to the string, or 0 on error + */ + optional azimuth_window_of_string(const std::string& s); + /** * Get string representation of an Azimuth Window * diff --git a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp index d1d3db9..d120733 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp @@ -28,6 +28,7 @@ struct Configuration int imu_port; int lidar_port; std::string lidar_mode; + std::string azimuth_window; std::string timestamp_mode; std::string metadata_filepath; std::string ethernet_device; diff --git a/ros2_ouster/params/driver_config.yaml b/ros2_ouster/params/driver_config.yaml index cf9345c..e48c402 100644 --- a/ros2_ouster/params/driver_config.yaml +++ b/ros2_ouster/params/driver_config.yaml @@ -3,6 +3,7 @@ ouster_driver: lidar_ip: 10.5.5.96 computer_ip: 10.5.5.1 lidar_mode: "1024x10" + azimuth_window: "[70000,290000]" imu_port: 7503 lidar_port: 7502 sensor_frame: laser_sensor_frame diff --git a/ros2_ouster/src/client/client.cpp b/ros2_ouster/src/client/client.cpp index 36d3a62..4ee9b6a 100644 --- a/ros2_ouster/src/client/client.cpp +++ b/ros2_ouster/src/client/client.cpp @@ -547,6 +547,7 @@ std::shared_ptr init_client( const std::string & hostname, const std::string & udp_dest_host, lidar_mode mode, timestamp_mode ts_mode, + AzimuthWindow azimuth_window, int lidar_port, int imu_port, int timeout_sec) { @@ -605,6 +606,15 @@ std::shared_ptr init_client( success &= res == "set_config_param"; } + // Setup Azimuth Window + if (azimuth_window) { + success &= do_tcp_cmd( + sock_fd, + {"set_config_param", "azimuth_window", to_string(azimuth_window)}, + res); + success &= res == "set_config_param"; + } + // wake up from STANDBY, if necessary success &= do_tcp_cmd( sock_fd, {"set_config_param", "auto_start_flag", "1"}, res); diff --git a/ros2_ouster/src/client/types.cpp b/ros2_ouster/src/client/types.cpp index accab96..cecfc80 100644 --- a/ros2_ouster/src/client/types.cpp +++ b/ros2_ouster/src/client/types.cpp @@ -419,6 +419,15 @@ optional nmea_baud_rate_of_string(const std::string& s) return res == end ? nullopt : make_optional(res->first); } +optional azimuth_window_of_string(const std::string& s) +{ + AzimuthWindow p; + + int res = sscanf(s.c_str(),"[%i,%i]",&p.first,&p.second); + + return res == 2 ? p : nullopt; +} + std::string to_string(AzimuthWindow azimuth_window) { std::stringstream ss; diff --git a/ros2_ouster/src/ouster_driver.cpp b/ros2_ouster/src/ouster_driver.cpp index 0a5bf74..54c9f7e 100644 --- a/ros2_ouster/src/ouster_driver.cpp +++ b/ros2_ouster/src/ouster_driver.cpp @@ -259,6 +259,7 @@ void OusterDriver::resetService( lidar_config.lidar_port = get_parameter("lidar_port").as_int(); lidar_config.lidar_mode = get_parameter("lidar_mode").as_string(); lidar_config.timestamp_mode = get_parameter("timestamp_mode").as_string(); + lidar_config.azimuth_window = get_parameter("azimuth_window").as_string(); _sensor->reset(lidar_config, shared_from_this()); } diff --git a/ros2_ouster/src/sensor.cpp b/ros2_ouster/src/sensor.cpp index 9073b07..c44acdc 100644 --- a/ros2_ouster/src/sensor.cpp +++ b/ros2_ouster/src/sensor.cpp @@ -61,6 +61,12 @@ void Sensor::configure( exit(-1); } + if (!ouster::sensor::azimuth_window_of_string(config.azimuth_window)) { + throw ros2_ouster::OusterDriverException( + "Invalid timestamp mode: " + config.azimuth_window); + exit(-1); + } + // Report to the user whether automatic address detection is being used, and // what the source / destination IPs are RCLCPP_INFO( @@ -81,6 +87,7 @@ void Sensor::configure( config.computer_ip, ouster::sensor::lidar_mode_of_string(config.lidar_mode), ouster::sensor::timestamp_mode_of_string(config.timestamp_mode), + ouster::sensor::azimuth_window_of_string(config.azimuth_window), config.lidar_port, config.imu_port); From f8b5c165b282fd783b5c0afc6128eb65f1c2d756 Mon Sep 17 00:00:00 2001 From: Bernardo Taveira Date: Thu, 7 Apr 2022 17:40:01 +0200 Subject: [PATCH 2/7] fix missing optional --- ros2_ouster/src/client/types.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_ouster/src/client/types.cpp b/ros2_ouster/src/client/types.cpp index cecfc80..6d63180 100644 --- a/ros2_ouster/src/client/types.cpp +++ b/ros2_ouster/src/client/types.cpp @@ -425,7 +425,7 @@ optional azimuth_window_of_string(const std::string& s) int res = sscanf(s.c_str(),"[%i,%i]",&p.first,&p.second); - return res == 2 ? p : nullopt; + return res == 2 ? make_optional(p) : nullopt; } std::string to_string(AzimuthWindow azimuth_window) From 23d529e2ef2cdcda5f28df0d259bad28f709c66b Mon Sep 17 00:00:00 2001 From: Bernardo Taveira Date: Thu, 7 Apr 2022 18:04:46 +0200 Subject: [PATCH 3/7] dereference optional --- ros2_ouster/src/client/client.cpp | 12 +++++------- ros2_ouster/src/sensor.cpp | 2 +- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/ros2_ouster/src/client/client.cpp b/ros2_ouster/src/client/client.cpp index 4ee9b6a..973b6bf 100644 --- a/ros2_ouster/src/client/client.cpp +++ b/ros2_ouster/src/client/client.cpp @@ -607,13 +607,11 @@ std::shared_ptr init_client( } // Setup Azimuth Window - if (azimuth_window) { - success &= do_tcp_cmd( - sock_fd, - {"set_config_param", "azimuth_window", to_string(azimuth_window)}, - res); - success &= res == "set_config_param"; - } + success &= do_tcp_cmd( + sock_fd, + {"set_config_param", "azimuth_window", to_string(azimuth_window)}, + res); + success &= res == "set_config_param"; // wake up from STANDBY, if necessary success &= do_tcp_cmd( diff --git a/ros2_ouster/src/sensor.cpp b/ros2_ouster/src/sensor.cpp index c44acdc..ad4174d 100644 --- a/ros2_ouster/src/sensor.cpp +++ b/ros2_ouster/src/sensor.cpp @@ -87,7 +87,7 @@ void Sensor::configure( config.computer_ip, ouster::sensor::lidar_mode_of_string(config.lidar_mode), ouster::sensor::timestamp_mode_of_string(config.timestamp_mode), - ouster::sensor::azimuth_window_of_string(config.azimuth_window), + *ouster::sensor::azimuth_window_of_string(config.azimuth_window), config.lidar_port, config.imu_port); From f8e48d3d1b3917d53261ed811cbe681134d63eeb Mon Sep 17 00:00:00 2001 From: Bernardo Taveira Date: Thu, 7 Apr 2022 18:23:37 +0200 Subject: [PATCH 4/7] add parameter --- ros2_ouster/src/ouster_driver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ros2_ouster/src/ouster_driver.cpp b/ros2_ouster/src/ouster_driver.cpp index 54c9f7e..25f246a 100644 --- a/ros2_ouster/src/ouster_driver.cpp +++ b/ros2_ouster/src/ouster_driver.cpp @@ -54,6 +54,7 @@ OusterDriver::OusterDriver( this->declare_parameter("lidar_port", 7502); this->declare_parameter("lidar_mode", std::string("512x10")); this->declare_parameter("timestamp_mode", std::string("TIME_FROM_INTERNAL_OSC")); + this->declare_parameter("azimuth_window", std::string("[0,360000]")); } OusterDriver::~OusterDriver() = default; @@ -75,6 +76,7 @@ void OusterDriver::onConfigure() lidar_config.lidar_port = this->get_parameter("lidar_port").as_int(); lidar_config.lidar_mode = this->get_parameter("lidar_mode").as_string(); lidar_config.timestamp_mode = this->get_parameter("timestamp_mode").as_string(); + lidar_config.azimuth_window = get_parameter("azimuth_window").as_string(); // Deliberately retrieve the IP parameters in a try block without defaults, as // we cannot estimate a reasonable default IP address for the LiDAR/computer. From 9eb3038bdeec181e25e66e6e1f09a8fda7570e6e Mon Sep 17 00:00:00 2001 From: Bernardo Taveira Date: Thu, 7 Apr 2022 18:30:45 +0200 Subject: [PATCH 5/7] fix error message --- ros2_ouster/src/sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_ouster/src/sensor.cpp b/ros2_ouster/src/sensor.cpp index ad4174d..fa48e85 100644 --- a/ros2_ouster/src/sensor.cpp +++ b/ros2_ouster/src/sensor.cpp @@ -63,7 +63,7 @@ void Sensor::configure( if (!ouster::sensor::azimuth_window_of_string(config.azimuth_window)) { throw ros2_ouster::OusterDriverException( - "Invalid timestamp mode: " + config.azimuth_window); + "Invalid azimuth mode: " + config.azimuth_window); exit(-1); } From a54042502f3e8bc731c98cf80d695325ff63a0cc Mon Sep 17 00:00:00 2001 From: Bernardo Taveira Date: Thu, 7 Apr 2022 18:35:06 +0200 Subject: [PATCH 6/7] add parameter to tins parameter file --- ros2_ouster/params/tins_driver_config.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_ouster/params/tins_driver_config.yaml b/ros2_ouster/params/tins_driver_config.yaml index 975e929..8954d99 100644 --- a/ros2_ouster/params/tins_driver_config.yaml +++ b/ros2_ouster/params/tins_driver_config.yaml @@ -3,6 +3,7 @@ ouster_driver: lidar_ip: 10.5.5.96 computer_ip: 10.5.5.1 lidar_mode: "1024x10" + azimuth_window: "[70000,290000]" imu_port: 7503 lidar_port: 7502 sensor_frame: laser_sensor_frame From 52c26aa2e9623eb59f31620a855103b4fa819bf9 Mon Sep 17 00:00:00 2001 From: Bernardo Taveira Date: Thu, 7 Apr 2022 19:10:05 +0200 Subject: [PATCH 7/7] set default azimuth window to 0-360 --- ros2_ouster/params/tins_driver_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_ouster/params/tins_driver_config.yaml b/ros2_ouster/params/tins_driver_config.yaml index 8954d99..5c1dd80 100644 --- a/ros2_ouster/params/tins_driver_config.yaml +++ b/ros2_ouster/params/tins_driver_config.yaml @@ -3,7 +3,7 @@ ouster_driver: lidar_ip: 10.5.5.96 computer_ip: 10.5.5.1 lidar_mode: "1024x10" - azimuth_window: "[70000,290000]" + azimuth_window: "[0,360000]" imu_port: 7503 lidar_port: 7502 sensor_frame: laser_sensor_frame