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

SW-4997: Switch from using ROS timers to thread for polling lidar data #140

Merged
merged 2 commits into from
May 17, 2023
Merged

SW-4997: Switch from using ROS timers to thread for polling lidar data #140

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
970be92
Switch from using ROS timers to thread for polling
Samahu May 16, 2023
55a0d9a
Specify param defaults for non-required params
Samahu May 17, 2023
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
2 changes: 1 addition & 1 deletion ouster-ros/config/parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,4 +15,4 @@ ouster/os_cloud:
sensor_frame: 'os_sensor'
lidar_frame: 'os_lidar'
imu_frame: 'os_imu'
timestamp_mode: ''
timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode
8 changes: 4 additions & 4 deletions ouster-ros/src/os_cloud_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,10 +91,10 @@ class OusterCloud : public OusterProcessingNodeBase {
}

void declare_parameters() {
declare_parameter<std::string>("sensor_frame");
declare_parameter<std::string>("lidar_frame");
declare_parameter<std::string>("imu_frame");
declare_parameter<std::string>("timestamp_mode");
declare_parameter<std::string>("sensor_frame", "os_sensor");
declare_parameter<std::string>("lidar_frame", "os_lidar");
declare_parameter<std::string>("imu_frame", "os_imu");
declare_parameter<std::string>("timestamp_mode", "");
}

void parse_parameters() {
Expand Down
80 changes: 40 additions & 40 deletions ouster-ros/src/os_sensor_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,21 +88,8 @@ class OusterSensor : public OusterSensorNodeBase {
const rclcpp_lifecycle::State& state) {
RCLCPP_DEBUG(get_logger(), "on_activate() is called.");
LifecycleNode::on_activate(state);

allocate_buffers();
if (!connection_loop_timer) {
// TOOD: replace with a thread instead?
connection_loop_timer =
rclcpp::create_timer(this, get_clock(), 0s, [this]() {
if (reset_in_progress) return;
auto& pf = sensor::get_format(info);
connection_loop(*sensor_client, pf);
});
} else {
connection_loop_timer->reset();
}

reset_in_progress = false;
start_sensor_connection_thread();
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

Expand All @@ -113,12 +100,29 @@ class OusterSensor : public OusterSensorNodeBase {
return LifecycleNodeInterface::CallbackReturn::FAILURE;
}

void start_sensor_connection_thread() {
sensor_connection_active = true;
sensor_connection_thread = std::make_unique<std::thread>([this]() {
auto& pf = sensor::get_format(info);
while (sensor_connection_active) {
connection_loop(*sensor_client, pf);
}
RCLCPP_INFO(get_logger(), "connection_loop DONE.");
});
}

void stop_sensor_connection_thread() {
if (sensor_connection_thread->joinable()) {
sensor_connection_active = false;
sensor_connection_thread->join();
}
}

LifecycleNodeInterface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State& state) {
RCLCPP_DEBUG(get_logger(), "on_deactivate() is called.");
LifecycleNode::on_deactivate(state);
connection_loop_timer->cancel();

stop_sensor_connection_thread();
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -148,8 +152,7 @@ class OusterSensor : public OusterSensorNodeBase {
}

if (state.label() == "active") {
// stop the timer first then perform cleanup
connection_loop_timer->cancel();
stop_sensor_connection_thread();
}

// whether state was 'active' or 'inactive' do cleanup
Expand All @@ -169,15 +172,15 @@ class OusterSensor : public OusterSensorNodeBase {
private:
void declare_parameters() {
declare_parameter<std::string>("sensor_hostname");
declare_parameter<std::string>("metadata");
declare_parameter<std::string>("udp_dest");
declare_parameter<std::string>("mtp_dest");
declare_parameter<bool>("mtp_main");
declare_parameter<std::string>("metadata", "");
declare_parameter<std::string>("udp_dest", "");
declare_parameter<std::string>("mtp_dest", "");
declare_parameter<bool>("mtp_main", false);
declare_parameter<int>("lidar_port", 0);
declare_parameter<int>("imu_port", 0);
declare_parameter<std::string>("lidar_mode");
declare_parameter<std::string>("timestamp_mode");
declare_parameter<std::string>("udp_profile_lidar");
declare_parameter<std::string>("lidar_mode", "");
declare_parameter<std::string>("timestamp_mode", "");
declare_parameter<std::string>("udp_profile_lidar", "");
}

std::string get_sensor_hostname() {
Expand Down Expand Up @@ -310,16 +313,14 @@ class OusterSensor : public OusterSensorNodeBase {

// param init_id_reset is overriden to true when force_reinit is true
void reset_sensor(bool force_reinit, bool init_id_reset = false) {
if (reset_in_progress) {
RCLCPP_WARN(
get_logger(),
"sensor reset is already in progress, ignoring second call!");
if (!sensor_connection_active) {
RCLCPP_WARN(get_logger(),
"sensor reset is invoked but sensor connection is not "
"active, ignoring call!");
return;
}

reset_in_progress = true;
force_sensor_reinit = force_reinit;
connection_loop_timer->cancel();
reset_last_init_id = force_reinit ? true : init_id_reset;
auto request_transitions = std::vector<uint8_t>{
lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE,
Expand All @@ -331,15 +332,14 @@ class OusterSensor : public OusterSensorNodeBase {

// TODO: need to notify dependent node(s) of the update
void reactivate_sensor(bool init_id_reset = false) {
if (reset_in_progress) {
RCLCPP_WARN(
get_logger(),
"sensor reset is already in progress, ignoring second call!");
if (!sensor_connection_active) {
// This may indicate that we are in the process of re-activation
RCLCPP_WARN(get_logger(),
"sensor reactivate is invoked but sensor connection is "
"not active, ignoring call!");
return;
}

reset_in_progress = true;
connection_loop_timer->cancel();
reset_last_init_id = init_id_reset;
update_config_and_metadata(*sensor_client);
publish_metadata();
Expand Down Expand Up @@ -798,7 +798,7 @@ class OusterSensor : public OusterSensorNodeBase {
get_metadata_srv.reset();
get_config_srv.reset();
set_config_srv.reset();
connection_loop_timer.reset();
sensor_connection_thread.reset();
}

private:
Expand All @@ -816,11 +816,11 @@ class OusterSensor : public OusterSensorNodeBase {
rclcpp::Service<GetConfig>::SharedPtr get_config_srv;
rclcpp::Service<SetConfig>::SharedPtr set_config_srv;
std::shared_ptr<rclcpp::Client<ChangeState>> change_state_client;
std::shared_ptr<rclcpp::TimerBase> connection_loop_timer;
std::unique_ptr<std::thread> sensor_connection_thread;

bool force_sensor_reinit = false;
bool reset_last_init_id = true;
std::atomic<bool> reset_in_progress = {false};
std::atomic<bool> sensor_connection_active = {false};

bool last_init_id_initialized = false;
uint32_t last_init_id;
Expand Down