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

ROS-227: Set LIDAR FOV on startup and add an option to persist the config [NOETIC] #356

Merged
merged 9 commits into from
Aug 20, 2024
Merged

ROS-227: Set LIDAR FOV on startup and add an option to persist the config [NOETIC] #356

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
a7709c3
Set LIDAR FOV on startup
Samahu Aug 18, 2024
341706f
Add persist flags option to launch files.
Samahu Aug 19, 2024
b816f01
Push the declartion of persist_config flag
Samahu Aug 19, 2024
1302188
Update CHANGELOG and increase version number
Samahu Aug 19, 2024
3721f32
fix typo
Samahu Aug 19, 2024
e806e79
Be more specific about using persist flag and port with 0 value
Samahu Aug 20, 2024
8a7a1d9
Add a breaking note about azimuth window config reset
Samahu Aug 20, 2024
546128a
Add the loop option
Samahu Aug 20, 2024
20b0332
Remove whitespaces
Samahu Aug 20, 2024
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
11 changes: 11 additions & 0 deletions launch/driver.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,14 @@
xyzir
}"/>

<arg name="azimuth_window_start" default="0" doc="azimuth window start;
values range [0, 360000] millideggrees"/>
<arg name="azimuth_window_end" default="360000" doc="azimuth window end;
values range [0, 360000] millideggrees"/>
Samahu marked this conversation as resolved.
Show resolved Hide resolved

<arg name="persist_config" default="false"
doc="request the sensor to persist settings"/>

<group ns="$(arg ouster_ns)">
<node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
output="screen" required="true" args="manager"/>
Expand Down Expand Up @@ -92,6 +100,9 @@
<param name="~/proc_mask" value="$(arg proc_mask)"/>
<param name="~/scan_ring" value="$(arg scan_ring)"/>
<param name="~/point_type" value="$(arg point_type)"/>
<param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
<param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
<param name="~/persist_config" value="$(arg persist_config)"/>
</node>
</group>

Expand Down
11 changes: 11 additions & 0 deletions launch/record.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,14 @@
xyzir
}"/>

<arg name="azimuth_window_start" default="0" doc="azimuth window start,
values range [0, 360000] millideggrees"/>
<arg name="azimuth_window_end" default="360000" doc="azimuth window end,
values range [0, 360000] millideggrees"/>

<arg name="persist_config" default="false"
doc="request the sensor to persist settings"/>

<group ns="$(arg ouster_ns)">
<node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
output="screen" required="true" args="manager"/>
Expand All @@ -86,6 +94,9 @@
<param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
<param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
<param name="~/metadata" type="str" value="$(arg metadata)"/>
<param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
<param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
<param name="~/persist_config" value="$(arg persist_config)"/>
</node>
</group>

Expand Down
5 changes: 4 additions & 1 deletion launch/replay.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,9 @@

<param name="use_sim_time" value="true"/>

<remap from="/os_node/imu_packets" to="/ouster/imu_packets"/>
<remap from="/os_node/lidar_packets" to="/ouster/lidar_packets"/>

<arg name="ouster_ns" default="ouster" doc="Override the default namespace of all ouster nodes"/>
<arg name="metadata" default="" doc="path to read metadata file when replaying sensor data"/>
<arg name="bag_file" doc="file name to use for the recorded bag file"/>
Expand Down Expand Up @@ -93,6 +96,6 @@
<node if="$(arg _use_bag_file_name)" pkg="rosbag" type="play" name="rosbag_play_recording"
launch-prefix="bash -c 'sleep 3; $0 $@' "
output="screen" required="true"
args="--clock $(arg bag_file)"/>
args="--clock $(arg bag_file) -l"/>

</launch>
11 changes: 11 additions & 0 deletions launch/sensor.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,14 @@
xyzir
}"/>

<arg name="azimuth_window_start" default="0" doc="azimuth window start;
values range [0, 360000] millideggrees"/>
<arg name="azimuth_window_end" default="360000" doc="azimuth window end;
values range [0, 360000] millideggrees"/>

<arg name="persist_config" default="false"
doc="request the sensor to persist settings"/>

<group ns="$(arg ouster_ns)">
<node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
output="screen" required="true" args="manager"/>
Expand All @@ -90,6 +98,9 @@
<param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
<param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
<param name="~/metadata" type="str" value="$(arg metadata)"/>
<param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
<param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
<param name="~/persist_config" value="$(arg persist_config)"/>
</node>
</group>

Expand Down
11 changes: 11 additions & 0 deletions launch/sensor_mtp.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,14 @@
xyzir
}"/>

<arg name="azimuth_window_start" default="0" doc="azimuth window start,
values range [0, 360000] millideggrees"/>
<arg name="azimuth_window_end" default="360000" doc="azimuth window end,
values range [0, 360000] millideggrees"/>

<arg name="persist_config" default="false"
doc="request the sensor to persist settings"/>

<group ns="$(arg ouster_ns)">
<node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
output="screen" required="true"
Expand All @@ -97,6 +105,9 @@
<param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
<param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
<param name="~/metadata" type="str" value="$(arg metadata)"/>
<param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
<param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
<param name="~/persist_config" value="$(arg persist_config)"/>
</node>
</group>

Expand Down
29 changes: 27 additions & 2 deletions src/os_sensor_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ using nonstd::optional;

using namespace std::chrono_literals;
using namespace std::string_literals;
using std::to_string;

namespace ouster_ros {

Expand Down Expand Up @@ -232,6 +233,9 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
auto lidar_mode_arg = nh.param("lidar_mode", std::string{});
auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{});
auto udp_profile_lidar_arg = nh.param("udp_profile_lidar", std::string{});
const int MIN_AZW = 0, MAX_AZW = 360000;
auto azimuth_window_start = nh.param("azimuth_window_start", MIN_AZW);
auto azimuth_window_end = nh.param("azimuth_window_end", MAX_AZW);

if (lidar_port < 0 || lidar_port > 65535) {
auto error_msg =
Expand Down Expand Up @@ -313,6 +317,13 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
config.udp_port_imu = imu_port;
}

persist_config = nh.param("persist_config", false);
if (persist_config && (lidar_port == 0 || imu_port == 0)) {
NODELET_WARN("When using persist_config it is recommended to not "
"use 0 for port value");
}
Samahu marked this conversation as resolved.
Show resolved Hide resolved


config.udp_profile_lidar = udp_profile_lidar;
config.operating_mode = sensor::OPERATING_NORMAL;
if (lidar_mode) config.ld_mode = lidar_mode;
Expand All @@ -325,6 +336,16 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
}
}

if (azimuth_window_start < MIN_AZW || azimuth_window_start > MAX_AZW ||
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);
throw std::runtime_error(error_msg);
}

config.azimuth_window = {azimuth_window_start, azimuth_window_end};

return config;
}

Expand Down Expand Up @@ -362,6 +383,11 @@ uint8_t OusterSensor::compose_config_flags(
config_flags |= ouster::sensor::CONFIG_FORCE_REINIT;
}

if (persist_config) {
NODELET_INFO("Configuration will be persisted");
config_flags |= ouster::sensor::CONFIG_PERSIST;
}

return config_flags;
}

Expand Down Expand Up @@ -450,9 +476,8 @@ void OusterSensor::allocate_buffers() {
bool OusterSensor::init_id_changed(const sensor::packet_format& pf,
const uint8_t* lidar_buf) {
uint32_t current_init_id = pf.init_id(lidar_buf);
if (!last_init_id_initialized) {
if (!last_init_id) {
last_init_id = current_init_id + 1;
last_init_id_initialized = true;
}
if (reset_last_init_id && last_init_id != current_init_id) {
last_init_id = current_init_id;
Expand Down
3 changes: 1 addition & 2 deletions src/os_sensor_nodelet.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,8 +141,7 @@ class OusterSensor : public OusterSensorNodeletBase {
bool force_sensor_reinit = false;
bool reset_last_init_id = true;

bool last_init_id_initialized = false;
uint32_t last_init_id;
nonstd::optional<uint32_t> last_init_id;

// TODO: add as a ros parameter
const int max_poll_client_error_count = 10;
Expand Down
Loading