Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add parameter to change azimuth window #98

Closed
wants to merge 7 commits into from
Closed

Add parameter to change azimuth window #98

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
e812b4d
add azimuth window parameter
bertaveira Apr 7, 2022
f8b5c16
fix missing optional
bertaveira Apr 7, 2022
23d529e
dereference optional
bertaveira Apr 7, 2022
f8e48d3
add parameter
bertaveira Apr 7, 2022
9eb3038
fix error message
bertaveira Apr 7, 2022
a540425
add parameter to tins parameter file
bertaveira Apr 7, 2022
52c26aa
set default azimuth window to 0-360
bertaveira Apr 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ros2_ouster/include/ros2_ouster/client/client.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
8 changes: 8 additions & 0 deletions ros2_ouster/include/ros2_ouster/client/types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -285,6 +285,14 @@ namespace ouster {
*/
optional<NMEABaudRate> 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<AzimuthWindow> azimuth_window_of_string(const std::string& s);

/**
* Get string representation of an Azimuth Window
*
Expand Down
1 change: 1 addition & 0 deletions ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
1 change: 1 addition & 0 deletions ros2_ouster/params/driver_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions ros2_ouster/params/tins_driver_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
8 changes: 8 additions & 0 deletions ros2_ouster/src/client/client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -547,6 +547,7 @@ std::shared_ptr<client> 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)
{
Expand Down Expand Up @@ -605,6 +606,13 @@ std::shared_ptr<client> 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);
Expand Down
9 changes: 9 additions & 0 deletions ros2_ouster/src/client/types.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -419,6 +419,15 @@ optional<NMEABaudRate> nmea_baud_rate_of_string(const std::string& s)
return res == end ? nullopt : make_optional<NMEABaudRate>(res->first);
}

optional<AzimuthWindow> azimuth_window_of_string(const std::string& s)
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@SteveMacenski How should I do here then? I see that this file is very different here in this repo compared to the example client.

This simillar function is quite different in here: https://github.com/ouster-lidar/ouster_example/blob/13ea8e8b8a4951fb630dbc9108666995c8443bf6/ouster_client/src/types.cpp#L325

Is there a reason for it? It seems that the ROS implementation uses a different way of formatting certain configs

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You're looking at an outdated version from search, use the branch mechanism https://github.com/ouster-lidar/ouster_example/blob/master/ouster_client/src/types.cpp

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You linked the exact same commit but in "master". The file seems very different from what is here. Functionally very similar but the use of a new function "rlookup()" for most of the string conversions as well as code organization. I am not sure how to go about it.

I am not sure that copying that more recent "client" and "types" .cpp and .hpp files would work here. I can change there and then change here similarly but keeping the same differences between the files.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We linted the file probably for our formatting but it should be the same, if its not that's because they've made updates and we just haven't gotten to adding them here as well.

{
AzimuthWindow p;

int res = sscanf(s.c_str(),"[%i,%i]",&p.first,&p.second);

return res == 2 ? make_optional<AzimuthWindow>(p) : nullopt;
}

std::string to_string(AzimuthWindow azimuth_window)
{
std::stringstream ss;
Expand Down
3 changes: 3 additions & 0 deletions ros2_ouster/src/ouster_driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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.
Expand Down Expand Up @@ -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());
}

Expand Down
7 changes: 7 additions & 0 deletions ros2_ouster/src/sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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);

Expand Down