Program Listing for File imu_ros2_node.cpp
↰ Return to documentation for file (src/imu_ros2_node.cpp
)
/*******************************************************************************
* @file imu_ros2_node.cpp
* @brief Implementation for IMU node.
* @author Vasile Holonec (Vasile.Holonec@analog.com)
*******************************************************************************/
// Copyright 2023 Analog Devices, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "adi_imu/accelgyrotemp_data_provider.h"
#include "adi_imu/accelgyrotemp_ros_publisher.h"
#include "adi_imu/adis_device_factory.h"
#include "adi_imu/adis_register_map.h"
#include "adi_imu/imu_control_parameters.h"
#include "adi_imu/imu_data_provider.h"
#include "adi_imu/imu_diag_data_provider.h"
#include "adi_imu/imu_diag_ros_publisher.h"
#include "adi_imu/imu_diag_ros_publisher_factory.h"
#include "adi_imu/imu_full_measured_data_provider.h"
#include "adi_imu/imu_full_measured_data_ros_publisher.h"
#include "adi_imu/imu_identification_data_provider.h"
#include "adi_imu/imu_identification_ros_publisher.h"
#include "adi_imu/imu_ros_publisher.h"
#include "adi_imu/ros_publisher_group.h"
#include "adi_imu/ros_publisher_group_interface.h"
#include "adi_imu/velangtemp_data_provider.h"
#include "adi_imu/velangtemp_ros_publisher.h"
#include "adi_imu/worker_thread.h"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char * argv[])
{
int ret;
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Node> imu_node = rclcpp::Node::make_shared("adi_imu_node");
std::thread::id this_id = std::this_thread::get_id();
std::cout << "mainthread " << this_id << " running...\n";
RCLCPP_INFO(rclcpp::get_logger("rclcpp_main"), "running: main thread");
auto iio_context_param_desc = rcl_interfaces::msg::ParameterDescriptor{};
iio_context_param_desc.description =
"\nDefault value is \'local:\', to be used when the adi_imu node is running locally."
"\nIf the adi_imu node is running remotely, please use \'ip:rpi_ip_address\',";
imu_node->declare_parameter("iio_context_string", "local:", iio_context_param_desc);
auto imu_device_name_param_desc = rcl_interfaces::msg::ParameterDescriptor{};
imu_device_name_param_desc.description =
"\nSet the IMU device name from the list of supported devices, e.g. 'adis16545-3'.";
imu_node->declare_parameter("imu_device_name", "unknown", imu_device_name_param_desc);
auto imu_device_name =
imu_node->get_parameter("imu_device_name").get_parameter_value().get<std::string>();
auto device_descriptor = adi_imu::ADISDeviceFactory::make(imu_device_name);
auto diag_data_enable_param_desc = rcl_interfaces::msg::ParameterDescriptor{};
diag_data_enable_param_desc.description =
"\nWhether to enable the publisher of IMU diagnostic data."
"\nWhen enabled, the node will continuously poll diagnostic registers to read and publish "
"device status.";
imu_node->declare_parameter("diag_data_enable", true, diag_data_enable_param_desc);
auto diag_data_enable =
imu_node->get_parameter("diag_data_enable").get_parameter_value().get<bool>();
auto ident_data_enable_param_desc = rcl_interfaces::msg::ParameterDescriptor{};
ident_data_enable_param_desc.description =
"\nWhether to enable the publisher of IMU identification data."
"\nWhen enabled, the node will read and publish device identification";
imu_node->declare_parameter("ident_data_enable", true, ident_data_enable_param_desc);
auto ident_data_enable =
imu_node->get_parameter("ident_data_enable").get_parameter_value().get<bool>();
/* First make sure IIO context is available */
std::string context =
imu_node->get_parameter("iio_context_string").get_parameter_value().get<std::string>();
adi_imu::IIOWrapper m_iio_wrapper;
m_iio_wrapper.setDeviceDescriptor(device_descriptor);
ret = m_iio_wrapper.createContext(context.c_str());
if (ret) {
std::runtime_error("Error IIO context, exiting ROS2 node");
rclcpp::shutdown();
return 0;
}
adi_imu::ImuControlParameters * ctrl_params =
new adi_imu::ImuControlParameters(imu_node, device_descriptor);
adi_imu::AccelGyroTempDataProviderInterface * accel_gyro_data_provider =
new adi_imu::AccelGyroTempDataProvider();
adi_imu::AccelGyroTempRosPublisherInterface * accel_gyro_publisher =
new adi_imu::AccelGyroTempRosPublisher(imu_node);
accel_gyro_publisher->setMessageProvider(accel_gyro_data_provider);
adi_imu::ImuDataProviderInterface * imu_std_data_provider = new adi_imu::ImuDataProvider();
adi_imu::ImuRosPublisherInterface * imu_std_publisher = new adi_imu::ImuRosPublisher(imu_node);
imu_std_publisher->setMessageProvider(imu_std_data_provider);
adi_imu::VelAngTempDataProviderInterface * vel_ang_data_provider = nullptr;
adi_imu::VelAngTempRosPublisherInterface * vel_ang_publisher = nullptr;
if (device_descriptor->has(adi_imu::ADISRegister::HAS_DELTA_BURST)) {
vel_ang_data_provider = new adi_imu::VelAngTempDataProvider();
vel_ang_publisher = new adi_imu::VelAngTempRosPublisher(imu_node);
vel_ang_publisher->setMessageProvider(vel_ang_data_provider);
} else {
RCLCPP_INFO(
imu_node->get_logger(),
"Device does not support delta burst, skipping VelAngTemp publisher.");
}
adi_imu::ImuFullMeasuredDataProviderInterface * full_data_provider =
new adi_imu::ImuFullMeasuredDataProvider();
adi_imu::ImuFullMeasuredDataRosPublisherInterface * full_data_publisher =
new adi_imu::ImuFullMeasuredDataRosPublisher(imu_node);
full_data_publisher->setMessageProvider(full_data_provider);
adi_imu::RosPublisherGroupInterface * publisher_group = new adi_imu::RosPublisherGroup(imu_node);
publisher_group->setAccelGyroTempRosPublisher(accel_gyro_publisher);
if (vel_ang_publisher != nullptr) {
publisher_group->setVelAngTempRosPublisher(vel_ang_publisher);
} else {
RCLCPP_INFO(imu_node->get_logger(), "VelAngTemp publisher is null, skipping.");
}
publisher_group->setImuRosPublisher(imu_std_publisher);
publisher_group->setImuFullMeasuredDataRosPublisher(full_data_publisher);
publisher_group->setImuControlParameters(ctrl_params);
adi_imu::RosTask * publisher_group_task = dynamic_cast<adi_imu::RosTask *>(publisher_group);
adi_imu::ImuIdentificationDataProviderInterface * ident_data_provider = nullptr;
adi_imu::ImuIdentificationRosPublisherInterface * ident_publisher = nullptr;
adi_imu::RosTask * ident_task = nullptr;
if (ident_data_enable) {
ident_data_provider = new adi_imu::ImuIdentificationDataProvider();
ident_publisher = new adi_imu::ImuIdentificationRosPublisher(imu_node);
ident_publisher->setMessageProvider(ident_data_provider);
ident_task = dynamic_cast<adi_imu::RosTask *>(ident_publisher);
} else {
RCLCPP_INFO(imu_node->get_logger(), "Identification data publishing is disabled.");
}
adi_imu::ImuDiagDataProviderInterface * diag_data_provider = nullptr;
std::unique_ptr<adi_imu::ImuDiagRosPublisherInterface> diag_publisher = nullptr;
adi_imu::RosTask * diag_task = nullptr;
if (diag_data_enable) {
diag_data_provider = new adi_imu::ImuDiagDataProvider();
try {
diag_publisher = adi_imu::ImuDiagPublisherFactory::make(device_descriptor, imu_node);
diag_publisher->setMessageProvider(diag_data_provider);
diag_publisher->setDeviceDescriptor(device_descriptor);
diag_task = dynamic_cast<adi_imu::RosTask *>(diag_publisher.get());
} catch (const std::exception & e) {
RCLCPP_ERROR(imu_node->get_logger(), "Failed to create diag publisher: %s", e.what());
}
} else {
RCLCPP_INFO(imu_node->get_logger(), "Diagnostic data publishing is disabled.");
}
adi_imu::WorkerThread publisher_group_thread(publisher_group_task);
std::unique_ptr<adi_imu::WorkerThread> ident_thread = nullptr;
std::unique_ptr<adi_imu::WorkerThread> diag_thread = nullptr;
if (ident_task != nullptr) {
ident_thread = std::make_unique<adi_imu::WorkerThread>(ident_task);
}
if (diag_task != nullptr) {
diag_thread = std::make_unique<adi_imu::WorkerThread>(diag_task);
}
// Join all threads after they have started
if (ident_thread) {
ident_thread->join();
}
if (diag_thread) {
diag_thread->join();
}
publisher_group_thread.join();
// Cleanup resources
if (device_descriptor->has(adi_imu::ADISRegister::HAS_DELTA_BURST)) {
if (vel_ang_data_provider != nullptr) {
delete vel_ang_data_provider;
}
if (vel_ang_publisher != nullptr) {
delete vel_ang_publisher;
}
}
delete full_data_provider;
delete imu_std_data_provider;
if (diag_data_enable) {
delete diag_data_provider;
}
if (ident_data_provider != nullptr) {
delete ident_data_provider;
}
if (ident_publisher != nullptr) {
delete ident_publisher;
}
delete accel_gyro_publisher;
delete imu_std_publisher;
delete full_data_publisher;
if (publisher_group != nullptr) {
delete publisher_group;
}
return 0;
}