.. _program_listing_file__tmp_ws_src_phidgets_drivers_phidgets_gyroscope_include_phidgets_gyroscope_gyroscope_ros_i.hpp: Program Listing for File gyroscope_ros_i.hpp ============================================ |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/phidgets_drivers/phidgets_gyroscope/include/phidgets_gyroscope/gyroscope_ros_i.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /* * Copyright (c) 2019, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef PHIDGETS_GYROSCOPE_GYROSCOPE_ROS_I_HPP #define PHIDGETS_GYROSCOPE_GYROSCOPE_ROS_I_HPP #include #include #include #include #include #include #include #include "phidgets_api/gyroscope.hpp" namespace phidgets { class GyroscopeRosI final : public rclcpp::Node { public: explicit GyroscopeRosI(const rclcpp::NodeOptions& options); private: std::unique_ptr gyroscope_; std::string frame_id_; double angular_velocity_variance_{0.0}; std::mutex gyro_mutex_; double last_gyro_x_{0.0}; double last_gyro_y_{0.0}; double last_gyro_z_{0.0}; rclcpp::Publisher::SharedPtr cal_publisher_; rclcpp::Service::SharedPtr cal_srv_; rclcpp::Publisher::SharedPtr gyroscope_pub_; void timerCallback(); rclcpp::TimerBase::SharedPtr timer_; double publish_rate_; rclcpp::Time ros_time_zero_; bool synchronize_timestamps_{true}; uint64_t data_time_zero_ns_{0}; uint64_t last_data_timestamp_ns_{0}; uint64_t last_ros_stamp_ns_{0}; int64_t time_resync_interval_ns_{0}; int64_t data_interval_ns_{0}; bool can_publish_{false}; rclcpp::Time last_cb_time_; int64_t cb_delta_epsilon_ns_{0}; void publishLatest(); void calibrate(); void calibrateService( const std::shared_ptr req, std::shared_ptr res); void gyroscopeChangeCallback(const double angular_rate[3], const double timestamp); }; } // namespace phidgets #endif // PHIDGETS_GYROSCOPE_GYROSCOPE_ROS_I_HPP