diff --git a/nao_lola_conversion/CHANGELOG.rst b/nao_lola_conversion/CHANGELOG.rst deleted file mode 100644 index 81c4fba..0000000 --- a/nao_lola_conversion/CHANGELOG.rst +++ /dev/null @@ -1,37 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package nao_lola_conversion -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.1 (2023-08-23) ------------------- - -1.1.0 (2023-08-03) ------------------- -* switch nao_lola_conversion to use nao_lola_sensor_msgs -* Contributors: ijnek - -1.0.0 (2023-08-02) ------------------- -* Migrate nao/nao_interfaces_package to nao_lola_conversion package. -* Contributors: ijnek - -0.3.1 (2023-05-20) ------------------- - -0.3.0 (2023-04-28) ------------------- - -0.2.0 (2022-07-21) ------------------- - -0.0.4 (2022-02-04) ------------------- - -0.0.3 (2021-07-29) ------------------- - -0.0.2 (2021-07-20) ------------------- - -0.0.1 (2021-07-17) ------------------- diff --git a/nao_lola_conversion/CMakeLists.txt b/nao_lola_conversion/CMakeLists.txt deleted file mode 100644 index 39d4d69..0000000 --- a/nao_lola_conversion/CMakeLists.txt +++ /dev/null @@ -1,60 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(nao_lola_conversion) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(message_filters REQUIRED) -find_package(nao_lola_sensor_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) - -set(THIS_PACKAGE_INCLUDE_DEPENDS - ament_cmake - message_filters - nao_lola_sensor_msgs - rclcpp - rclcpp_components - sensor_msgs - tf2_geometry_msgs) - -# Build -add_library(${PROJECT_NAME}_node SHARED - src/nao_lola_conversion.cpp) -target_include_directories(${PROJECT_NAME}_node PUBLIC - $ - $) - -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "nao_lola_conversion::NaoLolaConversion" - EXECUTABLE ${PROJECT_NAME}) - -ament_target_dependencies(${PROJECT_NAME}_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Install -install( - DIRECTORY include/ - DESTINATION include) -install( - TARGETS ${PROJECT_NAME}_node - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) - -# Test -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/nao_lola_conversion/include/nao_lola_conversion/nao_lola_conversion.hpp b/nao_lola_conversion/include/nao_lola_conversion/nao_lola_conversion.hpp deleted file mode 100644 index dcc86b4..0000000 --- a/nao_lola_conversion/include/nao_lola_conversion/nao_lola_conversion.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2023 Kenji Brameld -// -// 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. - -#ifndef NAO_LOLA_CONVERSION__NAO_LOLA_CONVERSION_HPP_ -#define NAO_LOLA_CONVERSION__NAO_LOLA_CONVERSION_HPP_ - -#include - -#include "message_filters/subscriber.h" -#include "message_filters/time_synchronizer.h" -#include "nao_lola_sensor_msgs/msg/accelerometer.hpp" -#include "nao_lola_sensor_msgs/msg/gyroscope.hpp" -#include "rclcpp/node.hpp" -#include "sensor_msgs/msg/imu.hpp" - -namespace nao_lola_conversion -{ - -class NaoLolaConversion : public rclcpp::Node -{ -public: - explicit NaoLolaConversion(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - -private: - // Subscriptions - message_filters::Subscriber accelerometer_sub_; - message_filters::Subscriber gyroscope_sub_; - - // Publishers - rclcpp::Publisher::SharedPtr imu_pub_; - - // Synchronizer - std::shared_ptr> synchronizer_; - - // Callbacks - void synchronizerCallback( - const nao_lola_sensor_msgs::msg::Accelerometer::SharedPtr & accelerometer, - const nao_lola_sensor_msgs::msg::Gyroscope::SharedPtr & gyroscope); -}; - -} // namespace nao_lola_conversion - -#endif // NAO_LOLA_CONVERSION__NAO_LOLA_CONVERSION_HPP_ diff --git a/nao_lola_conversion/package.xml b/nao_lola_conversion/package.xml deleted file mode 100644 index 7d4bcd3..0000000 --- a/nao_lola_conversion/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - nao_lola_conversion - 1.1.1 - A package that converts Nao Lola messages to / from common ROS interfaces - ijnek - Apache License 2.0 - - ament_cmake - - message_filters - nao_lola_sensor_msgs - rclcpp - rclcpp_components - sensor_msgs - tf2_geometry_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/nao_lola_conversion/src/nao_lola_conversion.cpp b/nao_lola_conversion/src/nao_lola_conversion.cpp deleted file mode 100644 index 2b73c21..0000000 --- a/nao_lola_conversion/src/nao_lola_conversion.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2023 Kenji Brameld -// -// 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 "nao_lola_conversion/nao_lola_conversion.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -namespace nao_lola_conversion -{ - -using nao_lola_sensor_msgs::msg::Accelerometer; -using nao_lola_sensor_msgs::msg::Gyroscope; -using sensor_msgs::msg::Imu; - -NaoLolaConversion::NaoLolaConversion(const rclcpp::NodeOptions & options) -: rclcpp::Node{"nao_lola_conversion", options} -{ - // Message filter subscribers - rclcpp::QoS qos(10); - auto rmw_qos_profile = qos.get_rmw_qos_profile(); - accelerometer_sub_.subscribe(this, "sensors/accelerometer", rmw_qos_profile); - gyroscope_sub_.subscribe(this, "sensors/gyroscope", rmw_qos_profile); - - // Publishers - imu_pub_ = create_publisher("sensors/imu", 10); - - // Synchronizer - synchronizer_ = - std::make_shared>( - accelerometer_sub_, gyroscope_sub_, 10); - synchronizer_->registerCallback(&NaoLolaConversion::synchronizerCallback, this); -} - -void NaoLolaConversion::synchronizerCallback( - const Accelerometer::SharedPtr & accelerometer, - const Gyroscope::SharedPtr & gyroscope) -{ - Imu imu; - imu.header.frame_id = "ImuTorsoAccelerometer_frame"; - - // Linear Acceleration - imu.linear_acceleration.x = accelerometer->x; - imu.linear_acceleration.y = accelerometer->y; - imu.linear_acceleration.z = accelerometer->z; - - // Angular Velocity - imu.angular_velocity.x = gyroscope->x; - imu.angular_velocity.y = gyroscope->y; - imu.angular_velocity.z = gyroscope->z; - - imu_pub_->publish(imu); -} - -} // namespace nao_lola_conversion - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(nao_lola_conversion::NaoLolaConversion)