Program Listing for File imu_ros2_node.cpp
↰ Return to documentation for file (src/imu_ros2_node.cpp
)
/*******************************************************************************
* @file adi_imu_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);
/* 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 =
new adi_imu::ImuIdentificationDataProvider();
adi_imu::ImuIdentificationRosPublisherInterface * ident_publisher =
new adi_imu::ImuIdentificationRosPublisher(imu_node);
ident_publisher->setMessageProvider(ident_data_provider);
adi_imu::RosTask * ident_task = dynamic_cast<adi_imu::RosTask *>(ident_publisher);
adi_imu::ImuDiagDataProviderInterface * diag_data_provider = nullptr;
std::unique_ptr<adi_imu::ImuDiagRosPublisherInterface> diag_publisher = nullptr;
adi_imu::RosTask * diag_task = nullptr;
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());
}
diag_task = dynamic_cast<adi_imu::RosTask *>(diag_publisher.get());
adi_imu::WorkerThread publisher_group_thread(publisher_group_task);
adi_imu::WorkerThread ident_thread(ident_task);
adi_imu::WorkerThread diag_thread(diag_task);
diag_thread.join();
ident_thread.join();
publisher_group_thread.join();
delete ctrl_params;
delete accel_gyro_publisher;
if (device_descriptor->has(adi_imu::ADISRegister::HAS_DELTA_BURST)) {
delete vel_ang_publisher;
}
delete imu_std_publisher;
delete full_data_publisher;
delete ident_publisher;
// delete diag_publisher;
rclcpp::shutdown();
return 0;
}