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/params/tins_driver_config.yaml b/ros2_ouster/params/tins_driver_config.yaml index 975e929..5c1dd80 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: "[0,360000]" 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..973b6bf 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,13 @@ std::shared_ptr init_client( success &= res == "set_config_param"; } + // Setup 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..6d63180 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 ? make_optional(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..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. @@ -259,6 +261,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..fa48e85 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 azimuth 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);