diff --git a/CMakeLists.txt b/CMakeLists.txt index 5330f85..0a1174a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,40 +17,49 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) +find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(sensor_msgs REQUIRED) find_package(serial REQUIRED) -add_executable(bno055_driver +add_library(bno055 src/bno055_driver.cpp - src/bno055_uart.cpp - src/main.cpp) -target_include_directories(bno055_driver PUBLIC + src/bno055_uart.cpp) +target_include_directories(bno055 PUBLIC $ $) ament_target_dependencies( - bno055_driver + bno055 "diagnostic_msgs" "lifecycle_msgs" + "rclcpp" "rclcpp_lifecycle" "sensor_msgs" "serial" ) -target_link_libraries(bno055_driver - ${diagnostic_msgs_LIBRARIES} - ${lifecycle_msgs_LIBRARIES} - ${rclcpp_lifecycle_LIBRARIES} - ${sensor_msgs_LIBRARIES} - ${serial_LIBRARIES} -) - # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(bno055_driver PRIVATE "BNO055_DRIVER_BUILDING_LIBRARY") +target_compile_definitions(bno055 PRIVATE "BNO055_DRIVER_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS bno055 + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +add_executable(bno055_driver + src/main.cpp) +target_link_libraries(bno055_driver + bno055) install(TARGETS bno055_driver EXPORT export_${PROJECT_NAME} @@ -58,13 +67,9 @@ install(TARGETS bno055_driver if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + add_subdirectory(test) endif() ament_package() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..6f63de9 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,13 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/include/bno055_driver/bno055_driver.hpp b/include/bno055_driver/bno055_driver.hpp index 1b5aeed..53327ec 100644 --- a/include/bno055_driver/bno055_driver.hpp +++ b/include/bno055_driver/bno055_driver.hpp @@ -15,12 +15,16 @@ #ifndef BNO055_DRIVER__BNO055_DRIVER_HPP_ #define BNO055_DRIVER__BNO055_DRIVER_HPP_ +#include "bno055_driver/bno055_reg.hpp" +#include "bno055_driver/bno055_uart.hpp" + #include -#include "lifecycle_msgs/srv/change_state.hpp" +#include +#include +#include "lifecycle_msgs/srv/change_state.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" - #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/magnetic_field.hpp" #include "sensor_msgs/msg/temperature.hpp" @@ -31,9 +35,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" -#include "bno055_driver/bno055_reg.hpp" -#include "bno055_driver/bno055_uart.hpp" -#include "bno055_driver/visibility_control.h" +#include "bno055_driver/visibility_control.hpp" namespace bno055_driver { @@ -41,7 +43,8 @@ namespace bno055_driver class BNO055Driver : public rclcpp_lifecycle::LifecycleNode { public: - explicit BNO055Driver(const std::string & node_name); + explicit BNO055Driver( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &); @@ -69,22 +72,22 @@ class BNO055Driver : public rclcpp_lifecycle::LifecycleNode std::shared_ptr port_; bool use_magnetometer_; - std::shared_ptr imu_msg_; - std::shared_ptr> imu_pub_; - std::shared_ptr mag_msg_; - std::shared_ptr> mag_pub_; - std::shared_ptr tmp_msg_; - std::shared_ptr> tmp_pub_; - std::shared_ptr diag_msg_; - std::shared_ptr> diag_pub_; + sensor_msgs::msg::Imu::SharedPtr imu_msg_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr imu_pub_; + sensor_msgs::msg::MagneticField::SharedPtr mag_msg_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr mag_pub_; + sensor_msgs::msg::Temperature::SharedPtr tmp_msg_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr tmp_pub_; + diagnostic_msgs::msg::DiagnosticArray::SharedPtr diag_msg_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr diag_pub_; rclcpp::TimerBase::SharedPtr imu_timer_; rclcpp::TimerBase::SharedPtr diag_timer_; rclcpp::GenericRate connection_rate_; - std::shared_ptr change_state_request_; - std::shared_ptr> change_state_client_; + lifecycle_msgs::srv::ChangeState::Request::SharedPtr change_state_request_; + rclcpp::Client::SharedPtr change_state_client_; rclcpp::Client::SharedFuture change_state_future_; }; diff --git a/include/bno055_driver/bno055_reg.hpp b/include/bno055_driver/bno055_reg.hpp index 2c52df3..6890c91 100644 --- a/include/bno055_driver/bno055_reg.hpp +++ b/include/bno055_driver/bno055_reg.hpp @@ -17,7 +17,7 @@ #include -#include "bno055_driver/visibility_control.h" +#include "bno055_driver/visibility_control.hpp" namespace bno055_driver { @@ -119,5 +119,4 @@ enum BNO055ResponseStatus : uint8_t } // namespace bno055_driver -#endif // BNO055_DRIVER__BNO055_DRIVER_HPP_ - +#endif // BNO055_DRIVER__BNO055_REG_HPP_ diff --git a/include/bno055_driver/bno055_uart.hpp b/include/bno055_driver/bno055_uart.hpp index a5c707f..c08e00e 100644 --- a/include/bno055_driver/bno055_uart.hpp +++ b/include/bno055_driver/bno055_uart.hpp @@ -15,52 +15,69 @@ #ifndef BNO055_DRIVER__BNO055_UART_HPP_ #define BNO055_DRIVER__BNO055_UART_HPP_ +#include "bno055_driver/bno055_reg.hpp" + +#include + #include "serial/serial.h" -#include "bno055_driver/bno055_reg.hpp" -#include "bno055_driver/visibility_control.h" +#include "bno055_driver/visibility_control.hpp" + +#ifdef _MSC_VER +#define BNO055_UART_PACKED(class_or_struct) __pragma(pack(push, 1)) class_or_struct \ + __pragma(pack(pop)) +#else // _MSC_VER +#define BNO055_UART_PACKED(class_or_struct) class_or_struct __attribute__((__packed__)) +#endif // _MSC_VER namespace bno055_driver { -struct BNO055ReadCommand -{ - BNO055MessageType message_type; - BNO055RegisterCommand command; - BNO055Register address; - uint8_t length; -} __attribute__ ((__packed__)); +BNO055_UART_PACKED( + struct BNO055ReadCommand + { + BNO055MessageType message_type; + BNO055RegisterCommand command; + BNO055Register address; + uint8_t length; + } +); -struct BNO055ReadResponse -{ - BNO055MessageType message_type; - union +BNO055_UART_PACKED( + struct BNO055ReadResponse { + BNO055MessageType message_type; + union { + uint8_t length; + BNO055ResponseStatus response_status; + }; + uint8_t data[128]; + } +); + +BNO055_UART_PACKED( + struct BNO055WriteCommand + { + BNO055MessageType message_type; + BNO055RegisterCommand command; + BNO055Register address; uint8_t length; - BNO055ResponseStatus response_status; - }; - uint8_t data[128]; -} __attribute__ ((__packed__)); + uint8_t data[128]; + } +); -struct BNO055WriteCommand -{ - BNO055MessageType message_type; - BNO055RegisterCommand command; - BNO055Register address; - uint8_t length; - uint8_t data[128]; -} __attribute__ ((__packed__)); - -struct BNO055WriteResponse -{ - BNO055MessageType message_type; - BNO055ResponseStatus response_status; -} __attribute__ ((__packed__)); +BNO055_UART_PACKED( + struct BNO055WriteResponse + { + BNO055MessageType message_type; + BNO055ResponseStatus response_status; + } +); class BNO055UART { public: - BNO055UART(const std::string &port); + explicit BNO055UART(const std::string & port); void close(); std::string getPort(); bool isOpen(); @@ -71,8 +88,14 @@ class BNO055UART BNO055ReadResponse read_response_; BNO055WriteCommand write_command_; BNO055WriteResponse write_response_; + protected: void setPage(uint8_t page); + template + size_t read(T * data, const size_t offset, size_t size); + template + size_t write(const T * data, const size_t offset, size_t size); + private: uint8_t page_; serial::Serial port_; @@ -80,5 +103,4 @@ class BNO055UART } // namespace bno055_driver -#endif // BNO055_DRIVER__BNO055_DRIVER_HPP_ - +#endif // BNO055_DRIVER__BNO055_UART_HPP_ diff --git a/include/bno055_driver/visibility_control.h b/include/bno055_driver/visibility_control.hpp similarity index 58% rename from include/bno055_driver/visibility_control.h rename to include/bno055_driver/visibility_control.hpp index f091da2..68d22d7 100644 --- a/include/bno055_driver/visibility_control.h +++ b/include/bno055_driver/visibility_control.hpp @@ -1,5 +1,19 @@ -#ifndef BNO055_DRIVER__VISIBILITY_CONTROL_H_ -#define BNO055_DRIVER__VISIBILITY_CONTROL_H_ +// Copyright 2019 Open Source Robotics Foundation, 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. + +#ifndef BNO055_DRIVER__VISIBILITY_CONTROL_HPP_ +#define BNO055_DRIVER__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility @@ -32,4 +46,4 @@ #define BNO055_DRIVER_PUBLIC_TYPE #endif -#endif // BNO055_DRIVER__VISIBILITY_CONTROL_H_ +#endif // BNO055_DRIVER__VISIBILITY_CONTROL_HPP_ diff --git a/package.xml b/package.xml index 37acf27..992caec 100644 --- a/package.xml +++ b/package.xml @@ -4,23 +4,23 @@ bno055_driver 0.0.0 Driver for the Bosch BNO055 IMU - Scott K Logan Scott K Logan Apache License 2.0 + Scott K Logan ament_cmake_ros diagnostic_msgs lifecycle_msgs + rclcpp rclcpp_lifecycle sensor_msgs serial + ament_cmake_gtest ament_lint_auto ament_lint_common - rosidl_interface_packages - ament_cmake diff --git a/src/bno055_driver.cpp b/src/bno055_driver.cpp index e46776f..eec1811 100644 --- a/src/bno055_driver.cpp +++ b/src/bno055_driver.cpp @@ -12,19 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "bno055_driver/bno055_driver.hpp" -#define bytes_to_ushort(addr) (*((uint16_t *)addr)) -#define bytes_to_short(addr) (*((int16_t *)addr)) +#include +#include +#include +#include + +#define bytes_to_ushort(addr) (*reinterpret_cast(addr)) +#define bytes_to_short(addr) (*reinterpret_cast(addr)) namespace bno055_driver { -BNO055Driver::BNO055Driver(const std::string & node_name) - : rclcpp_lifecycle::LifecycleNode(node_name), - connection_rate_(4.0) +BNO055Driver::BNO055Driver(const std::string & node_name, const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode(node_name, options), + connection_rate_(4.0) { RCLCPP_INFO(get_logger(), "%s is called.", __func__); @@ -41,11 +44,11 @@ BNO055Driver::BNO055Driver(const std::string & node_name) declare_parameter("magnetic_field_stdev"); declare_parameter("orientation_stdev"); - declare_parameter("calibration/accelerometer_offset"); - declare_parameter("calibration/gyroscope_offset"); - declare_parameter("calibration/magnetometer_offset"); - declare_parameter("calibration/accelerometer_radius"); - declare_parameter("calibration/magnetometer_radius"); + declare_parameter("calibration.accelerometer_offset"); + declare_parameter("calibration.gyroscope_offset"); + declare_parameter("calibration.magnetometer_offset"); + declare_parameter("calibration.accelerometer_radius"); + declare_parameter("calibration.magnetometer_radius"); if (get_parameter("self_manage").get_value()) { change_state_request_ = std::make_shared(); @@ -62,7 +65,7 @@ BNO055Driver::BNO055Driver(const std::string & node_name) } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - BNO055Driver::on_configure(const rclcpp_lifecycle::State &) +BNO055Driver::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "%s is called.", __func__); @@ -112,14 +115,13 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn // If given, set the calibration rclcpp::Parameter accelerometer_offset_param; - if (get_parameter("calibration/accelerometer_offset", accelerometer_offset_param)) { - const std::vector accelerometer_offset = accelerometer_offset_param.get_value>(); + if (get_parameter("calibration.accelerometer_offset", accelerometer_offset_param)) { + const std::vector accelerometer_offset = + accelerometer_offset_param.get_value>(); if (accelerometer_offset.size() != 3) { - RCLCPP_ERROR(get_logger(), "Invalid value for calibration/accelerometer_offset"); - } - else - { + RCLCPP_ERROR(get_logger(), "Invalid value for calibration.accelerometer_offset"); + } else { RCLCPP_INFO(get_logger(), "Setting accelerometer calibration offsets"); port_->write_command_.address = BNO055Register::ACC_OFFSET_X_LSB; @@ -135,14 +137,13 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } rclcpp::Parameter gyroscope_offset_param; - if (get_parameter("calibration/gyroscope_offset", gyroscope_offset_param)) { - const std::vector gyroscope_offset = gyroscope_offset_param.get_value>(); + if (get_parameter("calibration.gyroscope_offset", gyroscope_offset_param)) { + const std::vector gyroscope_offset = + gyroscope_offset_param.get_value>(); if (gyroscope_offset.size() != 3) { - RCLCPP_ERROR(get_logger(), "Invalid value for calibration/gyroscope_offset"); - } - else - { + RCLCPP_ERROR(get_logger(), "Invalid value for calibration.gyroscope_offset"); + } else { RCLCPP_INFO(get_logger(), "Setting gyroscope calibration offsets"); port_->write_command_.address = BNO055Register::GYR_OFFSET_X_LSB; @@ -158,14 +159,13 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } rclcpp::Parameter magnetometer_offset_param; - if (get_parameter("calibration/magnetometer_offset", magnetometer_offset_param)) { - const std::vector magnetometer_offset = magnetometer_offset_param.get_value>(); + if (get_parameter("calibration.magnetometer_offset", magnetometer_offset_param)) { + const std::vector magnetometer_offset = + magnetometer_offset_param.get_value>(); if (magnetometer_offset.size() != 3) { - RCLCPP_ERROR(get_logger(), "Invalid value for calibration/magnetometer_offset"); - } - else - { + RCLCPP_ERROR(get_logger(), "Invalid value for calibration.magnetometer_offset"); + } else { RCLCPP_INFO(get_logger(), "Setting magnetometer calibration offsets"); port_->write_command_.address = BNO055Register::MAG_OFFSET_X_LSB; @@ -181,39 +181,47 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } rclcpp::Parameter accelerometer_radius_param; - if (get_parameter("calibration/accelerometer_radius", accelerometer_radius_param)) { - const int64_t accelerometer_radius = accelerometer_radius_param.get_value(); + if (get_parameter("calibration.accelerometer_radius", accelerometer_radius_param)) { + const int64_t accelerometer_radius = accelerometer_radius_param.get_value(); - RCLCPP_INFO(get_logger(), "Setting accelerometer calibration radius"); + RCLCPP_INFO(get_logger(), "Setting accelerometer calibration radius"); - port_->write_command_.address = BNO055Register::ACC_RADIUS_LSB; - port_->write_command_.length = 2; - port_->write_command_.data[0] = accelerometer_radius & 0xFF; - port_->write_command_.data[1] = (accelerometer_radius >> 8) & 0xFF; - port_->write(); + port_->write_command_.address = BNO055Register::ACC_RADIUS_LSB; + port_->write_command_.length = 2; + port_->write_command_.data[0] = accelerometer_radius & 0xFF; + port_->write_command_.data[1] = (accelerometer_radius >> 8) & 0xFF; + port_->write(); } rclcpp::Parameter magnetometer_radius_param; - if (get_parameter("calibration/magnetometer_radius", magnetometer_radius_param)) { - const int64_t magnetometer_radius = magnetometer_radius_param.get_value(); + if (get_parameter("calibration.magnetometer_radius", magnetometer_radius_param)) { + const int64_t magnetometer_radius = magnetometer_radius_param.get_value(); - RCLCPP_INFO(get_logger(), "Setting magnetometer calibration radius"); + RCLCPP_INFO(get_logger(), "Setting magnetometer calibration radius"); - port_->write_command_.address = BNO055Register::MAG_RADIUS_LSB; - port_->write_command_.length = 2; - port_->write_command_.data[0] = magnetometer_radius & 0xFF; - port_->write_command_.data[1] = (magnetometer_radius >> 8) & 0xFF; - port_->write(); + port_->write_command_.address = BNO055Register::MAG_RADIUS_LSB; + port_->write_command_.length = 2; + port_->write_command_.data[0] = magnetometer_radius & 0xFF; + port_->write_command_.data[1] = (magnetometer_radius >> 8) & 0xFF; + port_->write(); } imu_msg_ = std::make_shared(); - imu_pub_ = this->create_publisher("imu/data", rclcpp::SensorDataQoS()); + imu_pub_ = this->create_publisher( + "imu/data", + rclcpp::SensorDataQoS()); mag_msg_ = std::make_shared(); - mag_pub_ = this->create_publisher("imu/mag", rclcpp::SensorDataQoS()); + mag_pub_ = this->create_publisher( + "imu/mag", + rclcpp::SensorDataQoS()); tmp_msg_ = std::make_shared(); - tmp_pub_ = this->create_publisher("imu/temp", rclcpp::SensorDataQoS()); + tmp_pub_ = this->create_publisher( + "imu/temp", + rclcpp::SensorDataQoS()); diag_msg_ = std::make_shared(); - diag_pub_ = this->create_publisher("/diagnostics", rclcpp::SystemDefaultsQoS()); + diag_pub_ = this->create_publisher( + "/diagnostics", + rclcpp::SystemDefaultsQoS()); diag_msg_->status.resize(1); diag_msg_->status[0].name = "BNO055 Status"; diag_msg_->status[0].hardware_id = std::string("BNO055 IMU on ") + port_->getPort(); @@ -240,7 +248,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn rclcpp::Parameter linear_acceleration_stdev_param; if (get_parameter("linear_acceleration_stdev", linear_acceleration_stdev_param)) { const double linear_acceleration_stdev = linear_acceleration_stdev_param.get_value(); - const double linear_acceleration_variance = linear_acceleration_stdev * linear_acceleration_stdev; + const double linear_acceleration_variance = + linear_acceleration_stdev * linear_acceleration_stdev; imu_msg_->linear_acceleration_covariance[0] = linear_acceleration_variance; imu_msg_->linear_acceleration_covariance[4] = linear_acceleration_variance; imu_msg_->linear_acceleration_covariance[8] = linear_acceleration_variance; @@ -266,14 +275,15 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn rclcpp::Parameter frequency = get_parameter("frequency"); imu_timer_ = create_wall_timer( - std::chrono::duration_cast(std::chrono::duration( - 1.0 / frequency.get_value())), + std::chrono::duration_cast( + std::chrono::duration(1.0 / frequency.get_value())), std::bind(&BNO055Driver::publish, this)); rclcpp::Parameter diagnostic_period = get_parameter("diagnostic_period"); diag_timer_ = create_wall_timer( - std::chrono::duration_cast(std::chrono::duration( - diagnostic_period.get_value())), + std::chrono::duration_cast( + std::chrono::duration( + diagnostic_period.get_value())), std::bind(&BNO055Driver::publish_diagnostics, this)); if (get_parameter("self_manage").get_value()) { @@ -286,7 +296,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - BNO055Driver::on_cleanup(const rclcpp_lifecycle::State &) +BNO055Driver::on_cleanup(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "%s is called.", __func__); @@ -316,7 +326,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - BNO055Driver::on_activate(const rclcpp_lifecycle::State &) +BNO055Driver::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "%s is called.", __func__); @@ -340,7 +350,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - BNO055Driver::on_deactivate(const rclcpp_lifecycle::State &) +BNO055Driver::on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "%s is called.", __func__); @@ -358,30 +368,36 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn port_->read_command_.address = BNO055Register::ACC_OFFSET_X_LSB; port_->read_command_.length = 22; port_->read(); - set_parameters(std::vector { - rclcpp::Parameter("calibration/accelerometer_offset", - std::vector { - bytes_to_short(&port_->read_response_.data[0]), - bytes_to_short(&port_->read_response_.data[2]), - bytes_to_short(&port_->read_response_.data[4]), - }), - rclcpp::Parameter("calibration/magnetometer_offset", - std::vector { - bytes_to_short(&port_->read_response_.data[6]), - bytes_to_short(&port_->read_response_.data[8]), - bytes_to_short(&port_->read_response_.data[10]), - }), - rclcpp::Parameter("calibration/gyroscope_offset", - std::vector { - bytes_to_short(&port_->read_response_.data[12]), - bytes_to_short(&port_->read_response_.data[14]), - bytes_to_short(&port_->read_response_.data[16]), - }), - rclcpp::Parameter("calibration/accelerometer_radius", - bytes_to_short(&port_->read_response_.data[18])), - rclcpp::Parameter("calibration/magnetometer_radius", - bytes_to_short(&port_->read_response_.data[20])), - }); + std::vector calibration_parameters { + rclcpp::Parameter( + "calibration.accelerometer_offset", + std::vector { + bytes_to_short(&port_->read_response_.data[0]), + bytes_to_short(&port_->read_response_.data[2]), + bytes_to_short(&port_->read_response_.data[4]), + }), + rclcpp::Parameter( + "calibration.magnetometer_offset", + std::vector { + bytes_to_short(&port_->read_response_.data[6]), + bytes_to_short(&port_->read_response_.data[8]), + bytes_to_short(&port_->read_response_.data[10]), + }), + rclcpp::Parameter( + "calibration.gyroscope_offset", + std::vector { + bytes_to_short(&port_->read_response_.data[12]), + bytes_to_short(&port_->read_response_.data[14]), + bytes_to_short(&port_->read_response_.data[16]), + }), + rclcpp::Parameter( + "calibration.accelerometer_radius", + bytes_to_short(&port_->read_response_.data[18])), + rclcpp::Parameter( + "calibration.magnetometer_radius", + bytes_to_short(&port_->read_response_.data[20])), + }; + set_parameters(calibration_parameters); publish_diagnostics(); diag_pub_->on_deactivate(); @@ -390,7 +406,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - BNO055Driver::on_shutdown(const rclcpp_lifecycle::State &) +BNO055Driver::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "%s is called.", __func__); @@ -422,7 +438,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - BNO055Driver::on_error(const rclcpp_lifecycle::State & previous_state) +BNO055Driver::on_error(const rclcpp_lifecycle::State & previous_state) { RCLCPP_INFO(get_logger(), "%s is called from %s.", __func__, previous_state.label().c_str()); @@ -499,7 +515,7 @@ void BNO055Driver::publish() try tmp_pub_->publish(*tmp_msg_); } } catch (std::exception & e) { - RCLCPP_ERROR(get_logger(), "Failed to poll and publish data"); + RCLCPP_ERROR(get_logger(), "Failed to poll and publish data: %s", e.what()); deactivate(); } @@ -514,9 +530,7 @@ try diag_msg_->status[0].level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; diag_msg_->status[0].message = "Disconnected"; diag_msg_->status[0].values.resize(0); - } - else - { + } else { port_->read_command_.address = BNO055Register::CALIB_STAT; port_->read_command_.length = 6; port_->read(); @@ -562,7 +576,7 @@ try diag_pub_->publish(*diag_msg_); } catch (std::exception & e) { - RCLCPP_ERROR(get_logger(), "Failed to poll and publish diagnostics"); + RCLCPP_ERROR(get_logger(), "Failed to poll and publish diagnostics: %s", e.what()); } } // namespace bno055_driver diff --git a/src/bno055_uart.cpp b/src/bno055_uart.cpp index d8cd5d7..0db025d 100644 --- a/src/bno055_uart.cpp +++ b/src/bno055_uart.cpp @@ -12,19 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "bno055_driver/bno055_uart.hpp" + #include #include #include +#include #include -#include "bno055_driver/bno055_uart.hpp" - namespace bno055_driver { -BNO055UART::BNO055UART(const std::string &port) - : page_(255), - port_(port, 115200) +BNO055UART::BNO055UART(const std::string & port) +: page_(255), + port_(port, 115200) { serial::Timeout timeout = serial::Timeout::simpleTimeout(200); port_.setTimeout(timeout); @@ -59,41 +60,43 @@ bool BNO055UART::isOpen() void BNO055UART::read(uint8_t page) { if (page != page_ && read_command_.address != BNO055Register::PAGE_ID) { - // TODO: Change pages automtically + // TODO(cottsay): Change pages automatically throw serial::SerialException("Page change required"); } for (size_t this_write, index = 0; index < sizeof(read_command_); index += this_write) { - this_write = port_.write((const uint8_t *)&read_command_ + index, 1); + this_write = write(&read_command_, index, 1); if (!this_write) { throw serial::SerialException("Timeout writing to device"); } std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - if (!port_.read((uint8_t *)&read_response_, 1)) { + if (!read(&read_response_, 0, 1)) { throw serial::SerialException("Timeout reading from device"); } if (read_response_.message_type == BNO055MessageType::RESPONSE_STATUS) { - if (!port_.read((uint8_t *)&read_response_ + 1, 1)) { + if (!read(&read_response_, 1, 1)) { throw serial::SerialException("Timeout reading from device"); } std::stringstream ss; - ss << "Register read failed with error code: 0x" << std::hex << std::setw(2) << read_response_.response_status; + ss << "Register read failed with error code: 0x" << std::hex << std::setw(2) << + read_response_.response_status; throw serial::SerialException(ss.str().c_str()); } else if (read_response_.message_type != BNO055MessageType::READ_RESPONSE) { std::stringstream ss; - ss << "Invalid response from read command: 0x" << std::hex << std::setw(2) << read_response_.message_type; + ss << "Invalid response from read command: 0x" << std::hex << std::setw(2) << + read_response_.message_type; throw serial::SerialException(ss.str().c_str()); } - if (!port_.read((uint8_t *)&read_response_ + 1, 1)) { + if (!read(&read_response_, 1, 1)) { throw serial::SerialException("Timeout reading from device"); } for (size_t this_read, remaining = read_response_.length; remaining > 0; remaining -= this_read) { - this_read = port_.read((uint8_t *)&read_response_ + 2, remaining); + this_read = read(&read_response_, 2, remaining); if (!this_read) { throw serial::SerialException("Timeout reading from device"); } @@ -101,15 +104,22 @@ void BNO055UART::read(uint8_t page) if (read_command_.length != read_response_.length) { std::stringstream ss; - ss << "Requested " << read_command_.length << " bytes from device, but got " << read_response_.length; + ss << "Requested " << read_command_.length << " bytes from device, but got " << + read_response_.length; throw serial::SerialException(ss.str().c_str()); } } +template +size_t BNO055UART::read(T * data, const size_t offset, size_t size) +{ + return port_.read(reinterpret_cast(data) + offset, size); +} + void BNO055UART::write(uint8_t page) { if (page != page_ && write_command_.address != BNO055Register::PAGE_ID) { - // TODO: Change pages automtically + // TODO(cottsay): Change pages automtically throw serial::SerialException("Page change required"); } @@ -118,32 +128,40 @@ void BNO055UART::write(uint8_t page) // pausing between each byte. for (size_t this_write, index = 0; index < write_command_.length + 4u; index += this_write) { - this_write = port_.write((const uint8_t *)&write_command_ + index, 1); + this_write = write(&write_command_, index, 1); if (!this_write) { throw serial::SerialException("Timeout writing to device"); } std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - if (!port_.read((uint8_t *)&write_response_, 1)) { + if (!read(&write_response_, 0, 1)) { throw serial::SerialException("Timeout reading from device"); } if (write_response_.message_type != BNO055MessageType::RESPONSE_STATUS) { std::stringstream ss; - ss << "Invalid response from write command: 0x" << std::hex << std::setw(2) << std::setfill('0') << write_response_.message_type; + ss << "Invalid response from write command: 0x" << std::hex << std::setw(2) << + std::setfill('0') << write_response_.message_type; throw serial::SerialException(ss.str().c_str()); } - if (!port_.read((uint8_t *)&write_response_ + 1, 1)) { + if (!read(&write_response_, 1, 1)) { throw serial::SerialException("Timeout reading from device"); } if (write_response_.response_status != BNO055ResponseStatus::WRITE_SUCCESS) { std::stringstream ss; - ss << "Register write failed with error code: 0x" << std::hex << std::setw(2) << std::setfill('0') << write_response_.response_status; + ss << "Register write failed with error code: 0x" << std::hex << std::setw(2) << + std::setfill('0') << write_response_.response_status; throw serial::SerialException(ss.str().c_str()); } } +template +size_t BNO055UART::write(const T * data, const size_t offset, size_t size) +{ + return port_.write(reinterpret_cast(data) + offset, size); } + +} // namespace bno055_driver diff --git a/src/main.cpp b/src/main.cpp index d9f50ec..62103e5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/rclcpp.hpp" +#include #include "bno055_driver/bno055_driver.hpp" +#include "rclcpp/rclcpp.hpp" + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..f2dbbd9 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,17 @@ +if(UNIX) + add_library(bno055_test_fixture EXCLUDE_FROM_ALL + fake_bno055.cpp + unix_test_harness.cpp) + target_link_libraries(bno055_test_fixture + bno055) + if(NOT APPLE) + target_link_libraries(bno055_test_fixture util) + endif() + + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(bno055_serial_tests + bno055_serial_tests.cpp + TIMEOUT 10) + target_link_libraries(bno055_serial_tests + bno055_test_fixture) +endif() diff --git a/test/bno055_serial_tests.cpp b/test/bno055_serial_tests.cpp new file mode 100644 index 0000000..f8f33f9 --- /dev/null +++ b/test/bno055_serial_tests.cpp @@ -0,0 +1,39 @@ +// Copyright 2019 Open Source Robotics Foundation, 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 + +#include "rclcpp/rclcpp.hpp" +#include "unix_test_harness.hpp" + +TEST(bno055_serial_tests, basic_configuration) +{ + bno055_test::UnixTestHarness test; + + test.driver->register_on_activate( + [executor = test.executor](const rclcpp_lifecycle::State &) { + executor->cancel(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + }); + + test.executor->spin(); +} + +int main(int argc, char ** argv) +{ + rclcpp::install_signal_handlers(); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/fake_bno055.cpp b/test/fake_bno055.cpp new file mode 100644 index 0000000..f2a6ade --- /dev/null +++ b/test/fake_bno055.cpp @@ -0,0 +1,189 @@ +// Copyright 2019 Open Source Robotics Foundation, 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 "fake_bno055.hpp" + +#include +#include + +#if defined(__linux__) +#include +#else +#include +#endif + +#include +#include +#include + +#include "bno055_driver/bno055_reg.hpp" + +namespace bno055_test +{ + +const uint8_t FakeBNO055::REGISTER_DEFAULTS[FAKE_BNO055_NUM_PAGES][FAKE_BNO055_PAGE_SIZE] +{ + { + 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + }, +}; + +FakeBNO055::FakeBNO055() +: port_name(""), master_fd(-1), slave_fd(-1) +{ + initialize(); + + if (::openpty(&master_fd, &slave_fd, port_name, NULL, NULL) == -1) { + throw std::system_error(errno, std::generic_category()); + } + + spin_thread = std::make_unique(&FakeBNO055::spin, this); +} + +FakeBNO055::~FakeBNO055() +{ + close(); + spin_thread->join(); +} + +void FakeBNO055::initialize() +{ + memcpy(®isters, ®ISTER_DEFAULTS, sizeof(registers)); +} + +void FakeBNO055::close() +{ + ::close(slave_fd); + slave_fd = -1; + + ::close(master_fd); + master_fd = -1; +} + +std::string FakeBNO055::get_port_name() const +{ + return std::string(port_name); +} + +void FakeBNO055::process_read_request( + const uint8_t page, const uint8_t address, const uint8_t size) const +{ + if (page > FAKE_BNO055_NUM_PAGES || (size_t)address + size > FAKE_BNO055_PAGE_SIZE) { + printf("invalid read request of %d bytes at %d:%d\n", size, page, address); + return; + } + + uint8_t buffer[] = + { + bno055_driver::BNO055MessageType::READ_RESPONSE, + size, + }; + + if (::write(master_fd, buffer, sizeof(buffer)) != sizeof(buffer)) { + throw std::system_error(errno, std::generic_category()); + } + if (::write(master_fd, ®isters[page][address], size) != size) { + throw std::system_error(errno, std::generic_category()); + } +} + +void FakeBNO055::process_write_request( + const uint8_t page, const uint8_t address, const uint8_t size, const uint8_t * data) +{ + if (page > FAKE_BNO055_NUM_PAGES || (size_t)address + size > FAKE_BNO055_PAGE_SIZE) { + printf("invalid read request of %d bytes at %d:%d\n", size, page, address); + return; + } + + memcpy(®isters[page][address], data, size); + + uint8_t buffer[] = + { + bno055_driver::BNO055MessageType::RESPONSE_STATUS, + bno055_driver::BNO055ResponseStatus::WRITE_SUCCESS, + }; + + if (::write(master_fd, buffer, sizeof(buffer)) != sizeof(buffer)) { + throw std::system_error(errno, std::generic_category()); + } +} + +void FakeBNO055::spin() +{ + uint8_t buffer[FAKE_BNO055_PAGE_SIZE]; + int ret, pos = 0, target = 4; + + struct timeval timeout + { + 0, + 0, + }; + + fd_set set; + FD_ZERO(&set); + + while (master_fd >= 0) { + timeout.tv_usec = 100000; + FD_SET(master_fd, &set); + + ret = ::select(master_fd + 1, &set, NULL, NULL, &timeout); + if (ret < 0) { + printf("select failed: %d\n", errno); + return; + } else if (ret == 0) { + pos = 0; + target = 4; + continue; + } + + ret = ::read(master_fd, &buffer[pos], 1); + if (ret <= 0) { + printf("read failed: %d\n", ret == 0 ? ENODATA : errno); + return; + } + + pos += ret; + if (pos < target) { + continue; + } + + if (buffer[0] != bno055_driver::BNO055MessageType::REGISTER_COMMAND) { + printf("invalid message type: %d\n", buffer[0]); + return; + } + + switch (buffer[1]) { + case bno055_driver::BNO055RegisterCommand::READ: + process_read_request( + registers[0][bno055_driver::BNO055Register::PAGE_ID], buffer[2], buffer[3]); + break; + case bno055_driver::BNO055RegisterCommand::WRITE: + if (target == 4) { + target += buffer[3]; + continue; + } + process_write_request( + registers[0][bno055_driver::BNO055Register::PAGE_ID], buffer[2], buffer[3], &buffer[4]); + break; + default: + printf("invalid register command: %d\n", buffer[1]); + return; + } + + pos = 0; + target = 4; + } +} + +} // namespace bno055_test diff --git a/test/fake_bno055.hpp b/test/fake_bno055.hpp new file mode 100644 index 0000000..43160ee --- /dev/null +++ b/test/fake_bno055.hpp @@ -0,0 +1,59 @@ +// Copyright 2019 Open Source Robotics Foundation, 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. + +#ifndef FAKE_BNO055_HPP_ +#define FAKE_BNO055_HPP_ + +#include + +#include +#include +#include + +#define FAKE_BNO055_NUM_PAGES 2 +#define FAKE_BNO055_PAGE_SIZE 256 + +namespace bno055_test +{ + +class FakeBNO055 +{ +public: + FakeBNO055(); + ~FakeBNO055(); + void initialize(); + void close(); + std::string get_port_name() const; + +protected: + void process_read_request(const uint8_t page, const uint8_t address, const uint8_t size) const; + void process_write_request( + const uint8_t page, const uint8_t address, const uint8_t size, const uint8_t * data); + void spin(); + + static const uint8_t REGISTER_DEFAULTS[FAKE_BNO055_NUM_PAGES][FAKE_BNO055_PAGE_SIZE]; + +private: + uint8_t registers[FAKE_BNO055_NUM_PAGES][FAKE_BNO055_PAGE_SIZE]; + + std::unique_ptr spin_thread; + + char port_name[256]; + int master_fd; + int slave_fd; +}; + +} // namespace bno055_test + +#endif // FAKE_BNO055_HPP_ diff --git a/test/unix_test_harness.cpp b/test/unix_test_harness.cpp new file mode 100644 index 0000000..37eea04 --- /dev/null +++ b/test/unix_test_harness.cpp @@ -0,0 +1,49 @@ +// Copyright 2019 Open Source Robotics Foundation, 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 "unix_test_harness.hpp" + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace bno055_test +{ + +UnixTestHarness::UnixTestHarness() +{ + context = std::make_shared(); + context->init(0, {}); + + fake_bno055 = std::make_unique(); + + rclcpp::NodeOptions node_options; + node_options.context(context); + node_options.parameter_overrides( + std::vector + { + rclcpp::Parameter("port", fake_bno055->get_port_name()), + rclcpp::Parameter("self_manage", true), + }); + + driver = std::make_unique("test_driver", node_options); + + rclcpp::executor::ExecutorArgs exe_args; + exe_args.context = context; + executor = std::make_shared(exe_args); + executor->add_node(driver->get_node_base_interface()); +} + +} // namespace bno055_test diff --git a/test/unix_test_harness.hpp b/test/unix_test_harness.hpp new file mode 100644 index 0000000..f3258e7 --- /dev/null +++ b/test/unix_test_harness.hpp @@ -0,0 +1,42 @@ +// Copyright 2019 Open Source Robotics Foundation, 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. + +#ifndef UNIX_TEST_HARNESS_HPP_ +#define UNIX_TEST_HARNESS_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" + +#include "fake_bno055.hpp" +#include "bno055_driver/bno055_driver.hpp" + +namespace bno055_test +{ + +class UnixTestHarness +{ +public: + UnixTestHarness(); + + rclcpp::Context::SharedPtr context; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor; + + bno055_driver::BNO055Driver::UniquePtr driver; + std::unique_ptr fake_bno055; +}; + +} // namespace bno055_test + +#endif // UNIX_TEST_HARNESS_HPP_