Skip to content

Commit

Permalink
Refactor code to avoid code duplication
Browse files Browse the repository at this point in the history
  • Loading branch information
@Samahu
Samahu committed Oct 29, 2024
1 parent 2ecf7a1 commit 35237ea
Show file tree
Hide file tree
Showing 5 changed files with 99 additions and 167 deletions.
21 changes: 18 additions & 3 deletions ouster-ros/include/ouster_ros/os_sensor_node_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,21 +7,23 @@
*
*/

#include <ouster/types.h>
#include <chrono>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <lifecycle_msgs/srv/change_state.hpp>
#include <std_msgs/msg/string.hpp>

#include "ouster_sensor_msgs/srv/get_metadata.hpp"

#include <ouster/types.h>

namespace ouster_ros {

class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
protected:
explicit OusterSensorNodeBase(const std::string& name,
const rclcpp::NodeOptions& options)
: rclcpp_lifecycle::LifecycleNode(name, options) {}
const rclcpp::NodeOptions& options);

protected:
bool is_arg_set(const std::string& arg) const {
Expand All @@ -41,7 +43,20 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
static bool write_text_to_file(const std::string& file_path,
const std::string& text);

static std::string transition_id_to_string(uint8_t transition_id);

template <typename CallbackT, typename... CallbackT_Args>
bool change_state(std::uint8_t transition_id, CallbackT callback,
CallbackT_Args... callback_args,
std::chrono::seconds time_out = std::chrono::seconds{3});

void execute_transitions_sequence(std::vector<uint8_t> transitions_sequence,
size_t at);


protected:
std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> change_state_client;

ouster::sensor::sensor_info info;
rclcpp::Service<ouster_sensor_msgs::srv::GetMetadata>::SharedPtr get_metadata_srv;
std::string cached_metadata;
Expand Down
77 changes: 1 addition & 76 deletions ouster-ros/src/os_pcap_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,6 @@

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace std::string_literals;
using lifecycle_msgs::srv::ChangeState;
namespace sensor = ouster::sensor;
using ouster::sensor_utils::PcapReader;
using ouster_sensor_msgs::msg::PacketMsg;
Expand All @@ -40,9 +38,7 @@ class OusterPcap : public OusterSensorNodeBase {
public:
OUSTER_ROS_PUBLIC
explicit OusterPcap(const rclcpp::NodeOptions& options)
: OusterSensorNodeBase("os_pcap", options),
change_state_client{
create_client<ChangeState>(get_name() + "/change_state"s)}
: OusterSensorNodeBase("os_pcap", options)
{
declare_parameters();
bool auto_start = get_parameter("auto_start").as_bool();
Expand All @@ -62,75 +58,6 @@ class OusterPcap : public OusterSensorNodeBase {
stop_packet_read_thread();
}

std::string transition_id_to_string(uint8_t transition_id) {
switch (transition_id) {
case lifecycle_msgs::msg::Transition::TRANSITION_CREATE:
return "create"s;
case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE:
return "configure"s;
case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP:
return "cleanup"s;
case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE:
return "activate";
case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE:
return "deactivate"s;
case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY:
return "destroy"s;
default:
return "unknown"s;
}
}

template <typename CallbackT, typename... CallbackT_Args>
bool change_state(std::uint8_t transition_id, CallbackT callback,
CallbackT_Args... callback_args,
std::chrono::seconds time_out = 3s) {
if (!change_state_client->wait_for_service(time_out)) {
RCLCPP_ERROR_STREAM(
get_logger(), "Service " << change_state_client->get_service_name()
<< "is not available.");
return false;
}

auto request = std::make_shared<ChangeState::Request>();
request->transition.id = transition_id;
// send an async request to perform the transition
change_state_client->async_send_request(
request, [this, callback,
callback_args...](rclcpp::Client<ChangeState>::SharedFuture ff) {
callback(callback_args..., ff.get()->success);
});
return true;
}

void execute_transitions_sequence(
std::vector<uint8_t> transitions_sequence, size_t at) {
assert(at < transitions_sequence.size() &&
"at index exceeds the number of transitions");
auto transition_id = transitions_sequence[at];
RCLCPP_DEBUG_STREAM(
get_logger(), "transition: [" << transition_id_to_string(transition_id)
<< "] started");
change_state(transition_id, [this, transitions_sequence, at](bool success) {
if (success) {
RCLCPP_DEBUG_STREAM(
get_logger(),
"transition: [" << transition_id_to_string(transitions_sequence[at])
<< "] completed");
if (at < transitions_sequence.size() - 1) {
execute_transitions_sequence(transitions_sequence, at + 1);
} else {
RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed");
}
} else {
RCLCPP_DEBUG_STREAM(
get_logger(),
"transition: [" << transition_id_to_string(transitions_sequence[at])
<< "] failed");
}
});
}

LifecycleNodeInterface::CallbackReturn on_configure(
const rclcpp_lifecycle::State&) {
RCLCPP_DEBUG(get_logger(), "on_configure() is called.");
Expand Down Expand Up @@ -367,8 +294,6 @@ class OusterPcap : public OusterSensorNodeBase {
}

private:
std::shared_ptr<rclcpp::Client<ChangeState>> change_state_client;

std::shared_ptr<PcapReader> pcap;
ouster_sensor_msgs::msg::PacketMsg lidar_packet;
ouster_sensor_msgs::msg::PacketMsg imu_packet;
Expand Down
74 changes: 1 addition & 73 deletions ouster-ros/src/os_sensor_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@ using ouster_sensor_msgs::srv::SetConfig;

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

using sensor::LidarPacket;
using sensor::ImuPacket;
Expand All @@ -31,9 +30,7 @@ namespace ouster_ros {

OusterSensor::OusterSensor(const std::string& name,
const rclcpp::NodeOptions& options)
: OusterSensorNodeBase(name, options),
change_state_client{
create_client<ChangeState>(get_name() + "/change_state"s)} {
: OusterSensorNodeBase(name, options) {
declare_parameters();
staged_config = parse_config_from_ros_parameters();
attempt_reconnect = get_parameter("attempt_reconnect").as_bool();
Expand Down Expand Up @@ -292,75 +289,6 @@ void OusterSensor::save_metadata() {
}
}

std::string OusterSensor::transition_id_to_string(uint8_t transition_id) {
switch (transition_id) {
case lifecycle_msgs::msg::Transition::TRANSITION_CREATE:
return "create"s;
case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE:
return "configure"s;
case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP:
return "cleanup"s;
case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE:
return "activate";
case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE:
return "deactivate"s;
case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY:
return "destroy"s;
default:
return "unknown"s;
}
}

template <typename CallbackT, typename... CallbackT_Args>
bool OusterSensor::change_state(std::uint8_t transition_id, CallbackT callback,
CallbackT_Args... callback_args,
std::chrono::seconds time_out) {
if (!change_state_client->wait_for_service(time_out)) {
RCLCPP_ERROR_STREAM(
get_logger(), "Service " << change_state_client->get_service_name()
<< "is not available.");
return false;
}

auto request = std::make_shared<ChangeState::Request>();
request->transition.id = transition_id;
// send an async request to perform the transition
change_state_client->async_send_request(
request, [this, callback,
callback_args...](rclcpp::Client<ChangeState>::SharedFuture ff) {
callback(callback_args..., ff.get()->success);
});
return true;
}

void OusterSensor::execute_transitions_sequence(
std::vector<uint8_t> transitions_sequence, size_t at) {
assert(at < transitions_sequence.size() &&
"at index exceeds the number of transitions");
auto transition_id = transitions_sequence[at];
RCLCPP_DEBUG_STREAM(
get_logger(), "transition: [" << transition_id_to_string(transition_id)
<< "] started");
change_state(transition_id, [this, transitions_sequence, at](bool success) {
if (success) {
RCLCPP_DEBUG_STREAM(
get_logger(),
"transition: [" << transition_id_to_string(transitions_sequence[at])
<< "] completed");
if (at < transitions_sequence.size() - 1) {
execute_transitions_sequence(transitions_sequence, at + 1);
} else {
RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed");
}
} else {
RCLCPP_DEBUG_STREAM(
get_logger(),
"transition: [" << transition_id_to_string(transitions_sequence[at])
<< "] failed");
}
});
}

// param init_id_reset is overriden to true when force_reinit is true
void OusterSensor::reset_sensor(bool force_reinit, bool init_id_reset) {
if (!sensor_connection_active) {
Expand Down
13 changes: 0 additions & 13 deletions ouster-ros/src/os_sensor_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,6 @@
#include <vector>

#include <std_srvs/srv/empty.hpp>
#include <lifecycle_msgs/msg/transition.hpp>
#include <lifecycle_msgs/srv/change_state.hpp>
#include "ouster_sensor_msgs/msg/packet_msg.hpp"
#include "ouster_sensor_msgs/srv/get_config.hpp"
#include "ouster_sensor_msgs/srv/set_config.hpp"
Expand All @@ -28,7 +26,6 @@


namespace sensor = ouster::sensor;
using lifecycle_msgs::srv::ChangeState;
using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;

namespace ouster_ros {
Expand Down Expand Up @@ -77,15 +74,6 @@ class OusterSensor : public OusterSensorNodeBase {

void save_metadata();

static std::string transition_id_to_string(uint8_t transition_id);
template <typename CallbackT, typename... CallbackT_Args>
bool change_state(std::uint8_t transition_id, CallbackT callback,
CallbackT_Args... callback_args,
std::chrono::seconds time_out = std::chrono::seconds{3});

void execute_transitions_sequence(std::vector<uint8_t> transitions_sequence,
size_t at);

// param init_id_reset is overriden to true when force_reinit is true
void reset_sensor(bool force_reinit, bool init_id_reset = false);

Expand Down Expand Up @@ -152,7 +140,6 @@ class OusterSensor : public OusterSensorNodeBase {
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_srv;
rclcpp::Service<ouster_sensor_msgs::srv::GetConfig>::SharedPtr get_config_srv;
rclcpp::Service<ouster_sensor_msgs::srv::SetConfig>::SharedPtr set_config_srv;
std::shared_ptr<rclcpp::Client<ChangeState>> change_state_client;

std::atomic<bool> sensor_connection_active = {false};
std::unique_ptr<std::thread> sensor_connection_thread;
Expand Down
81 changes: 79 additions & 2 deletions ouster-ros/src/os_sensor_node_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,17 +7,25 @@
*/

#include "ouster_ros/os_sensor_node_base.h"

#include <ouster/impl/build.h>

#include <fstream>
#include <lifecycle_msgs/msg/transition.hpp>

using namespace std::string_literals;
using lifecycle_msgs::srv::ChangeState;
namespace sensor = ouster::sensor;
using ouster_sensor_msgs::srv::GetMetadata;
using sensor::UDPProfileLidar;

namespace ouster_ros {

OusterSensorNodeBase::OusterSensorNodeBase(
const std::string& name, const rclcpp::NodeOptions& options)
: rclcpp_lifecycle::LifecycleNode(name, options),
change_state_client{create_client<ChangeState>(name + "/change_state"s)} {
}


void OusterSensorNodeBase::create_get_metadata_service() {
get_metadata_srv = create_service<GetMetadata>(
"get_metadata",
Expand Down Expand Up @@ -74,4 +82,73 @@ bool OusterSensorNodeBase::write_text_to_file(const std::string& file_path,
return true;
}

std::string OusterSensorNodeBase::transition_id_to_string(uint8_t transition_id) {
switch (transition_id) {
case lifecycle_msgs::msg::Transition::TRANSITION_CREATE:
return "create"s;
case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE:
return "configure"s;
case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP:
return "cleanup"s;
case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE:
return "activate";
case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE:
return "deactivate"s;
case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY:
return "destroy"s;
default:
return "unknown"s;
}
}

template <typename CallbackT, typename... CallbackT_Args>
bool OusterSensorNodeBase::change_state(std::uint8_t transition_id, CallbackT callback,
CallbackT_Args... callback_args,
std::chrono::seconds time_out) {
if (!change_state_client->wait_for_service(time_out)) {
RCLCPP_ERROR_STREAM(
get_logger(), "Service " << change_state_client->get_service_name()
<< "is not available.");
return false;
}

auto request = std::make_shared<ChangeState::Request>();
request->transition.id = transition_id;
// send an async request to perform the transition
change_state_client->async_send_request(
request, [this, callback,
callback_args...](rclcpp::Client<ChangeState>::SharedFuture ff) {
callback(callback_args..., ff.get()->success);
});
return true;
}

void OusterSensorNodeBase::execute_transitions_sequence(
std::vector<uint8_t> transitions_sequence, size_t at) {
assert(at < transitions_sequence.size() &&
"at index exceeds the number of transitions");
auto transition_id = transitions_sequence[at];
RCLCPP_DEBUG_STREAM(
get_logger(), "transition: [" << transition_id_to_string(transition_id)
<< "] started");
change_state(transition_id, [this, transitions_sequence, at](bool success) {
if (success) {
RCLCPP_DEBUG_STREAM(
get_logger(),
"transition: [" << transition_id_to_string(transitions_sequence[at])
<< "] completed");
if (at < transitions_sequence.size() - 1) {
execute_transitions_sequence(transitions_sequence, at + 1);
} else {
RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed");
}
} else {
RCLCPP_DEBUG_STREAM(
get_logger(),
"transition: [" << transition_id_to_string(transitions_sequence[at])
<< "] failed");
}
});
}

} // namespace ouster_ros

0 comments on commit 35237ea

Please sign in to comment.