将main分支代码从ros1升级到ros2，在仓库中新建了ros1的分支代码

.gitignore

@@ -25,6 +25,13 @@
 *.exe
 *.out
 *.app
+
+# ROS2/colcon generated files
+build/
+install/
+log/
+__pycache__/
+*.pyc
 *.i*86
 *.x86_64
 *.hex
@@ -61,4 +68,3 @@
 *.exe
 *.out
 *.app
-

README.md

@@ -1,51 +1,84 @@
 # rasp_imu_hat_6dof
 
-为树莓派IMU扩展板创建的ROS代码仓库，直接将该软件包源码放在ROS工作区src目录下即可。然后使用catkin_make就可以进行编译了。完整的下载命令如下:  
-`git clone https://code.xturtle.cn/corvin_zhang/rasp_imu_hat_6dof.git`  
-
-----
-## 使用串口发布IMU数据
-当编译完成后可以使用如下命令启动发布imu数据:  
-`roslaunch serial_imu_hat_6dof serial_imu_hat.launch`  
-
-当启动上述命令后,可以在ROS中查看到如下话题和服务可以调用:  
-* 发布imu数据的话题名: /imu_data
-* 发布imu模块温度的话题名: /imu_temp
-* 发布yaw角度的话题名: /yaw_data
-* 可将yaw角度归零的话题名: /yaw_zero_topic
-* 可将yaw角度归零的服务名: /yaw_zero_service
-* 可修改串口波特率的服务: /baud_update_service
-* 控制D0,D1,D2,D3输出数字高低电平的服务: /pin_outHL_service  
-* 可得到当前yaw角度的服务: /imu_node/GetYawData  
-对于以上这些话题名和服务名,都可以在serial_imu_hat_6dof/cfg/param.yaml配置文件中进行修改.  
-
-1. 如果想使用话题方式,将yaw角度归零,那就可以执行如下命令:  
-`rostopic pub -1 /yaw_zero_topic std_msgs/Empty "{}"`  
-
-2. 如果想使用服务调用方式,将yaw角度归零,那执行如下命令:  
-`rosservice call /yaw_zero_service "{}"`  
-
-3. 如果想修改串口通信的波特率,那就可以执行如下命令:  
-`rosservice call /baud_update_service "index: 1"`  
-这里需要注意index后面的数字,１表示更新波特率为9600,2表示为57600,3表示115200,
-当更新完波特率后,需要修改本地配置文件中串口波特率为新的,否则会无法正常获取imu数据.  
-
-4. 如果需要控制D0引脚输出高电平,那就可以使用如下命令:  
-rosservice call /pin_outHL_service "D0: 1  
-D1: 0  
-D2: 0  
-D3: 0"  
-这里可以看到D0后面的数字为1,那就表明D0引脚输出高电平.若想修改其他引脚的高低电平状态,那就修改D0,D1,D2,D3后面的对应数字即可.  
-
-5. 如果想通过服务调用方式来获取到当前的yaw角度信息,可在终端中执行如下命令:  
-`rosservice call /imu_node/GetYawData`  
-当然这些不仅可以在终端中执行命令获取到yaw数据,还可以在代码中编写服务的客户端程序,请求字段为空,就可以在response字段中获取到当前yaw角度.  
-
-----
-## 使用IIC发布IMU数据
-`roslaunch rasp_imu_hat_6dof imu_data_pub.launch`  
-
-----
-## 在Rviz中显示查看IMU效果
-`roslaunch rasp_imu_hat_6dof imu_rviz_display.launch`  
+这是树莓派 6DOF IMU HAT 的 ROS2 Humble 版本代码，包含两个驱动包：
 
+- `rasp_imu_hat_6dof`：通过 IIC 读取 IMU 数据，使用 Python `rclpy` 发布话题和服务。
+- `serial_imu_hat_6dof`：通过串口读取 IMU 数据，使用 C++ `rclcpp` 发布话题和服务。
+
+旧版 ROS1 第三方源码 `imu_tools/` 已删除。ROS2 Humble 下如需滤波或 RViz IMU 插件，请安装发行版包：
+
+```bash
+sudo apt install ros-humble-imu-tools
+```
+
+## 构建
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --packages-select rasp_imu_hat_6dof serial_imu_hat_6dof
+source install/setup.bash
+```
+
+IIC 版本依赖树莓派 SMBus：
+
+```bash
+sudo apt install python3-smbus
+```
+
+## 使用串口发布 IMU 数据
+
+启动串口节点：
+
+```bash
+ros2 launch serial_imu_hat_6dof serial_imu_hat.launch.py
+```
+
+默认话题和服务名可以在 `serial_imu_hat_6dof/cfg/param.yaml` 中修改：
+
+- `/imu_data`：发布 `sensor_msgs/msg/Imu`
+- `/yaw_data`：发布 yaw，单位弧度
+- `/pitch_data`：发布 pitch，单位弧度
+- `/roll_data`：发布 roll，单位弧度
+- `/yaw_zero_topic`：发布空消息将 yaw 归零
+- `/yaw_zero_service`：服务方式将 yaw 归零
+- `/baud_update_service`：修改 IMU 串口波特率
+- `/pin_outHL_service`：控制 D0/D1/D2/D3 引脚输出高低电平
+- `/imu_node/GetYawData`：获取当前 yaw
+
+常用命令：
+
+```bash
+ros2 topic pub --once /yaw_zero_topic std_msgs/msg/Empty "{}"
+ros2 service call /yaw_zero_service serial_imu_hat_6dof/srv/SetYawZero
+ros2 service call /baud_update_service serial_imu_hat_6dof/srv/SetBaudRate "{index: 1}"
+ros2 service call /pin_outHL_service serial_imu_hat_6dof/srv/SetPinOutHL "{d0: 1, d1: 0, d2: 0, d3: 0}"
+ros2 service call /imu_node/GetYawData serial_imu_hat_6dof/srv/GetYawData
+```
+
+`SetBaudRate.index` 的含义：`1` 为 9600，`2` 为 57600，`3` 为 115200。修改 IMU 模块波特率后，需要同步修改参数文件中的 `baud_rate` 并重新连接模块。
+
+## 使用 IIC 发布 IMU 数据
+
+启动 IIC 节点：
+
+```bash
+ros2 launch rasp_imu_hat_6dof imu_data_pub.launch.py
+```
+
+获取当前 yaw：
+
+```bash
+ros2 service call /imu_node/GetYawData rasp_imu_hat_6dof/srv/GetYawData
+```
+
+ROS1 的 `dynamic_reconfigure` 已替换为 ROS2 参数。运行时修正 yaw：
+
+```bash
+ros2 param set /imu_node yaw_calibration 10.0
+```
+
+## RViz2 显示
+
+```bash
+ros2 launch rasp_imu_hat_6dof imu_rviz_display.launch.py
+```

imu_tools/.gitignore

@@ -1,15 +0,0 @@
-.cproject
-.project
-.pydevproject
-cmake_install.cmake
-bin/
-build/
-lib/
-*.cfgc
-imu_filter_madgwick/cfg/cpp/
-imu_filter_madgwick/docs/ImuFilterMadgwickConfig-usage.dox
-imu_filter_madgwick/docs/ImuFilterMadgwickConfig.dox
-imu_filter_madgwick/docs/ImuFilterMadgwickConfig.wikidoc
-imu_filter_madgwick/src/imu_filter_madgwick/__init__.py
-imu_filter_madgwick/src/imu_filter_madgwick/cfg/
-*~

imu_tools/.travis.yml

@@ -1,15 +0,0 @@
-sudo: required
-
-services:
-  - docker
-
-env:
-  matrix:
-    - CI_ROS_DISTRO="melodic"
-    - CI_ROS_DISTRO="noetic"
-
-install:
-  - docker build -t imu_tools_$CI_ROS_DISTRO -f Dockerfile-$CI_ROS_DISTRO .
-
-script:
-  - docker run imu_tools_$CI_ROS_DISTRO /bin/bash -c "source devel/setup.bash && catkin run_tests && catkin_test_results"

imu_tools/Dockerfile-melodic

@@ -1,18 +0,0 @@
-FROM ros:melodic-ros-core
-
-RUN apt-get update \
-    && apt-get install -y build-essential clang-format python-catkin-tools python-rosdep \
-    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
-
-# Create ROS workspace
-COPY . /ws/src/imu_tools
-WORKDIR /ws
-
-# Use rosdep to install all dependencies (including ROS itself)
-RUN rosdep init && rosdep update && rosdep install --from-paths src -i -y --rosdistro melodic
-
-RUN /bin/bash -c "source /opt/ros/melodic/setup.bash && \
-    catkin init && \
-    catkin config --install -j 1 -p 1 && \
-    catkin build --limit-status-rate 0.1 --no-notify && \
-    catkin build --limit-status-rate 0.1 --no-notify --make-args tests"

imu_tools/Dockerfile-noetic

@@ -1,31 +0,0 @@
-FROM ros:noetic-ros-core
-
-RUN apt-get update \
-    && apt-get install -y build-essential file clang-format python3-rosdep \
-    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
-
-# workaround for https://github.com/catkin/catkin_tools/issues/594:
-# apt-get install python3-catkin-tools doesn't work because python3-trollius doesn't exist on Ubuntu Focal
-
-ENV PATH="/root/.local/bin:${PATH}"
-
-RUN apt-get update \
-    && apt-get install -y git python3-pip \
-    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
-
-RUN pip3 install --user git+https://github.com/catkin/catkin_tools.git
-
-# end workaround
-
-# Create ROS workspace
-COPY . /ws/src/imu_tools
-WORKDIR /ws
-
-# Use rosdep to install all dependencies (including ROS itself)
-RUN rosdep init && rosdep update && rosdep install --from-paths src -i -y --rosdistro noetic
-
-RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && \
-    catkin init && \
-    catkin config --install -j 1 -p 1 && \
-    catkin build --limit-status-rate 0.1 --no-notify && \
-    catkin build --limit-status-rate 0.1 --no-notify --make-args tests"

imu_tools/LICENSE

@@ -1,5 +0,0 @@
-The imu_filter_madgwick package is licensed under the GNU General Public Licence v3 or later.
-See LICENSE.gplv3 for the text of the license.
-
-The rest of the code in this repository is licensed under the 3-clause BSD license.
-See LICENSE.bsd for the text of the license.

imu_tools/LICENSE.bsd

@@ -1,31 +0,0 @@
-Copyright (c) 2021, DFKI GmbH
-Copyright (c) 2015, City University of New York
-Copyright (c) 2012, Willow Garage, Inc.
-Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
-Copyright (c) 2010, CCNY Robotics Lab
-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.
-

imu_tools/LICENSE.gplv3

@@ -1,674 +0,0 @@
-                    GNU GENERAL PUBLIC LICENSE
-                       Version 3, 29 June 2007
-
- Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
-                            Preamble
-
-  The GNU General Public License is a free, copyleft license for
-software and other kinds of works.
-
-  The licenses for most software and other practical works are designed
-to take away your freedom to share and change the works.  By contrast,
-the GNU General Public License is intended to guarantee your freedom to
-share and change all versions of a program--to make sure it remains free
-software for all its users.  We, the Free Software Foundation, use the
-GNU General Public License for most of our software; it applies also to
-any other work released this way by its authors.  You can apply it to
-your programs, too.
-
-  When we speak of free software, we are referring to freedom, not
-price.  Our General Public Licenses are designed to make sure that you
-have the freedom to distribute copies of free software (and charge for
-them if you wish), that you receive source code or can get it if you
-want it, that you can change the software or use pieces of it in new
-free programs, and that you know you can do these things.
-
-  To protect your rights, we need to prevent others from denying you
-these rights or asking you to surrender the rights.  Therefore, you have
-certain responsibilities if you distribute copies of the software, or if
-you modify it: responsibilities to respect the freedom of others.
-
-  For example, if you distribute copies of such a program, whether
-gratis or for a fee, you must pass on to the recipients the same
-freedoms that you received.  You must make sure that they, too, receive
-or can get the source code.  And you must show them these terms so they
-know their rights.
-
-  Developers that use the GNU GPL protect your rights with two steps:
-(1) assert copyright on the software, and (2) offer you this License
-giving you legal permission to copy, distribute and/or modify it.
-
-  For the developers' and authors' protection, the GPL clearly explains
-that there is no warranty for this free software.  For both users' and
-authors' sake, the GPL requires that modified versions be marked as
-changed, so that their problems will not be attributed erroneously to
-authors of previous versions.
-
-  Some devices are designed to deny users access to install or run
-modified versions of the software inside them, although the manufacturer
-can do so.  This is fundamentally incompatible with the aim of
-protecting users' freedom to change the software.  The systematic
-pattern of such abuse occurs in the area of products for individuals to
-use, which is precisely where it is most unacceptable.  Therefore, we
-have designed this version of the GPL to prohibit the practice for those
-products.  If such problems arise substantially in other domains, we
-stand ready to extend this provision to those domains in future versions
-of the GPL, as needed to protect the freedom of users.
-
-  Finally, every program is threatened constantly by software patents.
-States should not allow patents to restrict development and use of
-software on general-purpose computers, but in those that do, we wish to
-avoid the special danger that patents applied to a free program could
-make it effectively proprietary.  To prevent this, the GPL assures that
-patents cannot be used to render the program non-free.
-
-  The precise terms and conditions for copying, distribution and
-modification follow.
-
-                       TERMS AND CONDITIONS
-
-  0. Definitions.
-
-  "This License" refers to version 3 of the GNU General Public License.
-
-  "Copyright" also means copyright-like laws that apply to other kinds of
-works, such as semiconductor masks.
-
-  "The Program" refers to any copyrightable work licensed under this
-License.  Each licensee is addressed as "you".  "Licensees" and
-"recipients" may be individuals or organizations.
-
-  To "modify" a work means to copy from or adapt all or part of the work
-in a fashion requiring copyright permission, other than the making of an
-exact copy.  The resulting work is called a "modified version" of the
-earlier work or a work "based on" the earlier work.
-
-  A "covered work" means either the unmodified Program or a work based
-on the Program.
-
-  To "propagate" a work means to do anything with it that, without
-permission, would make you directly or secondarily liable for
-infringement under applicable copyright law, except executing it on a
-computer or modifying a private copy.  Propagation includes copying,
-distribution (with or without modification), making available to the
-public, and in some countries other activities as well.
-
-  To "convey" a work means any kind of propagation that enables other
-parties to make or receive copies.  Mere interaction with a user through
-a computer network, with no transfer of a copy, is not conveying.
-
-  An interactive user interface displays "Appropriate Legal Notices"
-to the extent that it includes a convenient and prominently visible
-feature that (1) displays an appropriate copyright notice, and (2)
-tells the user that there is no warranty for the work (except to the
-extent that warranties are provided), that licensees may convey the
-work under this License, and how to view a copy of this License.  If
-the interface presents a list of user commands or options, such as a
-menu, a prominent item in the list meets this criterion.
-
-  1. Source Code.
-
-  The "source code" for a work means the preferred form of the work
-for making modifications to it.  "Object code" means any non-source
-form of a work.
-
-  A "Standard Interface" means an interface that either is an official
-standard defined by a recognized standards body, or, in the case of
-interfaces specified for a particular programming language, one that
-is widely used among developers working in that language.
-
-  The "System Libraries" of an executable work include anything, other
-than the work as a whole, that (a) is included in the normal form of
-packaging a Major Component, but which is not part of that Major
-Component, and (b) serves only to enable use of the work with that
-Major Component, or to implement a Standard Interface for which an
-implementation is available to the public in source code form.  A
-"Major Component", in this context, means a major essential component
-(kernel, window system, and so on) of the specific operating system
-(if any) on which the executable work runs, or a compiler used to
-produce the work, or an object code interpreter used to run it.
-
-  The "Corresponding Source" for a work in object code form means all
-the source code needed to generate, install, and (for an executable
-work) run the object code and to modify the work, including scripts to
-control those activities.  However, it does not include the work's
-System Libraries, or general-purpose tools or generally available free
-programs which are used unmodified in performing those activities but
-which are not part of the work.  For example, Corresponding Source
-includes interface definition files associated with source files for
-the work, and the source code for shared libraries and dynamically
-linked subprograms that the work is specifically designed to require,
-such as by intimate data communication or control flow between those
-subprograms and other parts of the work.
-
-  The Corresponding Source need not include anything that users
-can regenerate automatically from other parts of the Corresponding
-Source.
-
-  The Corresponding Source for a work in source code form is that
-same work.
-
-  2. Basic Permissions.
-
-  All rights granted under this License are granted for the term of
-copyright on the Program, and are irrevocable provided the stated
-conditions are met.  This License explicitly affirms your unlimited
-permission to run the unmodified Program.  The output from running a
-covered work is covered by this License only if the output, given its
-content, constitutes a covered work.  This License acknowledges your
-rights of fair use or other equivalent, as provided by copyright law.
-
-  You may make, run and propagate covered works that you do not
-convey, without conditions so long as your license otherwise remains
-in force.  You may convey covered works to others for the sole purpose
-of having them make modifications exclusively for you, or provide you
-with facilities for running those works, provided that you comply with
-the terms of this License in conveying all material for which you do
-not control copyright.  Those thus making or running the covered works
-for you must do so exclusively on your behalf, under your direction
-and control, on terms that prohibit them from making any copies of
-your copyrighted material outside their relationship with you.
-
-  Conveying under any other circumstances is permitted solely under
-the conditions stated below.  Sublicensing is not allowed; section 10
-makes it unnecessary.
-
-  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
-
-  No covered work shall be deemed part of an effective technological
-measure under any applicable law fulfilling obligations under article
-11 of the WIPO copyright treaty adopted on 20 December 1996, or
-similar laws prohibiting or restricting circumvention of such
-measures.
-
-  When you convey a covered work, you waive any legal power to forbid
-circumvention of technological measures to the extent such circumvention
-is effected by exercising rights under this License with respect to
-the covered work, and you disclaim any intention to limit operation or
-modification of the work as a means of enforcing, against the work's
-users, your or third parties' legal rights to forbid circumvention of
-technological measures.
-
-  4. Conveying Verbatim Copies.
-
-  You may convey verbatim copies of the Program's source code as you
-receive it, in any medium, provided that you conspicuously and
-appropriately publish on each copy an appropriate copyright notice;
-keep intact all notices stating that this License and any
-non-permissive terms added in accord with section 7 apply to the code;
-keep intact all notices of the absence of any warranty; and give all
-recipients a copy of this License along with the Program.
-
-  You may charge any price or no price for each copy that you convey,
-and you may offer support or warranty protection for a fee.
-
-  5. Conveying Modified Source Versions.
-
-  You may convey a work based on the Program, or the modifications to
-produce it from the Program, in the form of source code under the
-terms of section 4, provided that you also meet all of these conditions:
-
-    a) The work must carry prominent notices stating that you modified
-    it, and giving a relevant date.
-
-    b) The work must carry prominent notices stating that it is
-    released under this License and any conditions added under section
-    7.  This requirement modifies the requirement in section 4 to
-    "keep intact all notices".
-
-    c) You must license the entire work, as a whole, under this
-    License to anyone who comes into possession of a copy.  This
-    License will therefore apply, along with any applicable section 7
-    additional terms, to the whole of the work, and all its parts,
-    regardless of how they are packaged.  This License gives no
-    permission to license the work in any other way, but it does not
-    invalidate such permission if you have separately received it.
-
-    d) If the work has interactive user interfaces, each must display
-    Appropriate Legal Notices; however, if the Program has interactive
-    interfaces that do not display Appropriate Legal Notices, your
-    work need not make them do so.
-
-  A compilation of a covered work with other separate and independent
-works, which are not by their nature extensions of the covered work,
-and which are not combined with it such as to form a larger program,
-in or on a volume of a storage or distribution medium, is called an
-"aggregate" if the compilation and its resulting copyright are not
-used to limit the access or legal rights of the compilation's users
-beyond what the individual works permit.  Inclusion of a covered work
-in an aggregate does not cause this License to apply to the other
-parts of the aggregate.
-
-  6. Conveying Non-Source Forms.
-
-  You may convey a covered work in object code form under the terms
-of sections 4 and 5, provided that you also convey the
-machine-readable Corresponding Source under the terms of this License,
-in one of these ways:
-
-    a) Convey the object code in, or embodied in, a physical product
-    (including a physical distribution medium), accompanied by the
-    Corresponding Source fixed on a durable physical medium
-    customarily used for software interchange.
-
-    b) Convey the object code in, or embodied in, a physical product
-    (including a physical distribution medium), accompanied by a
-    written offer, valid for at least three years and valid for as
-    long as you offer spare parts or customer support for that product
-    model, to give anyone who possesses the object code either (1) a
-    copy of the Corresponding Source for all the software in the
-    product that is covered by this License, on a durable physical
-    medium customarily used for software interchange, for a price no
-    more than your reasonable cost of physically performing this
-    conveying of source, or (2) access to copy the
-    Corresponding Source from a network server at no charge.
-
-    c) Convey individual copies of the object code with a copy of the
-    written offer to provide the Corresponding Source.  This
-    alternative is allowed only occasionally and noncommercially, and
-    only if you received the object code with such an offer, in accord
-    with subsection 6b.
-
-    d) Convey the object code by offering access from a designated
-    place (gratis or for a charge), and offer equivalent access to the
-    Corresponding Source in the same way through the same place at no
-    further charge.  You need not require recipients to copy the
-    Corresponding Source along with the object code.  If the place to
-    copy the object code is a network server, the Corresponding Source
-    may be on a different server (operated by you or a third party)
-    that supports equivalent copying facilities, provided you maintain
-    clear directions next to the object code saying where to find the
-    Corresponding Source.  Regardless of what server hosts the
-    Corresponding Source, you remain obligated to ensure that it is
-    available for as long as needed to satisfy these requirements.
-
-    e) Convey the object code using peer-to-peer transmission, provided
-    you inform other peers where the object code and Corresponding
-    Source of the work are being offered to the general public at no
-    charge under subsection 6d.
-
-  A separable portion of the object code, whose source code is excluded
-from the Corresponding Source as a System Library, need not be
-included in conveying the object code work.
-
-  A "User Product" is either (1) a "consumer product", which means any
-tangible personal property which is normally used for personal, family,
-or household purposes, or (2) anything designed or sold for incorporation
-into a dwelling.  In determining whether a product is a consumer product,
-doubtful cases shall be resolved in favor of coverage.  For a particular
-product received by a particular user, "normally used" refers to a
-typical or common use of that class of product, regardless of the status
-of the particular user or of the way in which the particular user
-actually uses, or expects or is expected to use, the product.  A product
-is a consumer product regardless of whether the product has substantial
-commercial, industrial or non-consumer uses, unless such uses represent
-the only significant mode of use of the product.
-
-  "Installation Information" for a User Product means any methods,
-procedures, authorization keys, or other information required to install
-and execute modified versions of a covered work in that User Product from
-a modified version of its Corresponding Source.  The information must
-suffice to ensure that the continued functioning of the modified object
-code is in no case prevented or interfered with solely because
-modification has been made.
-
-  If you convey an object code work under this section in, or with, or
-specifically for use in, a User Product, and the conveying occurs as
-part of a transaction in which the right of possession and use of the
-User Product is transferred to the recipient in perpetuity or for a
-fixed term (regardless of how the transaction is characterized), the
-Corresponding Source conveyed under this section must be accompanied
-by the Installation Information.  But this requirement does not apply
-if neither you nor any third party retains the ability to install
-modified object code on the User Product (for example, the work has
-been installed in ROM).
-
-  The requirement to provide Installation Information does not include a
-requirement to continue to provide support service, warranty, or updates
-for a work that has been modified or installed by the recipient, or for
-the User Product in which it has been modified or installed.  Access to a
-network may be denied when the modification itself materially and
-adversely affects the operation of the network or violates the rules and
-protocols for communication across the network.
-
-  Corresponding Source conveyed, and Installation Information provided,
-in accord with this section must be in a format that is publicly
-documented (and with an implementation available to the public in
-source code form), and must require no special password or key for
-unpacking, reading or copying.
-
-  7. Additional Terms.
-
-  "Additional permissions" are terms that supplement the terms of this
-License by making exceptions from one or more of its conditions.
-Additional permissions that are applicable to the entire Program shall
-be treated as though they were included in this License, to the extent
-that they are valid under applicable law.  If additional permissions
-apply only to part of the Program, that part may be used separately
-under those permissions, but the entire Program remains governed by
-this License without regard to the additional permissions.
-
-  When you convey a copy of a covered work, you may at your option
-remove any additional permissions from that copy, or from any part of
-it.  (Additional permissions may be written to require their own
-removal in certain cases when you modify the work.)  You may place
-additional permissions on material, added by you to a covered work,
-for which you have or can give appropriate copyright permission.
-
-  Notwithstanding any other provision of this License, for material you
-add to a covered work, you may (if authorized by the copyright holders of
-that material) supplement the terms of this License with terms:
-
-    a) Disclaiming warranty or limiting liability differently from the
-    terms of sections 15 and 16 of this License; or
-
-    b) Requiring preservation of specified reasonable legal notices or
-    author attributions in that material or in the Appropriate Legal
-    Notices displayed by works containing it; or
-
-    c) Prohibiting misrepresentation of the origin of that material, or
-    requiring that modified versions of such material be marked in
-    reasonable ways as different from the original version; or
-
-    d) Limiting the use for publicity purposes of names of licensors or
-    authors of the material; or
-
-    e) Declining to grant rights under trademark law for use of some
-    trade names, trademarks, or service marks; or
-
-    f) Requiring indemnification of licensors and authors of that
-    material by anyone who conveys the material (or modified versions of
-    it) with contractual assumptions of liability to the recipient, for
-    any liability that these contractual assumptions directly impose on
-    those licensors and authors.
-
-  All other non-permissive additional terms are considered "further
-restrictions" within the meaning of section 10.  If the Program as you
-received it, or any part of it, contains a notice stating that it is
-governed by this License along with a term that is a further
-restriction, you may remove that term.  If a license document contains
-a further restriction but permits relicensing or conveying under this
-License, you may add to a covered work material governed by the terms
-of that license document, provided that the further restriction does
-not survive such relicensing or conveying.
-
-  If you add terms to a covered work in accord with this section, you
-must place, in the relevant source files, a statement of the
-additional terms that apply to those files, or a notice indicating
-where to find the applicable terms.
-
-  Additional terms, permissive or non-permissive, may be stated in the
-form of a separately written license, or stated as exceptions;
-the above requirements apply either way.
-
-  8. Termination.
-
-  You may not propagate or modify a covered work except as expressly
-provided under this License.  Any attempt otherwise to propagate or
-modify it is void, and will automatically terminate your rights under
-this License (including any patent licenses granted under the third
-paragraph of section 11).
-
-  However, if you cease all violation of this License, then your
-license from a particular copyright holder is reinstated (a)
-provisionally, unless and until the copyright holder explicitly and
-finally terminates your license, and (b) permanently, if the copyright
-holder fails to notify you of the violation by some reasonable means
-prior to 60 days after the cessation.
-
-  Moreover, your license from a particular copyright holder is
-reinstated permanently if the copyright holder notifies you of the
-violation by some reasonable means, this is the first time you have
-received notice of violation of this License (for any work) from that
-copyright holder, and you cure the violation prior to 30 days after
-your receipt of the notice.
-
-  Termination of your rights under this section does not terminate the
-licenses of parties who have received copies or rights from you under
-this License.  If your rights have been terminated and not permanently
-reinstated, you do not qualify to receive new licenses for the same
-material under section 10.
-
-  9. Acceptance Not Required for Having Copies.
-
-  You are not required to accept this License in order to receive or
-run a copy of the Program.  Ancillary propagation of a covered work
-occurring solely as a consequence of using peer-to-peer transmission
-to receive a copy likewise does not require acceptance.  However,
-nothing other than this License grants you permission to propagate or
-modify any covered work.  These actions infringe copyright if you do
-not accept this License.  Therefore, by modifying or propagating a
-covered work, you indicate your acceptance of this License to do so.
-
-  10. Automatic Licensing of Downstream Recipients.
-
-  Each time you convey a covered work, the recipient automatically
-receives a license from the original licensors, to run, modify and
-propagate that work, subject to this License.  You are not responsible
-for enforcing compliance by third parties with this License.
-
-  An "entity transaction" is a transaction transferring control of an
-organization, or substantially all assets of one, or subdividing an
-organization, or merging organizations.  If propagation of a covered
-work results from an entity transaction, each party to that
-transaction who receives a copy of the work also receives whatever
-licenses to the work the party's predecessor in interest had or could
-give under the previous paragraph, plus a right to possession of the
-Corresponding Source of the work from the predecessor in interest, if
-the predecessor has it or can get it with reasonable efforts.
-
-  You may not impose any further restrictions on the exercise of the
-rights granted or affirmed under this License.  For example, you may
-not impose a license fee, royalty, or other charge for exercise of
-rights granted under this License, and you may not initiate litigation
-(including a cross-claim or counterclaim in a lawsuit) alleging that
-any patent claim is infringed by making, using, selling, offering for
-sale, or importing the Program or any portion of it.
-
-  11. Patents.
-
-  A "contributor" is a copyright holder who authorizes use under this
-License of the Program or a work on which the Program is based.  The
-work thus licensed is called the contributor's "contributor version".
-
-  A contributor's "essential patent claims" are all patent claims
-owned or controlled by the contributor, whether already acquired or
-hereafter acquired, that would be infringed by some manner, permitted
-by this License, of making, using, or selling its contributor version,
-but do not include claims that would be infringed only as a
-consequence of further modification of the contributor version.  For
-purposes of this definition, "control" includes the right to grant
-patent sublicenses in a manner consistent with the requirements of
-this License.
-
-  Each contributor grants you a non-exclusive, worldwide, royalty-free
-patent license under the contributor's essential patent claims, to
-make, use, sell, offer for sale, import and otherwise run, modify and
-propagate the contents of its contributor version.
-
-  In the following three paragraphs, a "patent license" is any express
-agreement or commitment, however denominated, not to enforce a patent
-(such as an express permission to practice a patent or covenant not to
-sue for patent infringement).  To "grant" such a patent license to a
-party means to make such an agreement or commitment not to enforce a
-patent against the party.
-
-  If you convey a covered work, knowingly relying on a patent license,
-and the Corresponding Source of the work is not available for anyone
-to copy, free of charge and under the terms of this License, through a
-publicly available network server or other readily accessible means,
-then you must either (1) cause the Corresponding Source to be so
-available, or (2) arrange to deprive yourself of the benefit of the
-patent license for this particular work, or (3) arrange, in a manner
-consistent with the requirements of this License, to extend the patent
-license to downstream recipients.  "Knowingly relying" means you have
-actual knowledge that, but for the patent license, your conveying the
-covered work in a country, or your recipient's use of the covered work
-in a country, would infringe one or more identifiable patents in that
-country that you have reason to believe are valid.
-
-  If, pursuant to or in connection with a single transaction or
-arrangement, you convey, or propagate by procuring conveyance of, a
-covered work, and grant a patent license to some of the parties
-receiving the covered work authorizing them to use, propagate, modify
-or convey a specific copy of the covered work, then the patent license
-you grant is automatically extended to all recipients of the covered
-work and works based on it.
-
-  A patent license is "discriminatory" if it does not include within
-the scope of its coverage, prohibits the exercise of, or is
-conditioned on the non-exercise of one or more of the rights that are
-specifically granted under this License.  You may not convey a covered
-work if you are a party to an arrangement with a third party that is
-in the business of distributing software, under which you make payment
-to the third party based on the extent of your activity of conveying
-the work, and under which the third party grants, to any of the
-parties who would receive the covered work from you, a discriminatory
-patent license (a) in connection with copies of the covered work
-conveyed by you (or copies made from those copies), or (b) primarily
-for and in connection with specific products or compilations that
-contain the covered work, unless you entered into that arrangement,
-or that patent license was granted, prior to 28 March 2007.
-
-  Nothing in this License shall be construed as excluding or limiting
-any implied license or other defenses to infringement that may
-otherwise be available to you under applicable patent law.
-
-  12. No Surrender of Others' Freedom.
-
-  If conditions are imposed on you (whether by court order, agreement or
-otherwise) that contradict the conditions of this License, they do not
-excuse you from the conditions of this License.  If you cannot convey a
-covered work so as to satisfy simultaneously your obligations under this
-License and any other pertinent obligations, then as a consequence you may
-not convey it at all.  For example, if you agree to terms that obligate you
-to collect a royalty for further conveying from those to whom you convey
-the Program, the only way you could satisfy both those terms and this
-License would be to refrain entirely from conveying the Program.
-
-  13. Use with the GNU Affero General Public License.
-
-  Notwithstanding any other provision of this License, you have
-permission to link or combine any covered work with a work licensed
-under version 3 of the GNU Affero General Public License into a single
-combined work, and to convey the resulting work.  The terms of this
-License will continue to apply to the part which is the covered work,
-but the special requirements of the GNU Affero General Public License,
-section 13, concerning interaction through a network will apply to the
-combination as such.
-
-  14. Revised Versions of this License.
-
-  The Free Software Foundation may publish revised and/or new versions of
-the GNU General Public License from time to time.  Such new versions will
-be similar in spirit to the present version, but may differ in detail to
-address new problems or concerns.
-
-  Each version is given a distinguishing version number.  If the
-Program specifies that a certain numbered version of the GNU General
-Public License "or any later version" applies to it, you have the
-option of following the terms and conditions either of that numbered
-version or of any later version published by the Free Software
-Foundation.  If the Program does not specify a version number of the
-GNU General Public License, you may choose any version ever published
-by the Free Software Foundation.
-
-  If the Program specifies that a proxy can decide which future
-versions of the GNU General Public License can be used, that proxy's
-public statement of acceptance of a version permanently authorizes you
-to choose that version for the Program.
-
-  Later license versions may give you additional or different
-permissions.  However, no additional obligations are imposed on any
-author or copyright holder as a result of your choosing to follow a
-later version.
-
-  15. Disclaimer of Warranty.
-
-  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
-APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
-HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
-OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
-THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
-PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
-IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
-ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
-
-  16. Limitation of Liability.
-
-  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
-WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
-THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
-GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
-USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
-DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
-PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
-EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
-SUCH DAMAGES.
-
-  17. Interpretation of Sections 15 and 16.
-
-  If the disclaimer of warranty and limitation of liability provided
-above cannot be given local legal effect according to their terms,
-reviewing courts shall apply local law that most closely approximates
-an absolute waiver of all civil liability in connection with the
-Program, unless a warranty or assumption of liability accompanies a
-copy of the Program in return for a fee.
-
-                     END OF TERMS AND CONDITIONS
-
-            How to Apply These Terms to Your New Programs
-
-  If you develop a new program, and you want it to be of the greatest
-possible use to the public, the best way to achieve this is to make it
-free software which everyone can redistribute and change under these terms.
-
-  To do so, attach the following notices to the program.  It is safest
-to attach them to the start of each source file to most effectively
-state the exclusion of warranty; and each file should have at least
-the "copyright" line and a pointer to where the full notice is found.
-
-    <one line to give the program's name and a brief idea of what it does.>
-    Copyright (C) <year>  <name of author>
-
-    This program is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-Also add information on how to contact you by electronic and paper mail.
-
-  If the program does terminal interaction, make it output a short
-notice like this when it starts in an interactive mode:
-
-    <program>  Copyright (C) <year>  <name of author>
-    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
-    This is free software, and you are welcome to redistribute it
-    under certain conditions; type `show c' for details.
-
-The hypothetical commands `show w' and `show c' should show the appropriate
-parts of the General Public License.  Of course, your program's commands
-might be different; for a GUI interface, you would use an "about box".
-
-  You should also get your employer (if you work as a programmer) or school,
-if any, to sign a "copyright disclaimer" for the program, if necessary.
-For more information on this, and how to apply and follow the GNU GPL, see
-<http://www.gnu.org/licenses/>.
-
-  The GNU General Public License does not permit incorporating your program
-into proprietary programs.  If your program is a subroutine library, you
-may consider it more useful to permit linking proprietary applications with
-the library.  If this is what you want to do, use the GNU Lesser General
-Public License instead of this License.  But first, please read
-<http://www.gnu.org/philosophy/why-not-lgpl.html>.

imu_tools/README.md

@@ -1,65 +0,0 @@
-IMU tools for ROS
-===================================
-
-Overview
------------------------------------
-
-IMU-related filters and visualizers. The stack contains:
-
- * `imu_filter_madgwick`: a filter which fuses angular velocities,
-accelerations, and (optionally) magnetic readings from a generic IMU 
-device into an orientation. Based on the work of [1].
-
- * `imu_complementary_filter`: a filter which fuses angular velocities,
-accelerations, and (optionally) magnetic readings from a generic IMU 
-device into an orientation quaternion using a novel approach based on a complementary fusion. Based on the work of [2].
-
- * `rviz_imu_plugin` a plugin for rviz which displays `sensor_msgs::Imu`
-messages
-
-Installing
------------------------------------
-
-### From source ###
-
-[Create a catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace)
-(e.g., `~/ros-hydro-ws/`) and source the `devel/setup.bash` file.
-
-Make sure you have git installed:
-
-    sudo apt-get install git-core
-
-Download the stack from our repository into your catkin workspace (e.g.,
-`ros-hydro-ws/src`; use the proper branch for your distro, e.g., `groovy`,
-`hydro`...):
-
-    git clone -b <distro> https://github.com/ccny-ros-pkg/imu_tools.git
-
-Install any dependencies using [rosdep](http://www.ros.org/wiki/rosdep).
-
-    rosdep install imu_tools
-
-Compile the stack:
-
-    cd ~/ros-hydro-ws
-    catkin_make
-
-More info
------------------------------------
-
-http://wiki.ros.org/imu_tools
-
-License
------------------------------------
-
- * `imu_filter_madgwick`: currently licensed as GPL, following the original implementation
- 
- * `imu_complementary_filter`: BSD
-
- * `rviz_imu_plugin`: BSD
-
-References
------------------------------------
- [1] http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-
- [2] http://www.mdpi.com/1424-8220/15/8/19302

imu_tools/imu_complementary_filter/CHANGELOG.rst

@@ -1,100 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package imu_complementary_filter
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.2.3 (2021-04-09)
-------------------
-* Fix "non standard content" warning in imu_tools metapackage
-  Fixes `#135 <https://github.com/ccny-ros-pkg/imu_tools/issues/135>`_.
-* Set cmake_policy CMP0048 to fix warning
-* Contributors: Martin Günther
-
-1.2.2 (2020-05-25)
-------------------
-* fix install path & boost linkage issues
-* Contributors: Martin Günther, Sean Yen
-
-1.2.1 (2019-05-06)
-------------------
-* Remove junk xml (`#93 <https://github.com/ccny-ros-pkg/imu_tools/issues/93>`_)
-* Fix C++14 builds (`#89 <https://github.com/ccny-ros-pkg/imu_tools/issues/89>`_)
-* Contributors: David V. Lu!!, Paul Bovbel
-
-1.2.0 (2018-05-25)
-------------------
-* Add std dev parameter to orientation estimate from filter (`#85 <https://github.com/ccny-ros-pkg/imu_tools/issues/85>`_)
-  Similar to `#41 <https://github.com/ccny-ros-pkg/imu_tools/issues/41>`_, but not using dynamic_reconfigure as not implemented for complementary filter
-* Contributors: Stefan Kohlbrecher
-
-1.1.5 (2017-05-24)
-------------------
-
-1.1.4 (2017-05-22)
-------------------
-
-1.1.3 (2017-03-10)
-------------------
-* complementary_filter: move const initializations out of header
-  Initialization of static consts other than int (here: float) inside the
-  class declaration is not permitted in C++. It works in gcc (due to a
-  non-standard extension), but throws an error in C++11.
-* Contributors: Martin Guenther
-
-1.1.2 (2016-09-07)
-------------------
-
-1.1.1 (2016-09-07)
-------------------
-
-1.1.0 (2016-04-25)
-------------------
-
-1.0.11 (2016-04-22)
--------------------
-
-1.0.10 (2016-04-22)
--------------------
-* Remove Eigen dependency
-  Eigen is not actually used anywhere. Thanks @asimay!
-* Removed main function from shared library
-* Contributors: Martin Guenther, Matthias Nieuwenhuisen
-
-1.0.9 (2015-10-16)
-------------------
-* complementary: Add Eigen dependency
-  Fixes `#54 <https://github.com/ccny-ros-pkg/imu_tools/issues/54>`_.
-* Contributors: Martin Günther
-
-1.0.8 (2015-10-07)
-------------------
-
-1.0.7 (2015-10-07)
-------------------
-* Allow remapping imu namespace
-* Publish RPY as Vector3Stamped
-* Add params: constant_dt, publish_tf, reverse_tf, publish_debug_topics
-* Use MagneticField instead of Vector3
-* Contributors: Martin Günther
-
-1.0.6 (2015-10-06)
-------------------
-* Add new package: imu_complementary_filter
-* Contributors: Roberto G. Valentini, Martin Günther, Michael Görner
-
-1.0.5 (2015-06-24)
-------------------
-
-1.0.4 (2015-05-06)
-------------------
-
-1.0.3 (2015-01-29)
-------------------
-
-1.0.2 (2015-01-27)
-------------------
-
-1.0.1 (2014-12-10)
-------------------
-
-1.0.0 (2014-11-28)
-------------------

imu_tools/imu_complementary_filter/CMakeLists.txt

@@ -1,59 +0,0 @@
-cmake_minimum_required(VERSION 3.5.1)
-project(imu_complementary_filter)
-
-find_package(Boost REQUIRED COMPONENTS thread)
-
-find_package(catkin REQUIRED COMPONENTS
-  cmake_modules
-  message_filters
-  roscpp
-  sensor_msgs
-  std_msgs
-  tf
-)
-
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES complementary_filter
-  CATKIN_DEPENDS message_filters roscpp sensor_msgs std_msgs tf
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
-)
-
-## Declare a cpp library
-add_library(complementary_filter
-  src/complementary_filter.cpp
-  src/complementary_filter_ros.cpp
-  include/imu_complementary_filter/complementary_filter.h
-  include/imu_complementary_filter/complementary_filter_ros.h
-)
-target_link_libraries(complementary_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-
-# create complementary_filter_node executable
-add_executable(complementary_filter_node
-  src/complementary_filter_node.cpp)
-target_link_libraries(complementary_filter_node complementary_filter ${catkin_LIBRARIES})
-
-install(TARGETS complementary_filter
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-)
-
-install(TARGETS complementary_filter_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-## Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  FILES_MATCHING PATTERN "*.h"
-  PATTERN ".svn" EXCLUDE
-)

imu_tools/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h

@@ -1,180 +0,0 @@
-/*
-  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
-
-	@section LICENSE
-  Copyright (c) 2015, City University of New York
-  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
-	All rights reserved.
-
-	Redistribution and use in source and binary forms, with or without
-	modification, are permitted provided that the following conditions are met:
-     1. Redistributions of source code must retain the above copyright
-        notice, this list of conditions and the following disclaimer.
-     2. 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.
-     3. Neither the name of the City College of New York 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 CCNY ROBOTICS LAB 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 IMU_TOOLS_COMPLEMENTARY_FILTER_H
-#define IMU_TOOLS_COMPLEMENTARY_FILTER_H
-
-namespace imu_tools {
-
-class ComplementaryFilter
-{
-  public:
-    ComplementaryFilter();    
-    virtual ~ComplementaryFilter();
-
-    bool setGainAcc(double gain);
-    bool setGainMag(double gain);
-    double getGainAcc() const;
-    double getGainMag() const;
-
-    bool setBiasAlpha(double bias_alpha);
-    double getBiasAlpha() const;
-
-    // When the filter is in the steady state, bias estimation will occur (if the
-    // parameter is enabled).
-    bool getSteadyState() const;
-
-    void setDoBiasEstimation(bool do_bias_estimation);
-    bool getDoBiasEstimation() const;
-
-    void setDoAdaptiveGain(bool do_adaptive_gain);
-    bool getDoAdaptiveGain() const;
-  
-    double getAngularVelocityBiasX() const;
-    double getAngularVelocityBiasY() const;
-    double getAngularVelocityBiasZ() const;
-
-    // Set the orientation, as a Hamilton Quaternion, of the body frame wrt the
-    // fixed frame.
-    void setOrientation(double q0, double q1, double q2, double q3);
-
-    // Get the orientation, as a Hamilton Quaternion, of the body frame wrt the
-    // fixed frame.
-    void getOrientation(double& q0, double& q1, double& q2, double& q3) const;
-
-    // Update from accelerometer and gyroscope data.
-    // [ax, ay, az]: Normalized gravity vector.
-    // [wx, wy, wz]: Angular veloctiy, in rad / s.
-    // dt: time delta, in seconds.
-    void update(double ax, double ay, double az, 
-                double wx, double wy, double wz,
-                double dt);
-
-    // Update from accelerometer, gyroscope, and magnetometer data.
-    // [ax, ay, az]: Normalized gravity vector.
-    // [wx, wy, wz]: Angular veloctiy, in rad / s.
-    // [mx, my, mz]: Magnetic field, units irrelevant.
-    // dt: time delta, in seconds.
-    void update(double ax, double ay, double az, 
-                double wx, double wy, double wz,
-                double mx, double my, double mz,
-                double dt);
-
-  private:
-    static const double kGravity;
-    static const double gamma_;
-    // Bias estimation steady state thresholds
-    static const double kAngularVelocityThreshold;
-    static const double kAccelerationThreshold;
-    static const double kDeltaAngularVelocityThreshold;
-
-    // Gain parameter for the complementary filter, belongs in [0, 1].
-    double gain_acc_;
-    double gain_mag_;
-
-    // Bias estimation gain parameter, belongs in [0, 1].
-    double bias_alpha_;
-
-    // Parameter whether to do bias estimation or not.
-    bool do_bias_estimation_;
-    
-    // Parameter whether to do adaptive gain or not.
-    bool do_adaptive_gain_;
-
-    bool initialized_;
-    bool steady_state_;
-
-    // The orientation as a Hamilton quaternion (q0 is the scalar). Represents
-    // the orientation of the fixed frame wrt the body frame.
-    double q0_, q1_, q2_, q3_; 
-
-    // Bias in angular velocities;
-    double wx_prev_, wy_prev_, wz_prev_;
-
-    // Bias in angular velocities;
-    double wx_bias_, wy_bias_, wz_bias_;
-
-    void updateBiases(double ax, double ay, double az, 
-                      double wx, double wy, double wz);
-
-    bool checkState(double ax, double ay, double az, 
-                    double wx, double wy, double wz) const;
-
-    void getPrediction(
-        double wx, double wy, double wz, double dt, 
-        double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const;
-   
-    void getMeasurement(
-        double ax, double ay, double az, 
-        double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas);
-
-    void getMeasurement(
-        double ax, double ay, double az, 
-        double mx, double my, double mz,  
-        double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas);
-
-    void getAccCorrection(
-        double ax, double ay, double az,
-        double p0, double p1, double p2, double p3,
-        double& dq0, double& dq1, double& dq2, double& dq3);
-   
-    void getMagCorrection(
-        double mx, double my, double mz,
-        double p0, double p1, double p2, double p3,
-        double& dq0, double& dq1, double& dq2, double& dq3); 
-    
-    double getAdaptiveGain(double alpha, double ax, double ay, double az);                   
-};
-
-// Utility math functions:
-
-void normalizeVector(double& x, double& y, double& z);
-
-void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3);
-
-void scaleQuaternion(double gain,
-                     double& dq0, double& dq1, double& dq2, double& dq3); 
-
-void invertQuaternion(
-    double q0, double q1, double q2, double q3,
-    double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv);
-
-void quaternionMultiplication(double p0, double p1, double p2, double p3,
-                              double q0, double q1, double q2, double q3,
-                              double& r0, double& r1, double& r2, double& r3);
-
-void rotateVectorByQuaternion(double x, double y, double z,
-                              double q0, double q1, double q2, double q3,
-                              double& vx, double& vy, double& vz);
-
-}  // namespace imu
-
-#endif  // IMU_TOOLS_COMPLEMENTARY_FILTER_H

imu_tools/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h

@@ -1,108 +0,0 @@
-/*
-  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
-
-	@section LICENSE
-  Copyright (c) 2015, City University of New York
-  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
-	All rights reserved.
-
-	Redistribution and use in source and binary forms, with or without
-	modification, are permitted provided that the following conditions are met:
-     1. Redistributions of source code must retain the above copyright
-        notice, this list of conditions and the following disclaimer.
-     2. 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.
-     3. Neither the name of the City College of New York 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 CCNY ROBOTICS LAB 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 IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H
-#define IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H
-
-#include <sensor_msgs/MagneticField.h>
-#include <geometry_msgs/Vector3Stamped.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/synchronizer.h>
-#include <ros/ros.h>
-#include <sensor_msgs/Imu.h>
-#include <tf/transform_datatypes.h>
-#include <tf/transform_broadcaster.h>
-
-#include "imu_complementary_filter/complementary_filter.h"
-
-namespace imu_tools {
-
-class ComplementaryFilterROS
-{
-  public:
-    ComplementaryFilterROS(const ros::NodeHandle& nh, 
-                           const ros::NodeHandle& nh_private);    
-    virtual ~ComplementaryFilterROS();
-
-  private:
-
-    // Convenience typedefs
-    typedef sensor_msgs::Imu ImuMsg;
-    typedef sensor_msgs::MagneticField MagMsg;
-    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu, 
-        MagMsg> MySyncPolicy;
-    typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg> 
-        SyncPolicy;
-    typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;    
-    typedef message_filters::Subscriber<ImuMsg> ImuSubscriber; 
-    typedef message_filters::Subscriber<MagMsg> MagSubscriber;
-
-    // ROS-related variables.
-    ros::NodeHandle nh_;
-    ros::NodeHandle nh_private_;
-    
-    boost::shared_ptr<Synchronizer> sync_;
-    boost::shared_ptr<ImuSubscriber> imu_subscriber_;
-    boost::shared_ptr<MagSubscriber> mag_subscriber_;
-
-    ros::Publisher imu_publisher_;
-    ros::Publisher rpy_publisher_;
-    ros::Publisher state_publisher_;
-    tf::TransformBroadcaster tf_broadcaster_;
-         
-    // Parameters:
-    bool use_mag_;
-    bool publish_tf_;
-    bool reverse_tf_;
-    double constant_dt_;
-    bool publish_debug_topics_;
-    std::string fixed_frame_;
-    double orientation_variance_;
-
-    // State variables:
-    ComplementaryFilter filter_;
-    ros::Time time_prev_;
-    bool initialized_filter_;
-
-    void initializeParams();
-    void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw);
-    void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
-                        const MagMsg::ConstPtr& mav_msg);
-    void publish(const sensor_msgs::Imu::ConstPtr& imu_msg_raw);
-
-    tf::Quaternion hamiltonToTFQuaternion(
-        double q0, double q1, double q2, double q3) const;
-};
-
-}  // namespace imu_tools
-
-#endif // IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H

imu_tools/imu_complementary_filter/launch/complementary_filter.launch

@@ -1,34 +0,0 @@
-<!-- ComplementaryFilter launch file -->
-<launch>
-  #### Nodelet manager ######################################################
-
-  <node pkg="nodelet" type="nodelet" name="imu_manager" 
-    args="manager" output="screen" />
-
-  #### IMU Driver ###########################################################
-
-  <node pkg="nodelet" type="nodelet" name="PhidgetsImuNodelet" 
-    args="load phidgets_imu/PhidgetsImuNodelet imu_manager" 
-    output="screen">
-
-    # supported data rates: 4 8 16 24 32 40 ... 1000 (in ms)
-    <param name="period" value="4"/>
-
-  </node>
-
-  #### Complementary filter
-
-  <node pkg="imu_complementary_filter" type="complementary_filter_node"
-      name="complementary_filter_gain_node" output="screen">
-    <param name="do_bias_estimation" value="true"/>
-    <param name="do_adaptive_gain" value="true"/>
-    <param name="use_mag" value="false"/>
-    <param name="gain_acc" value="0.01"/>
-    <param name="gain_mag" value="0.01"/>
- 
-  
-  </node>
-  
-
-
-</launch>

imu_tools/imu_complementary_filter/package.xml

@@ -1,25 +0,0 @@
-<?xml version="1.0"?>
-<package>
-  <name>imu_complementary_filter</name>
-  <version>1.2.3</version>
-  <description>Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into a quaternion to represent the orientation of the device wrt the global frame. Based on the algorithm by Roberto G. Valenti etal. described in the paper "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs" available at http://www.mdpi.com/1424-8220/15/8/19302 .</description>
-
-  <maintainer email="robertogl.valenti@gmail.com">Roberto G. Valenti</maintainer>
-  <license>BSD</license>
- 
-  <url>http://www.mdpi.com/1424-8220/15/8/19302</url>
-  <author email="robertogl.valenti@gmail.com">Roberto G. Valenti</author>
-
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>cmake_modules</build_depend>
-  <build_depend>message_filters</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>tf</build_depend>
-  <run_depend>message_filters</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>tf</run_depend>
-</package>

imu_tools/imu_complementary_filter/src/complementary_filter.cpp

@@ -1,551 +0,0 @@
-/*
-  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
-
-	@section LICENSE
-  Copyright (c) 2015, City University of New York
-  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
-	All rights reserved.
-
-	Redistribution and use in source and binary forms, with or without
-	modification, are permitted provided that the following conditions are met:
-     1. Redistributions of source code must retain the above copyright
-        notice, this list of conditions and the following disclaimer.
-     2. 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.
-     3. Neither the name of the City College of New York 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 CCNY ROBOTICS LAB 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.
-*/
-
-#include "imu_complementary_filter/complementary_filter.h"
-
-#include <cstdio>
-#include <cmath>
-#include <iostream>
-
-namespace imu_tools {
-
-const double ComplementaryFilter::kGravity = 9.81;
-const double ComplementaryFilter::gamma_ = 0.01;
-// Bias estimation steady state thresholds
-const double ComplementaryFilter::kAngularVelocityThreshold = 0.2;
-const double ComplementaryFilter::kAccelerationThreshold = 0.1;
-const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01;
-
-ComplementaryFilter::ComplementaryFilter() :
-    gain_acc_(0.01),
-    gain_mag_(0.01),
-    bias_alpha_(0.01),
-    do_bias_estimation_(true),
-    do_adaptive_gain_(false),
-    initialized_(false),
-    steady_state_(false),
-    q0_(1), q1_(0), q2_(0), q3_(0),
-    wx_prev_(0), wy_prev_(0), wz_prev_(0),
-    wx_bias_(0), wy_bias_(0), wz_bias_(0) { }
-
-ComplementaryFilter::~ComplementaryFilter() { }
-
-void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation)
-{
-  do_bias_estimation_ = do_bias_estimation;
-}
-
-bool ComplementaryFilter::getDoBiasEstimation() const
-{
-  return do_bias_estimation_;
-}
-
-void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain)
-{
-  do_adaptive_gain_ = do_adaptive_gain;
-}
-
-bool ComplementaryFilter::getDoAdaptiveGain() const
-{
-  return do_adaptive_gain_;
-}
-
-bool ComplementaryFilter::setGainAcc(double gain)
-{
-  if (gain >= 0 && gain <= 1.0)
-  {
-    gain_acc_ = gain;
-    return true;
-  }
-  else
-    return false;
-}
-bool ComplementaryFilter::setGainMag(double gain)
-{
-  if (gain >= 0 && gain <= 1.0)
-  {
-    gain_mag_ = gain;
-    return true;
-  }
-  else
-    return false;
-}
-
-double ComplementaryFilter::getGainAcc() const 
-{
-  return gain_acc_;
-}
-
-double ComplementaryFilter::getGainMag() const 
-{
-  return gain_mag_;
-}
-
-bool ComplementaryFilter::getSteadyState() const 
-{
-  return steady_state_;
-}
-
-bool ComplementaryFilter::setBiasAlpha(double bias_alpha)
-{
-  if (bias_alpha >= 0 && bias_alpha <= 1.0)
-  {
-    bias_alpha_ = bias_alpha;
-    return true;
-  }
-  else
-    return false;
-}
-
-double ComplementaryFilter::getBiasAlpha() const 
-{
-  return bias_alpha_;
-}
-
-void ComplementaryFilter::setOrientation(
-    double q0, double q1, double q2, double q3) 
-{
-  // Set the state to inverse (state is fixed wrt body).
-  invertQuaternion(q0, q1, q2, q3, q0_, q1_, q2_, q3_);
-}
-
-
-double ComplementaryFilter::getAngularVelocityBiasX() const
-{
-  return wx_bias_;
-}
-
-double ComplementaryFilter::getAngularVelocityBiasY() const
-{
-  return wy_bias_;
-}
-
-double ComplementaryFilter::getAngularVelocityBiasZ() const
-{
-  return wz_bias_;
-}
-
-void ComplementaryFilter::update(double ax, double ay, double az, 
-                                 double wx, double wy, double wz,
-                                 double dt)
-{
-  if (!initialized_) 
-  {
-    // First time - ignore prediction:
-    getMeasurement(ax, ay, az,
-                   q0_, q1_, q2_, q3_);
-    initialized_ = true;
-    return;
-  }
-  
-  // Bias estimation.
-  if (do_bias_estimation_)
-    updateBiases(ax, ay, az, wx, wy, wz);
-
-  // Prediction.
-  double q0_pred, q1_pred, q2_pred, q3_pred;
-  getPrediction(wx, wy, wz, dt,
-                q0_pred, q1_pred, q2_pred, q3_pred);   
-     
-  // Correction (from acc): 
-  // q_ = q_pred * [(1-gain) * qI + gain * dq_acc]
-  // where qI = identity quaternion
-  double dq0_acc, dq1_acc, dq2_acc, dq3_acc;  
-  getAccCorrection(ax, ay, az,
-                   q0_pred, q1_pred, q2_pred, q3_pred,
-                   dq0_acc, dq1_acc, dq2_acc, dq3_acc);
-  
-  double gain;
-  if (do_adaptive_gain_)
-  {  
-    gain = getAdaptiveGain(gain_acc_, ax, ay, az);
-    
-  }
-  else
-  {
-    gain = gain_acc_;
-    
-  }
-
-  scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
-
-  quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
-                           dq0_acc, dq1_acc, dq2_acc, dq3_acc,
-                           q0_, q1_, q2_, q3_);
-
-  normalizeQuaternion(q0_, q1_, q2_, q3_);
-}
-
-void ComplementaryFilter::update(double ax, double ay, double az, 
-                                 double wx, double wy, double wz,
-                                 double mx, double my, double mz,
-                                 double dt)
-{
-  if (!initialized_) 
-  {
-    // First time - ignore prediction:
-    getMeasurement(ax, ay, az,
-                   mx, my, mz,
-                   q0_, q1_, q2_, q3_);
-    initialized_ = true;
-    return;
-  }
-
-  // Bias estimation.
-  if (do_bias_estimation_)
-    updateBiases(ax, ay, az, wx, wy, wz);
-
-  // Prediction.
-  double q0_pred, q1_pred, q2_pred, q3_pred;
-  getPrediction(wx, wy, wz, dt,
-                q0_pred, q1_pred, q2_pred, q3_pred);   
-     
-  // Correction (from acc): 
-  // q_temp = q_pred * [(1-gain) * qI + gain * dq_acc]
-  // where qI = identity quaternion
-  double dq0_acc, dq1_acc, dq2_acc, dq3_acc;  
-  getAccCorrection(ax, ay, az,
-                   q0_pred, q1_pred, q2_pred, q3_pred,
-                   dq0_acc, dq1_acc, dq2_acc, dq3_acc);
-  double alpha = gain_acc_;  
-  if (do_adaptive_gain_)
-     alpha = getAdaptiveGain(gain_acc_, ax, ay, az);
-  scaleQuaternion(alpha, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
-
-  double q0_temp, q1_temp, q2_temp, q3_temp;
-  quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
-                           dq0_acc, dq1_acc, dq2_acc, dq3_acc,
-                           q0_temp, q1_temp, q2_temp, q3_temp);
-  
-  // Correction (from mag):
-  // q_ = q_temp * [(1-gain) * qI + gain * dq_mag]
-  // where qI = identity quaternion
-  double dq0_mag, dq1_mag, dq2_mag, dq3_mag;  
-  getMagCorrection(mx, my, mz,
-                   q0_temp, q1_temp, q2_temp, q3_temp,
-                   dq0_mag, dq1_mag, dq2_mag, dq3_mag);
-
-  scaleQuaternion(gain_mag_, dq0_mag, dq1_mag, dq2_mag, dq3_mag);
-
-  quaternionMultiplication(q0_temp, q1_temp, q2_temp, q3_temp,
-                           dq0_mag, dq1_mag, dq2_mag, dq3_mag,
-                           q0_, q1_, q2_, q3_);
-
-  normalizeQuaternion(q0_, q1_, q2_, q3_);
-}
-
-bool ComplementaryFilter::checkState(double ax, double ay, double az, 
-                                     double wx, double wy, double wz) const
-{
-  double acc_magnitude = sqrt(ax*ax + ay*ay + az*az);
-  if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold)
-    return false;
-
-  if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold ||
-      fabs(wy - wy_prev_) > kDeltaAngularVelocityThreshold ||
-      fabs(wz - wz_prev_) > kDeltaAngularVelocityThreshold)
-    return false;
-
-  if (fabs(wx - wx_bias_) > kAngularVelocityThreshold ||
-      fabs(wy - wy_bias_) > kAngularVelocityThreshold ||
-      fabs(wz - wz_bias_) > kAngularVelocityThreshold)
-    return false;
-
-  return true;
-}
-
-void ComplementaryFilter::updateBiases(double ax, double ay, double az, 
-                                       double wx, double wy, double wz)
-{
-  steady_state_ = checkState(ax, ay, az, wx, wy, wz);
-
-  if (steady_state_)
-  {
-    wx_bias_ += bias_alpha_ * (wx - wx_bias_);
-    wy_bias_ += bias_alpha_ * (wy - wy_bias_);
-    wz_bias_ += bias_alpha_ * (wz - wz_bias_);
-  }
-
-  wx_prev_ = wx; 
-  wy_prev_ = wy; 
-  wz_prev_ = wz;
-}
-
-void ComplementaryFilter::getPrediction(
-    double wx, double wy, double wz, double dt, 
-    double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const
-{
-  double wx_unb = wx - wx_bias_;
-  double wy_unb = wy - wy_bias_;
-  double wz_unb = wz - wz_bias_;
-
-  q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_);
-  q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_);
-  q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_);
-  q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_);
-  
-  normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred);
-}
-
-void ComplementaryFilter::getMeasurement(
-    double ax, double ay, double az, 
-    double mx, double my, double mz,  
-    double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
-{
-  // q_acc is the quaternion obtained from the acceleration vector representing 
-  // the orientation of the Global frame wrt the Local frame with arbitrary yaw
-  // (intermediary frame). q3_acc is defined as 0.
-  double q0_acc, q1_acc, q2_acc, q3_acc;
-    
-  // Normalize acceleration vector.
-  normalizeVector(ax, ay, az);
-  if (az >=0)
-    {
-      q0_acc =  sqrt((az + 1) * 0.5);	
-      q1_acc = -ay/(2.0 * q0_acc);
-      q2_acc =  ax/(2.0 * q0_acc);
-      q3_acc = 0;
-    }
-    else 
-    {
-      double X = sqrt((1 - az) * 0.5);
-      q0_acc = -ay/(2.0 * X);
-      q1_acc = X;
-      q2_acc = 0;
-      q3_acc = ax/(2.0 * X);
-    }  
-  
-  // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary
-  // frame by the inverse of q_acc.
-  // l = R(q_acc)^-1 m
-  double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx + 
-      2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz;
-  double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc + 
-      q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz;
-  
-  // q_mag is the quaternion that rotates the Global frame (North West Up) into
-  // the intermediary frame. q1_mag and q2_mag are defined as 0.
-	double gamma = lx*lx + ly*ly;	
-	double beta = sqrt(gamma + lx*sqrt(gamma));
-  double q0_mag = beta / (sqrt(2.0 * gamma));  
-  double q3_mag = ly / (sqrt(2.0) * beta); 
-    
-  // The quaternion multiplication between q_acc and q_mag represents the 
-  // quaternion, orientation of the Global frame wrt the local frame.  
-  // q = q_acc times q_mag 
-  quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc, 
-                           q0_mag, 0, 0, q3_mag,
-                           q0_meas, q1_meas, q2_meas, q3_meas ); 
-  //q0_meas = q0_acc*q0_mag;     
-  //q1_meas = q1_acc*q0_mag + q2_acc*q3_mag;
-  //q2_meas = q2_acc*q0_mag - q1_acc*q3_mag;
-  //q3_meas = q0_acc*q3_mag;
-}
-
-
-void ComplementaryFilter::getMeasurement(
-    double ax, double ay, double az, 
-    double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
-{
-  // q_acc is the quaternion obtained from the acceleration vector representing 
-  // the orientation of the Global frame wrt the Local frame with arbitrary yaw
-  // (intermediary frame). q3_acc is defined as 0.
-     
-  // Normalize acceleration vector.
-  normalizeVector(ax, ay, az);
-
-  if (az >=0)
-  {
-    q0_meas =  sqrt((az + 1) * 0.5);	
-    q1_meas = -ay/(2.0 * q0_meas);
-    q2_meas =  ax/(2.0 * q0_meas);
-    q3_meas = 0;
-  }
-  else 
-  {
-    double X = sqrt((1 - az) * 0.5);
-    q0_meas = -ay/(2.0 * X);
-    q1_meas = X;
-    q2_meas = 0;
-    q3_meas = ax/(2.0 * X);
-  }  
-}
-
-void ComplementaryFilter::getAccCorrection(
-  double ax, double ay, double az,
-  double p0, double p1, double p2, double p3,
-  double& dq0, double& dq1, double& dq2, double& dq3)
-{
-  // Normalize acceleration vector.
-  normalizeVector(ax, ay, az);
-  
-  // Acceleration reading rotated into the world frame by the inverse predicted
-  // quaternion (predicted gravity):
-  double gx, gy, gz;
-  rotateVectorByQuaternion(ax, ay, az,
-                           p0, -p1, -p2, -p3, 
-                           gx, gy, gz);
-  
-  // Delta quaternion that rotates the predicted gravity into the real gravity:
-  dq0 =  sqrt((gz + 1) * 0.5);	
-  dq1 = -gy/(2.0 * dq0);
-  dq2 =  gx/(2.0 * dq0);
-  dq3 =  0.0;
-}
-
-void ComplementaryFilter::getMagCorrection(
-  double mx, double my, double mz,
-  double p0, double p1, double p2, double p3,
-  double& dq0, double& dq1, double& dq2, double& dq3)
-{
-  // Magnetic reading rotated into the world frame by the inverse predicted
-  // quaternion:
-  double lx, ly, lz;
-  rotateVectorByQuaternion(mx, my, mz,
-                           p0, -p1, -p2, -p3, 
-                           lx, ly, lz);
-   
-  // Delta quaternion that rotates the l so that it lies in the xz-plane 
-  // (points north):
-  double gamma = lx*lx + ly*ly;	
-	double beta = sqrt(gamma + lx*sqrt(gamma));
-  dq0 = beta / (sqrt(2.0 * gamma));
-  dq1 = 0.0;
-  dq2 = 0.0;  
-  dq3 = ly / (sqrt(2.0) * beta);  
-}
- 
-void ComplementaryFilter::getOrientation(
-    double& q0, double& q1, double& q2, double& q3) const
-{
-  // Return the inverse of the state (state is fixed wrt body).
-  invertQuaternion(q0_, q1_, q2_, q3_, q0, q1, q2, q3);
-}
-
-double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, double az)
-{
-  double a_mag = sqrt(ax*ax + ay*ay + az*az);
-  double error = fabs(a_mag - kGravity)/kGravity;
-  double factor;
-  double error1 = 0.1;
-  double error2 = 0.2;
-  double m = 1.0/(error1 - error2);
-  double b = 1.0 - m*error1;
-  if (error < error1)
-    factor = 1.0;
-  else if (error < error2)
-    factor = m*error + b;
-  else 
-    factor = 0.0;
-  //printf("FACTOR: %f \n", factor);
-  return factor*alpha;
-}
-
-void normalizeVector(double& x, double& y, double& z)
-{
-  double norm = sqrt(x*x + y*y + z*z);
-
-  x /= norm;
-  y /= norm;
-  z /= norm;
-}
-
-void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
-{
-  double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
-  q0 /= norm;  
-  q1 /= norm;
-  q2 /= norm;
-  q3 /= norm;
-}
-
-void invertQuaternion(
-  double q0, double q1, double q2, double q3,
-  double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv)
-{
-  // Assumes quaternion is normalized.
-  q0_inv = q0;
-  q1_inv = -q1;
-  q2_inv = -q2;
-  q3_inv = -q3;
-}
-
-void scaleQuaternion(
-  double gain,
-  double& dq0, double& dq1, double& dq2, double& dq3)
-{
-	if (dq0 < 0.0)//0.9
-  {
-    // Slerp (Spherical linear interpolation):
-    double angle = acos(dq0);
-    double A = sin(angle*(1.0 - gain))/sin(angle);
-    double B = sin(angle * gain)/sin(angle);
-    dq0 = A + B * dq0;
-    dq1 = B * dq1;
-    dq2 = B * dq2;
-    dq3 = B * dq3;
-  }
-  else
-  {
-    // Lerp (Linear interpolation):
-    dq0 = (1.0 - gain) + gain * dq0;
-    dq1 = gain * dq1;
-    dq2 = gain * dq2;
-    dq3 = gain * dq3;
-  }
-
-  normalizeQuaternion(dq0, dq1, dq2, dq3);  
-}
-
-void quaternionMultiplication(
-  double p0, double p1, double p2, double p3,
-  double q0, double q1, double q2, double q3,
-  double& r0, double& r1, double& r2, double& r3)
-{
-  // r = p q
-  r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3;
-  r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2;
-  r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1;
-  r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0;
-}
-
-void rotateVectorByQuaternion( 
-  double x, double y, double z,
-  double q0, double q1, double q2, double q3,
-  double& vx, double& vy, double& vz)
-{ 
-  vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z;
-  vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z;
-  vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z;
-}
-
-
-}  // namespace imu_tools

imu_tools/imu_complementary_filter/src/complementary_filter_node.cpp

@@ -1,42 +0,0 @@
-/*
-  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
-
-	@section LICENSE
-  Copyright (c) 2015, City University of New York
-  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
-	All rights reserved.
-
-	Redistribution and use in source and binary forms, with or without
-	modification, are permitted provided that the following conditions are met:
-     1. Redistributions of source code must retain the above copyright
-        notice, this list of conditions and the following disclaimer.
-     2. 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.
-     3. Neither the name of the City College of New York 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 CCNY ROBOTICS LAB 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.
-*/
-
-#include "imu_complementary_filter/complementary_filter_ros.h"
-
-int main (int argc, char **argv)
-{
-  ros::init (argc, argv, "ComplementaryFilterROS");
-  ros::NodeHandle nh;
-  ros::NodeHandle nh_private("~");
-  imu_tools::ComplementaryFilterROS filter(nh, nh_private);
-  ros::spin();
-  return 0;
-}

imu_tools/imu_complementary_filter/src/complementary_filter_ros.cpp

@@ -1,305 +0,0 @@
-/*
-  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
-
-	@section LICENSE
-  Copyright (c) 2015, City University of New York
-  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
-	All rights reserved.
-
-	Redistribution and use in source and binary forms, with or without
-	modification, are permitted provided that the following conditions are met:
-     1. Redistributions of source code must retain the above copyright
-        notice, this list of conditions and the following disclaimer.
-     2. 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.
-     3. Neither the name of the City College of New York 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 CCNY ROBOTICS LAB 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.
-*/
-
-#include "imu_complementary_filter/complementary_filter_ros.h"
-
-#include <std_msgs/Float64.h>
-#include <std_msgs/Bool.h>
-
-namespace imu_tools {
-
-ComplementaryFilterROS::ComplementaryFilterROS(
-    const ros::NodeHandle& nh,
-    const ros::NodeHandle& nh_private):
-  nh_(nh),
-  nh_private_(nh_private),
-  initialized_filter_(false)
-{
-  ROS_INFO("Starting ComplementaryFilterROS");
-  initializeParams();
-
-  int queue_size = 5;
-
-  // Register publishers:
-  imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(ros::names::resolve("imu") + "/data", queue_size);
-
-  if (publish_debug_topics_)
-  {
-      rpy_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
-                  ros::names::resolve("imu") + "/rpy/filtered", queue_size);
-
-      if (filter_.getDoBiasEstimation())
-      {
-        state_publisher_ = nh_.advertise<std_msgs::Bool>(
-                    ros::names::resolve("imu") + "/steady_state", queue_size);
-      }
-  }
-
-  // Register IMU raw data subscriber.
-  imu_subscriber_.reset(new ImuSubscriber(nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
-
-  // Register magnetic data subscriber.
-  if (use_mag_)
-  {
-    mag_subscriber_.reset(new MagSubscriber(nh_, ros::names::resolve("imu") + "/mag", queue_size));
-
-    sync_.reset(new Synchronizer(
-        SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
-    sync_->registerCallback(
-        boost::bind(&ComplementaryFilterROS::imuMagCallback, this, _1, _2));
-  }
-  else
-  {
-    imu_subscriber_->registerCallback(
-        &ComplementaryFilterROS::imuCallback, this);
-  }
-}
-
-ComplementaryFilterROS::~ComplementaryFilterROS()
-{
-  ROS_INFO("Destroying ComplementaryFilterROS");
-}
-
-void ComplementaryFilterROS::initializeParams()
-{
-  double gain_acc;
-  double gain_mag;
-  bool do_bias_estimation;
-  double bias_alpha;
-  bool do_adaptive_gain;
-
-  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
-    fixed_frame_ = "odom";
-  if (!nh_private_.getParam ("use_mag", use_mag_))
-    use_mag_ = false;
-  if (!nh_private_.getParam ("publish_tf", publish_tf_))
-    publish_tf_ = false;
-  if (!nh_private_.getParam ("reverse_tf", reverse_tf_))
-    reverse_tf_ = false;
-  if (!nh_private_.getParam ("constant_dt", constant_dt_))
-    constant_dt_ = 0.0;
-  if (!nh_private_.getParam ("publish_debug_topics", publish_debug_topics_))
-    publish_debug_topics_ = false;
-  if (!nh_private_.getParam ("gain_acc", gain_acc))
-    gain_acc = 0.01;
-  if (!nh_private_.getParam ("gain_mag", gain_mag))
-    gain_mag = 0.01;
-  if (!nh_private_.getParam ("do_bias_estimation", do_bias_estimation))
-    do_bias_estimation = true;
-  if (!nh_private_.getParam ("bias_alpha", bias_alpha))
-    bias_alpha = 0.01;
-  if (!nh_private_.getParam ("do_adaptive_gain", do_adaptive_gain))
-    do_adaptive_gain = true;
-
-  double orientation_stddev;
-  if (!nh_private_.getParam ("orientation_stddev", orientation_stddev))
-    orientation_stddev = 0.0;
-
-  orientation_variance_ = orientation_stddev * orientation_stddev;
-
-  filter_.setDoBiasEstimation(do_bias_estimation);
-  filter_.setDoAdaptiveGain(do_adaptive_gain);
-
-  if(!filter_.setGainAcc(gain_acc))
-    ROS_WARN("Invalid gain_acc passed to ComplementaryFilter.");
-  if (use_mag_)
-  {
-    if(!filter_.setGainMag(gain_mag))
-      ROS_WARN("Invalid gain_mag passed to ComplementaryFilter.");
-  }
-  if (do_bias_estimation)
-  {
-    if(!filter_.setBiasAlpha(bias_alpha))
-      ROS_WARN("Invalid bias_alpha passed to ComplementaryFilter.");
-  }
-
-  // check for illegal constant_dt values
-  if (constant_dt_ < 0.0)
-  {
-    // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt
-    // otherwise, it will be constant
-    ROS_WARN("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
-    constant_dt_ = 0.0;
-  }
-}
-
-void ComplementaryFilterROS::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
-{
-  const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
-  const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity;
-  const ros::Time& time = imu_msg_raw->header.stamp;
-
-  // Initialize.
-  if (!initialized_filter_)
-  {
-    time_prev_ = time;
-    initialized_filter_ = true;
-    return;
-  }
-
-  // determine dt: either constant, or from IMU timestamp
-  double dt;
-  if (constant_dt_ > 0.0)
-    dt = constant_dt_;
-  else
-    dt = (time - time_prev_).toSec();
-
-  time_prev_ = time;
-
-  // Update the filter.
-  filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt);
-
-  // Publish state.
-  publish(imu_msg_raw);
-}
-
-void ComplementaryFilterROS::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
-                                            const MagMsg::ConstPtr& mag_msg)
-{
-  const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
-  const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity;
-  const geometry_msgs::Vector3& m = mag_msg->magnetic_field;
-  const ros::Time& time = imu_msg_raw->header.stamp;
-
-  // Initialize.
-  if (!initialized_filter_)
-  {
-    time_prev_ = time;
-    initialized_filter_ = true;
-    return;
-  }
-
-  // Calculate dt.
-  double dt = (time - time_prev_).toSec();
-  time_prev_ = time;
-   //ros::Time t_in, t_out;
-  //t_in = ros::Time::now();
-  // Update the filter.
-  if (std::isnan(m.x) || std::isnan(m.y) || std::isnan(m.z))
-    filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt);
-  else
-    filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, m.x, m.y, m.z, dt);
-
-  //t_out = ros::Time::now();
-  //float dt_tot = (t_out - t_in).toSec() * 1000.0; // In msec.
-  //printf("%.6f\n", dt_tot);
-  // Publish state.
-  publish(imu_msg_raw);
-}
-
-tf::Quaternion ComplementaryFilterROS::hamiltonToTFQuaternion(
-    double q0, double q1, double q2, double q3) const
-{
-  // ROS uses the Hamilton quaternion convention (q0 is the scalar). However,
-  // the ROS quaternion is in the form [x, y, z, w], with w as the scalar.
-  return tf::Quaternion(q1, q2, q3, q0);
-}
-
-void ComplementaryFilterROS::publish(
-    const sensor_msgs::Imu::ConstPtr& imu_msg_raw)
-{
-  // Get the orientation:
-  double q0, q1, q2, q3;
-  filter_.getOrientation(q0, q1, q2, q3);
-  tf::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3);
-
-  // Create and publish fitlered IMU message.
-  boost::shared_ptr<sensor_msgs::Imu> imu_msg =
-      boost::make_shared<sensor_msgs::Imu>(*imu_msg_raw);
-  tf::quaternionTFToMsg(q, imu_msg->orientation);
-
-  imu_msg->orientation_covariance[0] = orientation_variance_;
-  imu_msg->orientation_covariance[1] = 0.0;
-  imu_msg->orientation_covariance[2] = 0.0;
-  imu_msg->orientation_covariance[3] = 0.0;
-  imu_msg->orientation_covariance[4] = orientation_variance_;
-  imu_msg->orientation_covariance[5] = 0.0;
-  imu_msg->orientation_covariance[6] = 0.0;
-  imu_msg->orientation_covariance[7] = 0.0;
-  imu_msg->orientation_covariance[8] = orientation_variance_;
-
-  // Account for biases.
-  if (filter_.getDoBiasEstimation())
-  {
-    imu_msg->angular_velocity.x -= filter_.getAngularVelocityBiasX();
-    imu_msg->angular_velocity.y -= filter_.getAngularVelocityBiasY();
-    imu_msg->angular_velocity.z -= filter_.getAngularVelocityBiasZ();
-  }
-
-  imu_publisher_.publish(imu_msg);
-
-  if (publish_debug_topics_)
-  {
-      // Create and publish roll, pitch, yaw angles
-      geometry_msgs::Vector3Stamped rpy;
-      rpy.header = imu_msg_raw->header;
-
-      tf::Matrix3x3 M;
-      M.setRotation(q);
-      M.getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
-      rpy_publisher_.publish(rpy);
-
-      // Publish whether we are in the steady state, when doing bias estimation
-      if (filter_.getDoBiasEstimation())
-      {
-        std_msgs::Bool state_msg;
-        state_msg.data = filter_.getSteadyState();
-        state_publisher_.publish(state_msg);
-      }
-  }
-
-  if (publish_tf_)
-  {
-      // Create and publish the ROS tf.
-      tf::Transform transform;
-      transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
-      transform.setRotation(q);
-
-      if (reverse_tf_)
-      {
-          tf_broadcaster_.sendTransform(
-              tf::StampedTransform(transform.inverse(),
-                                   imu_msg_raw->header.stamp,
-                                   imu_msg_raw->header.frame_id,
-                                   fixed_frame_));
-      }
-      else
-      {
-          tf_broadcaster_.sendTransform(
-              tf::StampedTransform(transform,
-                                   imu_msg_raw->header.stamp,
-                                   fixed_frame_,
-                                   imu_msg_raw->header.frame_id));
-      }
-  }
-}
-
-}  // namespace imu_tools

imu_tools/imu_filter_madgwick/CHANGELOG.rst

@@ -1,217 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package imu_filter_madgwick
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.2.3 (2021-04-09)
-------------------
-* Fix "non standard content" warning in imu_tools metapackage
-  Fixes `#135 <https://github.com/ccny-ros-pkg/imu_tools/issues/135>`_.
-* Add example launch file for imu_filter_madgwick (`#132 <https://github.com/ccny-ros-pkg/imu_tools/issues/132>`_)
-* Update maintainers in package.xml
-* Fix warnings: reordering and unused vars
-* Set cmake_policy CMP0048 to fix warning
-* Contributors: Martin Günther, pietrocolombo
-
-1.2.2 (2020-05-25)
-------------------
-* Drop the signals component of Boost (`#103 <https://github.com/ccny-ros-pkg/imu_tools/issues/103>`_)
-* Add the option to remove the gravity vector (`#101 <https://github.com/ccny-ros-pkg/imu_tools/issues/101>`_)
-* fix install path & boost linkage issues
-* Contributors: Alexis Paques, Martin Günther, Mike Purvis, Sean Yen
-
-1.2.1 (2019-05-06)
-------------------
-* Skip messages and warn if computeOrientation fails
-* Contributors: Martin Günther
-
-1.2.0 (2018-05-25)
-------------------
-* Remove outdated Makefile
-* Add warning when IMU time stamp is zero
-  Closes `#82 <https://github.com/ccny-ros-pkg/imu_tools/issues/82>`_.
-* update to use non deprecated pluginlib macro (`#77 <https://github.com/ccny-ros-pkg/imu_tools/issues/77>`_)
-* Contributors: Martin Günther, Mikael Arguedas
-
-1.1.5 (2017-05-24)
-------------------
-* Initial release into Lunar
-* Remove support for Vector3 mag messages
-* Change default world_frame = enu
-* Rewrite rosbags: Use MagneticField for magnetometer
-* Contributors: Martin Günther
-
-1.1.4 (2017-05-22)
-------------------
-* Print warning if waiting for topic
-  Closes `#61 <https://github.com/ccny-ros-pkg/imu_tools/issues/61>`_.
-* Fix boost::lock_error on shutdown
-* Contributors: Martin Günther
-
-1.1.3 (2017-03-10)
-------------------
-* Return precisely normalized quaternions
-  Fixes `#67 <https://github.com/ccny-ros-pkg/imu_tools/issues/67>`_ : TF_DENORMALIZED_QUATERNION warning added in TF2 0.5.14.
-* Tests: Check that output quaternions are normalized
-* Fixed lock so it stays in scope until end of method.
-* Contributors: Jason Mercer, Martin Guenther, Martin Günther
-
-1.1.2 (2016-09-07)
-------------------
-* Add missing dependency on tf2_geometry_msgs
-* Contributors: Martin Guenther
-
-1.1.1 (2016-09-07)
-------------------
-* Add parameter "world_frame": optionally use ENU or NED instead of NWU
-  convention (from `#60 <https://github.com/ccny-ros-pkg/imu_tools/issues/60>`_;
-  closes `#36 <https://github.com/ccny-ros-pkg/imu_tools/issues/36>`_)
-* Add parameter "stateless" for debugging purposes: don't do any stateful
-  filtering, but instead publish the orientation directly computed from the
-  latest accelerometer (+ optionally magnetometer) readings alone
-* Replace the (buggy) Euler-angle-based initialization routine
-  (ImuFilterRos::computeRPY) by a correct transformation
-  matrix based one (StatelessOrientation::computeOrientation) and make it
-  available as a library function
-* Refactor madgwickAHRSupdate() (pull out some functions, remove micro
-  optimizations to improve readability)
-* Add unit tests
-* Contributors: Martin Guenther, Michael Stoll
-
-1.1.0 (2016-04-25)
-------------------
-
-1.0.11 (2016-04-22)
--------------------
-* Jade: Change default: use_magnetic_field_msg = true
-* Contributors: Martin Guenther
-
-1.0.10 (2016-04-22)
--------------------
-
-1.0.9 (2015-10-16)
-------------------
-
-1.0.8 (2015-10-07)
-------------------
-
-1.0.7 (2015-10-07)
-------------------
-
-1.0.6 (2015-10-06)
-------------------
-* Split ImuFilter class into ImuFilter and ImuFilterRos in order to
-  have a C++ API to the Madgwick algorithm
-* Properly install header files.
-* Contributors: Martin Günther, Michael Stoll
-
-1.0.5 (2015-06-24)
-------------------
-* Add "~use_magnetic_field_msg" param.
-  This allows the user to subscribe to the /imu/mag topic as a
-  sensor_msgs/MagneticField rather than a geometry_msgs/Vector3Stamped.
-  The default for now is false, which preserves the legacy behaviour via a
-  separate subscriber which converts Vector3Stamped to MagneticField and
-  republishes.
-* Contributors: Mike Purvis, Martin Günther
-
-1.0.4 (2015-05-06)
-------------------
-* update dynamic reconfigure param descriptions
-* only advertise debug topics if they are used
-* allow remapping of the whole imu namespace
-  with this change, all topics can be remapped at once, like this:
-  rosrun imu_filter_madgwick imu_filter_node imu:=my_imu
-* Contributors: Martin Günther
-
-1.0.3 (2015-01-29)
-------------------
-* Add std dev parameter to orientation estimate covariance matrix
-* Port imu_filter_madgwick to tf2
-* Switch to smart pointer
-* Contributors: Paul Bovbel, Martin Günther
-
-1.0.2 (2015-01-27)
-------------------
-* fix tf publishing (switch parent + child frames)
-  The orientation is between a fixed inertial frame (``fixed_frame_``) and
-  the frame that the IMU is mounted in (``imu_frame_``). Also,
-  ``imu_msg.header.frame`` should be ``imu_frame_``, but the corresponding TF
-  goes from ``fixed_frame_`` to ``imu_frame_``. This commit fixes that; for
-  the ``reverse_tf`` case, it was already correct.
-  Also see http://answers.ros.org/question/50870/what-frame-is-sensor_msgsimuorientation-relative-to/.
-  Note that tf publishing should be enabled for debug purposes only, since we can only
-  provide the orientation, not the translation.
-* Add ~reverse_tf parameter for the robots which does not have IMU on root-link
-* Log mag bias on startup to assist with debugging.
-* add boost depends to CMakeLists
-  All non-catkin things that we expose in our headers should be added to
-  the DEPENDS, so that packages which depend on our package will also
-  automatically link against it.
-* Contributors: Martin Günther, Mike Purvis, Ryohei Ueda
-
-1.0.1 (2014-12-10)
-------------------
-* add me as maintainer to package.xml
-* turn mag_bias into a dynamic reconfigure param
-  Also rename mag_bias/x --> mag_bias_x etc., since dynamic reconfigure
-  doesn't allow slashes.
-* gain and zeta already set via dynamic_reconfigure
-  Reading the params explicitly is not necessary. Instead,
-  dynamic_reconfigure will read them and set them as soon as we call
-  config_server->setCallback().
-* reconfigure server: use proper namespace
-  Before, the reconfigure server used the private namespace of the nodelet
-  *manager* instead of the nodelet, so the params on the parameter server
-  and the ones from dynamic_reconfigure were out of sync.
-* check for NaNs in magnetometer message
-  Some magnetometer drivers (e.g. phidgets_drivers) output NaNs, which
-  is a valid way of saying that this measurement is invalid. During
-  initialization, we simply wait for the first valid message, assuming
-  there will be one soon.
-* magnetometer msg check: isnan() -> !isfinite()
-  This catches both inf and NaN. Not sure whether sending inf in a Vector3
-  message is valid (Nan is), but this doesn't hurt and is just good
-  defensive programming.
-* Initialize yaw from calibrated magnetometer data
-  * Add magnetometer biases (mag_bias/x and mag_bias/y) for hard-iron compensation.
-  * Initialize yaw orientation from magnetometer reading.
-  * Add imu/rpy/raw and imu/rpy/filtered as debug topics. imu/rpy/raw can be used for computing magnetometer biases. imu/rpy/filtered topic is for user readability only.
-* Contributors: Martin Günther, Shokoofeh Pourmehr
-
-1.0.0 (2014-09-03)
-------------------
-* First public release
-* Remove setting imu message frame to fixed/odom
-* CMakeLists: remove unnecessary link_directories, LIBRARY_OUTPUT_PATH
-* add missing build dependency on generated config
-  This removes a racing condition from the build process.
-* install nodelet xml file
-  Otherwise the nodelet can't be found
-* fix implementation of invSqrt()
-  The old invSqrt() implementation causes the estimate to diverge under
-  constant input. The problem was the line `long i = (long)&y;`, where 64
-  bits are read from a 32 bit number. Thanks to @tomas-c for spotting this
-  and pointing out the solution.
-* catkinization of imu_tools metapackage
-* fix typo: zeta -> ``zeta_``
-* fix initialization of initial rotation
-* gyro drift correction function added in MARG implementation
-* set "zeta" as a parameter for dynamic reconfigure in the .cfg file
-* add new test bag: phidgets_imu_upside_down
-* add parameter publish_tf
-  When the imu is used together with other packages, such as
-  robot_pose_ekf, publishing the transform often interferes with those
-  packages. This parameter allows to disable tf publishing.
-* add some sample imu data
-* more informative constant_dt message. Reverts to 0.0 on illegal param value
-* imu_filter_madgwick manifest now correctly lists the package as GPL license.
-* orientation is initialized from acceleration vector on first message received
-* added dynamic reconfigure for gain parameter. Added better messages about constant_dt param at startup
-* the tf published is now timestamped as the imu msg, and not as now(). Also added constant dt option for the imu+mag callback
-* fix the transform publish -- from the fixed frame to the frame of the imu
-* add a tf broadcaster with the orientation
-* as per PaulKemppi: added option to set constant dt
-* walchko: Needed to add namespace: std::isnan() and needed to add rosbuild_link_boost(imu_filter signals) to CMakeLists.txt
-* added sebastian's name and link to the manifest
-* renamed imu_filter to imu_filter_madgwick
-* Contributors: Ivan Dryanovski, Martin Günther, Mike Purvis, Sameer Parekh, TUG-DESTOP, Francisco Vina, Michael Görner, Paul Kemppi, Tomas Cerskus, Kevin Walchko

imu_tools/imu_filter_madgwick/CMakeLists.txt

@@ -1,74 +0,0 @@
-cmake_minimum_required(VERSION 3.5.1)
-project(imu_filter_madgwick)
-
-find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs tf2 tf2_geometry_msgs tf2_ros nodelet pluginlib message_filters dynamic_reconfigure)
-
-find_package(Boost REQUIRED COMPONENTS system thread)
-
-# Generate dynamic parameters
-generate_dynamic_reconfigure_options(cfg/ImuFilterMadgwick.cfg)
-
-
-catkin_package(
-  DEPENDS Boost
-  CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs tf2_ros tf2_geometry_msgs nodelet pluginlib message_filters dynamic_reconfigure
-  INCLUDE_DIRS
-  LIBRARIES imu_filter imu_filter_nodelet
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
-)
-
-
-# create imu_filter library
-add_library (imu_filter src/imu_filter.cpp  src/imu_filter_ros.cpp src/stateless_orientation.cpp)
-add_dependencies(imu_filter ${PROJECT_NAME}_gencfg)
-target_link_libraries(imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-# create imu_filter_nodelet library
-add_library (imu_filter_nodelet src/imu_filter_nodelet.cpp)
-add_dependencies(imu_filter_nodelet ${PROJECT_NAME}_gencfg)
-target_link_libraries(imu_filter_nodelet imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-# create imu_filter_node executable
-add_executable(imu_filter_node src/imu_filter_node.cpp)
-add_dependencies(imu_filter_node ${PROJECT_NAME}_gencfg)
-target_link_libraries(imu_filter_node imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-
-install(TARGETS imu_filter_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(TARGETS imu_filter imu_filter_nodelet
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-)
-
-install(DIRECTORY include/${PROJECT_NAME}/
-   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-   FILES_MATCHING PATTERN "*.h"
-)
-
-install(FILES imu_filter_nodelet.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-
-
-if(CATKIN_ENABLE_TESTING)
-  catkin_add_gtest(${PROJECT_NAME}-madgwick_test
-    test/stateless_orientation_test.cpp
-    test/madgwick_test.cpp
-  )
-  target_link_libraries(${PROJECT_NAME}-madgwick_test
-    imu_filter
-    ${catkin_LIBRARIES}
-  )
-endif()

imu_tools/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg

@@ -1,18 +0,0 @@
-#! /usr/bin/env python
-# IMU Filter dynamic reconfigure
-
-PACKAGE='imu_filter_madgwick'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-                                                                    
-gen.add("gain", double_t, 0, "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal.", 0.1, 0.0, 1.0) 
-gen.add("zeta", double_t, 0, "Gyro drift gain (approx. rad/s).", 0, -1.0, 1.0) 
-gen.add("mag_bias_x", double_t, 0, "Magnetometer bias (hard iron correction), x component.", 0, -10.0, 10.0)
-gen.add("mag_bias_y", double_t, 0, "Magnetometer bias (hard iron correction), y component.", 0, -10.0, 10.0)
-gen.add("mag_bias_z", double_t, 0, "Magnetometer bias (hard iron correction), z component.", 0, -10.0, 10.0)
-gen.add("orientation_stddev", double_t, 0, "Standard deviation of the orientation estimate.", 0, 0, 1.0)
-
-exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "ImuFilterMadgwick"))
-

imu_tools/imu_filter_madgwick/imu_filter_nodelet.xml

@@ -1,9 +0,0 @@
-<!-- Imu Filter nodelet publisher -->
-<library path="lib/libimu_filter_nodelet">
-  <class name="imu_filter_madgwick/ImuFilterNodelet" type="ImuFilterNodelet" 
-    base_class_type="nodelet::Nodelet">
-    <description>
-      Imu Filter nodelet publisher.
-    </description>
-  </class>
-</library>

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h

@@ -1,107 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef IMU_FILTER_MADWICK_IMU_FILTER_H
-#define IMU_FILTER_MADWICK_IMU_FILTER_H
-
-#include <imu_filter_madgwick/world_frame.h>
-#include <iostream>
-#include <cmath>
-
-class ImuFilter
-{
-  public:
-
-    ImuFilter();
-    virtual ~ImuFilter();
-
-  private:
-    // **** paramaters
-    double gain_;    // algorithm gain
-    double zeta_;    // gyro drift bias gain
-    WorldFrame::WorldFrame world_frame_;    // NWU, ENU, NED
-
-    // **** state variables
-    double q0, q1, q2, q3;  // quaternion
-    float w_bx_, w_by_, w_bz_; //
-
-public:
-    void setAlgorithmGain(double gain)
-    {
-        gain_ = gain;
-    }
-
-    void setDriftBiasGain(double zeta)
-    {
-        zeta_ = zeta;
-    }
-
-    void setWorldFrame(WorldFrame::WorldFrame frame)
-    {
-        world_frame_ = frame;
-    }
-
-    void getOrientation(double& q0, double& q1, double& q2, double& q3)
-    {
-        q0 = this->q0;
-        q1 = this->q1;
-        q2 = this->q2;
-        q3 = this->q3;
-
-        // perform precise normalization of the output, using 1/sqrt()
-        // instead of the fast invSqrt() approximation. Without this,
-        // TF2 complains that the quaternion is not normalized.
-        double recipNorm = 1 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-        q0 *= recipNorm;
-        q1 *= recipNorm;
-        q2 *= recipNorm;
-        q3 *= recipNorm;
-    }
-
-    void setOrientation(double q0, double q1, double q2, double q3)
-    {
-        this->q0 = q0;
-        this->q1 = q1;
-        this->q2 = q2;
-        this->q3 = q3;
-
-        w_bx_ = 0;
-        w_by_ = 0;
-        w_bz_ = 0;
-    }
-
-    void madgwickAHRSupdate(float gx, float gy, float gz,
-                            float ax, float ay, float az,
-                            float mx, float my, float mz,
-                            float dt);
-
-    void madgwickAHRSupdateIMU(float gx, float gy, float gz,
-                               float ax, float ay, float az,
-                               float dt);
-
-    void getGravity(float& rx, float& ry, float& rz,
-                    float gravity = 9.80665);
-};
-
-#endif // IMU_FILTER_IMU_MADWICK_FILTER_H

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_nodelet.h

@@ -1,41 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H
-#define IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H
-
-#include <nodelet/nodelet.h>
-
-#include "imu_filter_madgwick/imu_filter_ros.h"
-
-class ImuFilterNodelet : public nodelet::Nodelet
-{
-  public:
-    virtual void onInit();
-
-  private:
-    boost::shared_ptr<ImuFilterRos> filter_;
-};
-
-#endif // IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h

@@ -1,116 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef IMU_FILTER_MADWICK_IMU_FILTER_ROS_H
-#define IMU_FILTER_MADWICK_IMU_FILTER_ROS_H
-
-#include <ros/ros.h>
-#include <sensor_msgs/Imu.h>
-#include <sensor_msgs/MagneticField.h>
-#include <geometry_msgs/Vector3Stamped.h>
-#include "tf2_ros/transform_broadcaster.h"
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <dynamic_reconfigure/server.h>
-
-#include "imu_filter_madgwick/imu_filter.h"
-#include "imu_filter_madgwick/ImuFilterMadgwickConfig.h"
-
-class ImuFilterRos
-{
-  typedef sensor_msgs::Imu              ImuMsg;
-  typedef sensor_msgs::MagneticField    MagMsg;
-
-  typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg> SyncPolicy;
-  typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
-  typedef message_filters::Subscriber<ImuMsg> ImuSubscriber;
-  typedef message_filters::Subscriber<MagMsg> MagSubscriber;
-
-  typedef imu_filter_madgwick::ImuFilterMadgwickConfig   FilterConfig;
-  typedef dynamic_reconfigure::Server<FilterConfig>   FilterConfigServer;
-
-  public:
-
-    ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private);
-    virtual ~ImuFilterRos();
-
-  private:
-
-    // **** ROS-related
-
-    ros::NodeHandle nh_;
-    ros::NodeHandle nh_private_;
-
-    boost::shared_ptr<ImuSubscriber> imu_subscriber_;
-    boost::shared_ptr<MagSubscriber> mag_subscriber_;
-    boost::shared_ptr<Synchronizer> sync_;
-
-    ros::Publisher rpy_filtered_debug_publisher_;
-    ros::Publisher rpy_raw_debug_publisher_;
-    ros::Publisher imu_publisher_;
-    tf2_ros::TransformBroadcaster tf_broadcaster_;
-
-    boost::shared_ptr<FilterConfigServer> config_server_;
-    ros::Timer check_topics_timer_;
-
-    // **** paramaters
-    WorldFrame::WorldFrame world_frame_;
-    bool use_mag_;
-    bool stateless_;
-    bool publish_tf_;
-    bool reverse_tf_;
-    std::string fixed_frame_;
-    std::string imu_frame_;
-    double constant_dt_;
-    bool publish_debug_topics_;
-    bool remove_gravity_vector_;
-    geometry_msgs::Vector3 mag_bias_;
-    double orientation_variance_;
-
-    // **** state variables
-    boost::mutex mutex_;
-    bool initialized_;
-    ros::Time last_time_;
-
-    // **** filter implementation
-    ImuFilter filter_;
-
-    // **** member functions
-    void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
-                        const MagMsg::ConstPtr& mav_msg);
-
-    void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw);
-
-    void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw);
-    void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw);
-
-    void publishRawMsg(const ros::Time& t,
-                       float roll, float pitch, float yaw);
-
-    void reconfigCallback(FilterConfig& config, uint32_t level);
-    void checkTopicsTimerCallback(const ros::TimerEvent&);
-};
-
-#endif // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/stateless_orientation.h

@@ -1,48 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H
-#define IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H
-
-#include <geometry_msgs/Vector3.h>
-#include <geometry_msgs/Quaternion.h>
-#include <imu_filter_madgwick/world_frame.h>
-
-class StatelessOrientation
-{
-public:
-  static bool computeOrientation(
-    WorldFrame::WorldFrame frame,
-    geometry_msgs::Vector3 acceleration,
-    geometry_msgs::Vector3 magneticField,
-    geometry_msgs::Quaternion& orientation);
-
-  static bool computeOrientation(
-    WorldFrame::WorldFrame frame,
-    geometry_msgs::Vector3 acceleration,
-    geometry_msgs::Quaternion& orientation);
-
-};
-
-#endif // IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H

imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/world_frame.h

@@ -1,8 +0,0 @@
-#ifndef IMU_FILTER_MADGWICK_WORLD_FRAME_H_
-#define IMU_FILTER_MADGWICK_WORLD_FRAME_H_
-
-namespace WorldFrame {
-  enum WorldFrame { ENU, NED, NWU };
-}
-
-#endif

imu_tools/imu_filter_madgwick/launch/imu_filter_madgwick.launch

@@ -1,17 +0,0 @@
-<!-- Node for merging magnetometer and accelerometer data into a single imu message -->
-<launch>
-  #### Nodelet manager ######################################################
-
-  <node pkg="nodelet" type="nodelet" name="imu_manager" 
-    args="manager" output="screen" />
-
-  #### IMU Driver ###########################################################
-
-  <node pkg="nodelet" type="nodelet" name="ImuFilterNodelet" 
-    args="load imu_filter_madgwick/ImuFilterNodelet imu_manager" 
-    output="screen">
-    
-    <param name="publish_tf" value="false"/>
-
-  </node>
-</launch>

imu_tools/imu_filter_madgwick/package.xml

@@ -1,44 +0,0 @@
-<package>
-  <name>imu_filter_madgwick</name>
-  <version>1.2.3</version>
-  <description>  
-  Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into an orientation. Based on code by Sebastian Madgwick, http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms.
-  </description>
-  <license>GPL</license>
-  <url>http://ros.org/wiki/imu_filter_madgwick</url>
-  <author email="ivan.dryanovski@gmail.com">Ivan Dryanovski</author>
-  <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>tf2</build_depend>
-  <build_depend>tf2_geometry_msgs</build_depend>
-  <build_depend>tf2_ros</build_depend>
-  <build_depend>nodelet</build_depend>
-  <build_depend>pluginlib</build_depend>
-  <build_depend>message_filters</build_depend>
-  <build_depend>dynamic_reconfigure</build_depend>
-
-  <run_depend>roscpp</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>tf2</run_depend>
-  <run_depend>tf2_geometry_msgs</run_depend>
-  <run_depend>tf2_ros</run_depend>
-  <run_depend>nodelet</run_depend>
-  <run_depend>pluginlib</run_depend>
-  <run_depend>message_filters</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-
-  <test_depend>rosunit</test_depend>
-
-  <export>
-    <nodelet plugin="${prefix}/imu_filter_nodelet.xml" />
-  </export>
-
-</package>
-
-

imu_tools/imu_filter_madgwick/src/imu_filter.cpp

@@ -1,338 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <cmath>
-#include "imu_filter_madgwick/imu_filter.h"
-
-// Fast inverse square-root
-// See: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Reciprocal_of_the_square_root
-static float invSqrt(float x)
-{
-  float xhalf = 0.5f * x;
-  union
-  {
-    float x;
-    int i;
-  } u;
-  u.x = x;
-  u.i = 0x5f3759df - (u.i >> 1);
-  /* The next line can be repeated any number of times to increase accuracy */
-  u.x = u.x * (1.5f - xhalf * u.x * u.x);
-  return u.x;
-}
-
-template<typename T>
-static inline void normalizeVector(T& vx, T& vy, T& vz)
-{
-  T recipNorm = invSqrt (vx * vx + vy * vy + vz * vz);
-  vx *= recipNorm;
-  vy *= recipNorm;
-  vz *= recipNorm;
-}
-
-template<typename T>
-static inline void normalizeQuaternion(T& q0, T& q1, T& q2, T& q3)
-{
-  T recipNorm = invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-  q0 *= recipNorm;
-  q1 *= recipNorm;
-  q2 *= recipNorm;
-  q3 *= recipNorm;
-}
-
-static inline void rotateAndScaleVector(
-    float q0, float q1, float q2, float q3,
-    float _2dx, float _2dy, float _2dz,
-    float& rx, float& ry, float& rz) {
-
-  // result is half as long as input
-  rx = _2dx * (0.5f - q2 * q2 - q3 * q3)
-     + _2dy * (q0 * q3 + q1 * q2)
-     + _2dz * (q1 * q3 - q0 * q2);
-  ry = _2dx * (q1 * q2 - q0 * q3)
-     + _2dy * (0.5f - q1 * q1 - q3 * q3)
-     + _2dz * (q0 * q1 + q2 * q3);
-  rz = _2dx * (q0 * q2 + q1 * q3)
-     + _2dy * (q2 * q3 - q0 * q1)
-     + _2dz * (0.5f - q1 * q1 - q2 * q2);
-}
-
-
-static inline void compensateGyroDrift(
-    float q0, float q1, float q2, float q3,
-    float s0, float s1, float s2, float s3,
-    float dt, float zeta,
-    float& w_bx, float& w_by, float& w_bz,
-    float& gx, float& gy, float& gz)
-{
-  // w_err = 2 q x s
-  float w_err_x = 2.0f * q0 * s1 - 2.0f * q1 * s0 - 2.0f * q2 * s3 + 2.0f * q3 * s2;
-  float w_err_y = 2.0f * q0 * s2 + 2.0f * q1 * s3 - 2.0f * q2 * s0 - 2.0f * q3 * s1;
-  float w_err_z = 2.0f * q0 * s3 - 2.0f * q1 * s2 + 2.0f * q2 * s1 - 2.0f * q3 * s0;
-
-  w_bx += w_err_x * dt * zeta;
-  w_by += w_err_y * dt * zeta;
-  w_bz += w_err_z * dt * zeta;
-
-  gx -= w_bx;
-  gy -= w_by;
-  gz -= w_bz;
-}
-
-static inline void orientationChangeFromGyro(
-    float q0, float q1, float q2, float q3,
-    float gx, float gy, float gz,
-    float& qDot1, float& qDot2, float& qDot3, float& qDot4)
-{
-  // Rate of change of quaternion from gyroscope
-  // See EQ 12
-  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
-  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
-  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
-  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
-}
-
-static inline void addGradientDescentStep(
-    float q0, float q1, float q2, float q3,
-    float _2dx, float _2dy, float _2dz,
-    float mx, float my, float mz,
-    float& s0, float& s1, float& s2, float& s3)
-{
-  float f0, f1, f2;
-
-  // Gradient decent algorithm corrective step
-  // EQ 15, 21
-  rotateAndScaleVector(q0,q1,q2,q3, _2dx, _2dy, _2dz, f0, f1, f2);
-
-  f0 -= mx;
-  f1 -= my;
-  f2 -= mz;
-
-
-  // EQ 22, 34
-  // Jt * f
-  s0 += (_2dy * q3 - _2dz * q2) * f0
-      + (-_2dx * q3 + _2dz * q1) * f1
-      + (_2dx * q2 - _2dy * q1) * f2;
-  s1 += (_2dy * q2 + _2dz * q3) * f0
-      + (_2dx * q2 - 2.0f * _2dy * q1 + _2dz * q0) * f1
-      + (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2;
-  s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0
-      + (_2dx * q1 + _2dz * q3) * f1
-      + (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2;
-  s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0
-      + (-_2dx * q0 - 2.0f * _2dy * q3 + _2dz * q2) * f1
-      + (_2dx * q1 + _2dy * q2) * f2;
-}
-
-static inline void compensateMagneticDistortion(
-    float q0, float q1, float q2, float q3,
-    float mx, float my, float mz,
-    float& _2bxy, float& _2bz)
-{
-  float hx, hy, hz;
-  // Reference direction of Earth's magnetic field (See EQ 46)
-  rotateAndScaleVector(q0, -q1, -q2, -q3, mx, my, mz, hx, hy, hz);
-
-  _2bxy = 4.0f * sqrt (hx * hx + hy * hy);
-  _2bz = 4.0f * hz;
-
-}
-
-
-ImuFilter::ImuFilter() :
-    gain_ (0.0), zeta_ (0.0), world_frame_(WorldFrame::ENU),
-    q0(1.0), q1(0.0), q2(0.0), q3(0.0),
-    w_bx_(0.0), w_by_(0.0), w_bz_(0.0)
-{
-}
-
-ImuFilter::~ImuFilter()
-{
-}
-
-void ImuFilter::madgwickAHRSupdate(
-    float gx, float gy, float gz,
-    float ax, float ay, float az,
-    float mx, float my, float mz,
-    float dt)
-{
-  float s0, s1, s2, s3;
-  float qDot1, qDot2, qDot3, qDot4;
-  float _2bz, _2bxy;
-
-  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
-  if (!std::isfinite(mx) || !std::isfinite(my) || !std::isfinite(mz))
-  {
-    madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);
-    return;
-  }
-
-  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
-  {
-    // Normalise accelerometer measurement
-    normalizeVector(ax, ay, az);
-
-    // Normalise magnetometer measurement
-    normalizeVector(mx, my, mz);
-
-    // Compensate for magnetic distortion
-    compensateMagneticDistortion(q0, q1, q2, q3, mx, my, mz, _2bxy, _2bz);
-
-    // Gradient decent algorithm corrective step
-    s0 = 0.0;  s1 = 0.0;  s2 = 0.0;  s3 = 0.0;
-    switch (world_frame_) {
-      case WorldFrame::NED:
-        // Gravity: [0, 0, -1]
-        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
-
-        // Earth magnetic field: = [bxy, 0, bz]
-        addGradientDescentStep(q0,q1,q2,q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
-        break;
-      case WorldFrame::NWU:
-        // Gravity: [0, 0, 1]
-        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
-
-        // Earth magnetic field: = [bxy, 0, bz]
-        addGradientDescentStep(q0,q1,q2,q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
-        break;
-      default:
-      case WorldFrame::ENU:
-        // Gravity: [0, 0, 1]
-        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
-
-        // Earth magnetic field: = [0, bxy, bz]
-        addGradientDescentStep(q0, q1, q2, q3, 0.0, _2bxy, _2bz, mx, my, mz, s0, s1, s2, s3);
-        break;
-    }
-    normalizeQuaternion(s0, s1, s2, s3);
-
-    // compute gyro drift bias
-    compensateGyroDrift(q0, q1, q2, q3, s0, s1, s2, s3, dt, zeta_, w_bx_, w_by_, w_bz_, gx, gy, gz);
-
-    orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
-
-    // Apply feedback step
-    qDot1 -= gain_ * s0;
-    qDot2 -= gain_ * s1;
-    qDot3 -= gain_ * s2;
-    qDot4 -= gain_ * s3;
-  }
-  else
-  {
-    orientationChangeFromGyro(q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
-  }
-
-  // Integrate rate of change of quaternion to yield quaternion
-  q0 += qDot1 * dt;
-  q1 += qDot2 * dt;
-  q2 += qDot3 * dt;
-  q3 += qDot4 * dt;
-
-  // Normalise quaternion
-  normalizeQuaternion(q0, q1, q2, q3);
-}
-
-void ImuFilter::madgwickAHRSupdateIMU(
-    float gx, float gy, float gz,
-    float ax, float ay, float az,
-    float dt)
-{
-  float s0, s1, s2, s3;
-  float qDot1, qDot2, qDot3, qDot4;
-
-  // Rate of change of quaternion from gyroscope
-  orientationChangeFromGyro (q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
-
-  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
-  {
-    // Normalise accelerometer measurement
-    normalizeVector(ax, ay, az);
-
-    // Gradient decent algorithm corrective step
-    s0 = 0.0;  s1 = 0.0;  s2 = 0.0;  s3 = 0.0;
-    switch (world_frame_) {
-      case WorldFrame::NED:
-        // Gravity: [0, 0, -1]
-        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
-        break;
-      case WorldFrame::NWU:
-        // Gravity: [0, 0, 1]
-        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
-        break;
-      default:
-      case WorldFrame::ENU:
-        // Gravity: [0, 0, 1]
-        addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
-        break;
-    }
-
-    normalizeQuaternion(s0, s1, s2, s3);
-
-    // Apply feedback step
-    qDot1 -= gain_ * s0;
-    qDot2 -= gain_ * s1;
-    qDot3 -= gain_ * s2;
-    qDot4 -= gain_ * s3;
-  }
-
-  // Integrate rate of change of quaternion to yield quaternion
-  q0 += qDot1 * dt;
-  q1 += qDot2 * dt;
-  q2 += qDot3 * dt;
-  q3 += qDot4 * dt;
-
-  // Normalise quaternion
-  normalizeQuaternion (q0, q1, q2, q3);
-}
-
-
-void ImuFilter::getGravity(float& rx, float& ry, float& rz,
-    float gravity)
-{
-    // Estimate gravity vector from current orientation
-    switch (world_frame_) {
-      case WorldFrame::NED:
-        // Gravity: [0, 0, -1]
-        rotateAndScaleVector(q0, q1, q2, q3,
-            0.0, 0.0, -2.0*gravity,
-            rx, ry, rz);
-        break;
-      case WorldFrame::NWU:
-        // Gravity: [0, 0, 1]
-        rotateAndScaleVector(q0, q1, q2, q3,
-            0.0, 0.0, 2.0*gravity,
-            rx, ry, rz);
-        break;
-      default:
-      case WorldFrame::ENU:
-        // Gravity: [0, 0, 1]
-        rotateAndScaleVector(q0, q1, q2, q3,
-            0.0, 0.0, 2.0*gravity,
-            rx, ry, rz);
-        break;
-    }
-}

imu_tools/imu_filter_madgwick/src/imu_filter_node.cpp

@@ -1,35 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include "imu_filter_madgwick/imu_filter_ros.h"
-
-int main (int argc, char **argv)
-{
-  ros::init (argc, argv, "ImuFilter");
-  ros::NodeHandle nh;
-  ros::NodeHandle nh_private("~");
-  ImuFilterRos imu_filter(nh, nh_private);
-  ros::spin();
-  return 0;
-}

imu_tools/imu_filter_madgwick/src/imu_filter_nodelet.cpp

@@ -1,39 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include "imu_filter_madgwick/imu_filter_nodelet.h"
-#include <pluginlib/class_list_macros.h>
-
-void ImuFilterNodelet::onInit()
-{
-  NODELET_INFO("Initializing IMU Filter Nodelet");
-  
-  // TODO: Do we want the single threaded or multithreaded NH?
-  ros::NodeHandle nh         = getMTNodeHandle();
-  ros::NodeHandle nh_private = getMTPrivateNodeHandle();
-
-  filter_.reset(new ImuFilterRos(nh, nh_private));
-}
-
-PLUGINLIB_EXPORT_CLASS(ImuFilterNodelet, nodelet::Nodelet)

imu_tools/imu_filter_madgwick/src/imu_filter_ros.cpp

@@ -1,393 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include "imu_filter_madgwick/imu_filter_ros.h"
-#include "imu_filter_madgwick/stateless_orientation.h"
-#include "geometry_msgs/TransformStamped.h"
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2/LinearMath/Matrix3x3.h>
-
-ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private):
-  nh_(nh),
-  nh_private_(nh_private),
-  initialized_(false)
-{
-  ROS_INFO ("Starting ImuFilter");
-
-  // **** get paramters
-  if (!nh_private_.getParam ("stateless", stateless_))
-    stateless_ = false;
-  if (!nh_private_.getParam ("use_mag", use_mag_))
-   use_mag_ = true;
-  if (!nh_private_.getParam ("publish_tf", publish_tf_))
-   publish_tf_ = true;
-  if (!nh_private_.getParam ("reverse_tf", reverse_tf_))
-   reverse_tf_ = false;
-  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
-   fixed_frame_ = "odom";
-  if (!nh_private_.getParam ("constant_dt", constant_dt_))
-    constant_dt_ = 0.0;
-  if (!nh_private_.getParam ("remove_gravity_vector", remove_gravity_vector_))
-    remove_gravity_vector_= false;
-  if (!nh_private_.getParam ("publish_debug_topics", publish_debug_topics_))
-    publish_debug_topics_= false;
-
-  std::string world_frame;
-  if (!nh_private_.getParam ("world_frame", world_frame))
-    world_frame = "enu";
-
-  if (world_frame == "ned") {
-    world_frame_ = WorldFrame::NED;
-  } else if (world_frame == "nwu"){
-    world_frame_ = WorldFrame::NWU;
-  } else if (world_frame == "enu"){
-    world_frame_ = WorldFrame::ENU;
-  } else {
-    ROS_ERROR("The parameter world_frame was set to invalid value '%s'.", world_frame.c_str());
-    ROS_ERROR("Valid values are 'enu', 'ned' and 'nwu'. Setting to 'enu'.");
-    world_frame_ = WorldFrame::ENU;
-  }
-  filter_.setWorldFrame(world_frame_);
-
-  // check for illegal constant_dt values
-  if (constant_dt_ < 0.0)
-  {
-    ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
-    constant_dt_ = 0.0;
-  }
-
-  // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt
-  // otherwise, it will be constant
-  if (constant_dt_ == 0.0)
-    ROS_INFO("Using dt computed from message headers");
-  else
-    ROS_INFO("Using constant dt of %f sec", constant_dt_);
-
-  if (remove_gravity_vector_)
-    ROS_INFO("The gravity vector will be removed from the acceleration");
-  else
-    ROS_INFO("The gravity vector is kept in the IMU message.");
-
-  // **** register dynamic reconfigure
-  config_server_.reset(new FilterConfigServer(nh_private_));
-  FilterConfigServer::CallbackType f = boost::bind(&ImuFilterRos::reconfigCallback, this, _1, _2);
-  config_server_->setCallback(f);
-
-  // **** register publishers
-  imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
-    ros::names::resolve("imu") + "/data", 5);
-
-  if (publish_debug_topics_)
-  {
-    rpy_filtered_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
-      ros::names::resolve("imu") + "/rpy/filtered", 5);
-
-    rpy_raw_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
-      ros::names::resolve("imu") + "/rpy/raw", 5);
-  }
-
-  // **** register subscribers
-  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
-  int queue_size = 5;
-
-  imu_subscriber_.reset(new ImuSubscriber(
-    nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
-
-  if (use_mag_)
-  {
-    mag_subscriber_.reset(new MagSubscriber(
-      nh_, ros::names::resolve("imu") + "/mag", queue_size));
-
-    sync_.reset(new Synchronizer(
-      SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
-    sync_->registerCallback(boost::bind(&ImuFilterRos::imuMagCallback, this, _1, _2));
-  }
-  else
-  {
-    imu_subscriber_->registerCallback(&ImuFilterRos::imuCallback, this);
-  }
-
-  check_topics_timer_ = nh_.createTimer(ros::Duration(10.0), &ImuFilterRos::checkTopicsTimerCallback, this);
-}
-
-ImuFilterRos::~ImuFilterRos()
-{
-  ROS_INFO ("Destroying ImuFilter");
-
-  // Explicitly stop callbacks; they could execute after we're destroyed
-  check_topics_timer_.stop();
-}
-
-void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
-{
-  boost::mutex::scoped_lock lock(mutex_);
-
-  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
-  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
-
-  ros::Time time = imu_msg_raw->header.stamp;
-  imu_frame_ = imu_msg_raw->header.frame_id;
-
-  if (!initialized_ || stateless_)
-  {
-    geometry_msgs::Quaternion init_q;
-    if (!StatelessOrientation::computeOrientation(world_frame_, lin_acc, init_q))
-    {
-      ROS_WARN_THROTTLE(5.0, "The IMU seems to be in free fall, cannot determine gravity direction!");
-      return;
-    }
-    filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
-  }
-
-  if (!initialized_)
-  {
-    ROS_INFO("First IMU message received.");
-    check_topics_timer_.stop();
-
-    // initialize time
-    last_time_ = time;
-    initialized_ = true;
-  }
-
-  // determine dt: either constant, or from IMU timestamp
-  float dt;
-  if (constant_dt_ > 0.0)
-    dt = constant_dt_;
-  else
-  {
-    dt = (time - last_time_).toSec();
-    if (time.isZero())
-      ROS_WARN_STREAM_THROTTLE(5.0, "The IMU message time stamp is zero, and the parameter constant_dt is not set!" <<
-                                    " The filter will not update the orientation.");
-  }
-
-  last_time_ = time;
-
-  if (!stateless_)
-    filter_.madgwickAHRSupdateIMU(
-      ang_vel.x, ang_vel.y, ang_vel.z,
-      lin_acc.x, lin_acc.y, lin_acc.z,
-      dt);
-
-  publishFilteredMsg(imu_msg_raw);
-  if (publish_tf_)
-    publishTransform(imu_msg_raw);
-}
-
-void ImuFilterRos::imuMagCallback(
-  const ImuMsg::ConstPtr& imu_msg_raw,
-  const MagMsg::ConstPtr& mag_msg)
-{
-  boost::mutex::scoped_lock lock(mutex_);
-
-  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
-  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
-  const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;
-
-  ros::Time time = imu_msg_raw->header.stamp;
-  imu_frame_ = imu_msg_raw->header.frame_id;
-
-  /*** Compensate for hard iron ***/
-  geometry_msgs::Vector3 mag_compensated;
-  mag_compensated.x = mag_fld.x - mag_bias_.x;
-  mag_compensated.y = mag_fld.y - mag_bias_.y;
-  mag_compensated.z = mag_fld.z - mag_bias_.z;
-
-  double roll = 0.0;
-  double pitch = 0.0;
-  double yaw = 0.0;
-
-  if (!initialized_ || stateless_)
-  {
-    // wait for mag message without NaN / inf
-    if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
-    {
-      return;
-    }
-
-    geometry_msgs::Quaternion init_q;
-    if (!StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, init_q))
-    {
-      ROS_WARN_THROTTLE(5.0, "The IMU seems to be in free fall or close to magnetic north pole, cannot determine gravity direction!");
-      return;
-    }
-    filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
-  }
-
-  if (!initialized_)
-  {
-    ROS_INFO("First pair of IMU and magnetometer messages received.");
-    check_topics_timer_.stop();
-
-    // initialize time
-    last_time_ = time;
-    initialized_ = true;
-  }
-
-  // determine dt: either constant, or from IMU timestamp
-  float dt;
-  if (constant_dt_ > 0.0)
-    dt = constant_dt_;
-  else
-  {
-    dt = (time - last_time_).toSec();
-    if (time.isZero())
-      ROS_WARN_STREAM_THROTTLE(5.0, "The IMU message time stamp is zero, and the parameter constant_dt is not set!" <<
-                                    " The filter will not update the orientation.");
-  }
-
-  last_time_ = time;
-
-  if (!stateless_)
-    filter_.madgwickAHRSupdate(
-      ang_vel.x, ang_vel.y, ang_vel.z,
-      lin_acc.x, lin_acc.y, lin_acc.z,
-      mag_compensated.x, mag_compensated.y, mag_compensated.z,
-      dt);
-
-  publishFilteredMsg(imu_msg_raw);
-  if (publish_tf_)
-    publishTransform(imu_msg_raw);
-
-  if(publish_debug_topics_)
-  {
-    geometry_msgs::Quaternion orientation;
-    if (StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, orientation))
-    {
-      tf2::Matrix3x3(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)).getRPY(roll, pitch, yaw, 0);
-      publishRawMsg(time, roll, pitch, yaw);
-    }
-  }
-}
-
-void ImuFilterRos::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
-{
-  double q0,q1,q2,q3;
-  filter_.getOrientation(q0,q1,q2,q3);
-  geometry_msgs::TransformStamped transform;
-  transform.header.stamp = imu_msg_raw->header.stamp;
-  if (reverse_tf_)
-  {
-    transform.header.frame_id = imu_frame_;
-    transform.child_frame_id = fixed_frame_;
-    transform.transform.rotation.w = q0;
-    transform.transform.rotation.x = -q1;
-    transform.transform.rotation.y = -q2;
-    transform.transform.rotation.z = -q3;
-  }
-  else {
-    transform.header.frame_id = fixed_frame_;
-    transform.child_frame_id = imu_frame_;
-    transform.transform.rotation.w = q0;
-    transform.transform.rotation.x = q1;
-    transform.transform.rotation.y = q2;
-    transform.transform.rotation.z = q3;
-  }
-  tf_broadcaster_.sendTransform(transform);
-
-}
-
-void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
-{
-  double q0,q1,q2,q3;
-  filter_.getOrientation(q0,q1,q2,q3);
-
-  // create and publish filtered IMU message
-  boost::shared_ptr<ImuMsg> imu_msg =
-    boost::make_shared<ImuMsg>(*imu_msg_raw);
-
-  imu_msg->orientation.w = q0;
-  imu_msg->orientation.x = q1;
-  imu_msg->orientation.y = q2;
-  imu_msg->orientation.z = q3;
-
-  imu_msg->orientation_covariance[0] = orientation_variance_;
-  imu_msg->orientation_covariance[1] = 0.0;
-  imu_msg->orientation_covariance[2] = 0.0;
-  imu_msg->orientation_covariance[3] = 0.0;
-  imu_msg->orientation_covariance[4] = orientation_variance_;
-  imu_msg->orientation_covariance[5] = 0.0;
-  imu_msg->orientation_covariance[6] = 0.0;
-  imu_msg->orientation_covariance[7] = 0.0;
-  imu_msg->orientation_covariance[8] = orientation_variance_;
-
-
-  if(remove_gravity_vector_) {
-    float gx, gy, gz;
-    filter_.getGravity(gx, gy, gz);
-    imu_msg->linear_acceleration.x -= gx;
-    imu_msg->linear_acceleration.y -= gy;
-    imu_msg->linear_acceleration.z -= gz;
-  }
-
-  imu_publisher_.publish(imu_msg);
-
-  if(publish_debug_topics_)
-  {
-    geometry_msgs::Vector3Stamped rpy;
-    tf2::Matrix3x3(tf2::Quaternion(q1,q2,q3,q0)).getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
-
-    rpy.header = imu_msg_raw->header;
-    rpy_filtered_debug_publisher_.publish(rpy);
-  }
-}
-
-void ImuFilterRos::publishRawMsg(const ros::Time& t,
-  float roll, float pitch, float yaw)
-{
-  geometry_msgs::Vector3Stamped rpy;
-  rpy.vector.x = roll;
-  rpy.vector.y = pitch;
-  rpy.vector.z = yaw ;
-  rpy.header.stamp = t;
-  rpy.header.frame_id = imu_frame_;
-  rpy_raw_debug_publisher_.publish(rpy);
-}
-
-
-void ImuFilterRos::reconfigCallback(FilterConfig& config, uint32_t level)
-{
-  double gain, zeta;
-  boost::mutex::scoped_lock lock(mutex_);
-  gain = config.gain;
-  zeta = config.zeta;
-  filter_.setAlgorithmGain(gain);
-  filter_.setDriftBiasGain(zeta);
-  ROS_INFO("Imu filter gain set to %f", gain);
-  ROS_INFO("Gyro drift bias set to %f", zeta);
-  mag_bias_.x = config.mag_bias_x;
-  mag_bias_.y = config.mag_bias_y;
-  mag_bias_.z = config.mag_bias_z;
-  orientation_variance_ = config.orientation_stddev * config.orientation_stddev;
-  ROS_INFO("Magnetometer bias values: %f %f %f", mag_bias_.x, mag_bias_.y, mag_bias_.z);
-}
-
-void ImuFilterRos::checkTopicsTimerCallback(const ros::TimerEvent&)
-{
-  if (use_mag_)
-    ROS_WARN_STREAM("Still waiting for data on topics " << ros::names::resolve("imu") << "/data_raw"
-                    << " and " << ros::names::resolve("imu") << "/mag" << "...");
-  else
-    ROS_WARN_STREAM("Still waiting for data on topic " << ros::names::resolve("imu") << "/data_raw" << "...");
-}

imu_tools/imu_filter_madgwick/src/stateless_orientation.cpp

@@ -1,172 +0,0 @@
-/*
- *  Copyright (C) 2010, CCNY Robotics Lab
- *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
- *
- *  http://robotics.ccny.cuny.edu
- *
- *  Based on implementation of Madgwick's IMU and AHRS algorithms.
- *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
- *
- *
- *  This program is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include "imu_filter_madgwick/stateless_orientation.h"
-#include <tf2/LinearMath/Matrix3x3.h>
-#include <tf2/convert.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-
-template<typename T>
-static inline void crossProduct(
-      T ax, T ay, T az,
-      T bx, T by, T bz,
-      T& rx, T& ry, T& rz) {
-  rx = ay*bz - az*by;
-  ry = az*bx - ax*bz;
-  rz = ax*by - ay*bx;
-}
-
-
-template<typename T>
-static inline T normalizeVector(T& vx, T& vy, T& vz) {
-  T norm = sqrt(vx*vx + vy*vy + vz*vz);
-  T inv = 1.0 / norm;
-  vx *= inv;
-  vy *= inv;
-  vz *= inv;
-  return norm;
-
-}
-
-
-bool StatelessOrientation::computeOrientation(
-  WorldFrame::WorldFrame frame,
-  geometry_msgs::Vector3 A,
-  geometry_msgs::Vector3 E,
-  geometry_msgs::Quaternion& orientation) {
-
-  float Hx, Hy, Hz;
-  float Mx, My, Mz;
-  float normH;
-
-  // A: pointing up
-  float Ax = A.x, Ay = A.y, Az = A.z;
-
-  // E: pointing down/north
-  float Ex = E.x, Ey = E.y, Ez = E.z;
-
-  // H: vector horizontal, pointing east
-  // H = E x A
-  crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz);
-
-  // normalize H
-  normH = normalizeVector(Hx, Hy, Hz);
-  if (normH < 1E-7) {
-    // device is close to free fall (or in space?), or close to
-    // magnetic north pole.
-    // mag in T => Threshold 1E-7, typical values are  > 1E-5.
-    return false;
-  }
-
-  // normalize A
-  normalizeVector(Ax, Ay, Az);
-
-  // M: vector horizontal, pointing north
-  // M = A x H
-  crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz);
-
-  // Create matrix for basis transformation
-  tf2::Matrix3x3 R;
-  switch (frame) {
-    case WorldFrame::NED:
-      // vector space world W:
-      // Basis: bwx (1,0,0) north, bwy (0,1,0) east, bwz (0,0,1) down
-      // vector space local L:
-      // Basis: M ,H, -A
-      // W(1,0,0) => L(M)
-      // W(0,1,0) => L(H)
-      // W(0,0,1) => L(-A)
-
-      // R: Transform Matrix local => world equals basis of L, because basis of W is I
-      R[0][0] = Mx;     R[0][1] = Hx;     R[0][2] = -Ax;
-      R[1][0] = My;     R[1][1] = Hy;     R[1][2] = -Ay;
-      R[2][0] = Mz;     R[2][1] = Hz;     R[2][2] = -Az;
-      break;
-
-    case WorldFrame::NWU:
-      // vector space world W:
-      // Basis: bwx (1,0,0) north, bwy (0,1,0) west, bwz (0,0,1) up
-      // vector space local L:
-      // Basis: M ,H, -A
-      // W(1,0,0) => L(M)
-      // W(0,1,0) => L(-H)
-      // W(0,0,1) => L(A)
-
-      // R: Transform Matrix local => world equals basis of L, because basis of W is I
-      R[0][0] = Mx;     R[0][1] = -Hx;     R[0][2] = Ax;
-      R[1][0] = My;     R[1][1] = -Hy;     R[1][2] = Ay;
-      R[2][0] = Mz;     R[2][1] = -Hz;     R[2][2] = Az;
-      break;
-
-    default:
-    case WorldFrame::ENU:
-      // vector space world W:
-      // Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up
-      // vector space local L:
-      // Basis: H, M , A
-      // W(1,0,0) => L(H)
-      // W(0,1,0) => L(M)
-      // W(0,0,1) => L(A)
-
-      // R: Transform Matrix local => world equals basis of L, because basis of W is I
-      R[0][0] = Hx;     R[0][1] = Mx;     R[0][2] = Ax;
-      R[1][0] = Hy;     R[1][1] = My;     R[1][2] = Ay;
-      R[2][0] = Hz;     R[2][1] = Mz;     R[2][2] = Az;
-      break;
-  }
-
-  // Matrix.getRotation assumes vector rotation, but we're using
-  // coordinate systems. Thus negate rotation angle (inverse).
-  tf2::Quaternion q;
-  R.getRotation(q);
-  tf2::convert(q.inverse(), orientation);
-  return true;
-}
-
-
-bool StatelessOrientation::computeOrientation(
-  WorldFrame::WorldFrame frame,
-  geometry_msgs::Vector3 A,
-  geometry_msgs::Quaternion& orientation) {
-
-  // This implementation could be optimized regarding speed.
-
-  // magnetic Field E must not be parallel to A,
-  // choose an arbitrary orthogonal vector
-  geometry_msgs::Vector3 E;
-  if (fabs(A.x) > 0.1 || fabs(A.y) > 0.1) {
-      E.x = A.y;
-      E.y = A.x;
-      E.z = 0.0;
-  } else if (fabs(A.z) > 0.1) {
-      E.x = 0.0;
-      E.y = A.z;
-      E.z = A.y;
-  } else {
-      // free fall
-      return false;
-  }
-
-  return computeOrientation(frame, A, E, orientation);
-}

imu_tools/imu_filter_madgwick/test/madgwick_test.cpp

@@ -1,121 +0,0 @@
-#include <imu_filter_madgwick/imu_filter.h>
-#include <cmath>
-#include "test_helpers.h"
-
-#define FILTER_ITERATIONS 10000
-
-
-template <WorldFrame::WorldFrame FRAME>
-void filterStationary(
-    float Ax, float Ay, float Az,
-    float Mx, float My, float Mz,
-    double& q0, double& q1, double& q2, double& q3) {
-  float dt = 0.1;
-  float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0)
-
-  ImuFilter filter;
-  filter.setDriftBiasGain(0.0);
-  filter.setAlgorithmGain(0.1);
-
-  // initialize with some orientation
-  filter.setOrientation(q0,q1,q2,q3);
-  filter.setWorldFrame(FRAME);
-
-  for (int i = 0; i < FILTER_ITERATIONS; i++) {
-      filter.madgwickAHRSupdate(Gx, Gy, Gz, Ax, Ay, Az, Mx, My, Mz, dt);
-  }
-
-  filter.getOrientation(q0,q1,q2,q3);
-}
-
-
-template <WorldFrame::WorldFrame FRAME>
-void filterStationary(
-    float Ax, float Ay, float Az,
-    double& q0, double& q1, double& q2, double& q3) {
-  float dt = 0.1;
-  float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0)
-
-  ImuFilter filter;
-  filter.setDriftBiasGain(0.0);
-  filter.setAlgorithmGain(0.1);
-
-  // initialize with some orientation
-  filter.setOrientation(q0,q1,q2,q3);
-  filter.setWorldFrame(FRAME);
-
-  for (int i = 0; i < FILTER_ITERATIONS; i++) {
-      filter.madgwickAHRSupdateIMU(Gx, Gy, Gz, Ax, Ay, Az, dt);
-  }
-
-  filter.getOrientation(q0,q1,q2,q3);
-}
-
-
-#define TEST_STATIONARY_ENU(in_am, exp_result)       \
-  TEST(MadgwickTest, Stationary_ENU_ ## in_am){      \
-  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
-  filterStationary<WorldFrame::ENU>(in_am, q0, q1, q2, q3);  \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
-  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }   \
-  TEST(MadgwickTest, Stationary_ENU_NM_ ## in_am){   \
-  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
-  filterStationary<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3);  \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
-  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
-
-#define TEST_STATIONARY_NED(in_am, exp_result)       \
-  TEST(MadgwickTest, Stationary_NED_ ## in_am){      \
-  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
-  filterStationary<WorldFrame::NED>(in_am, q0, q1, q2, q3);  \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
-  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }   \
-  TEST(MadgwickTest, Stationary_NED_NM_ ## in_am){   \
-  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
-  filterStationary<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3);  \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
-  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
-
-#define TEST_STATIONARY_NWU(in_am, exp_result)       \
-  TEST(MadgwickTest, Stationary_NWU_ ## in_am){      \
-  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
-  filterStationary<WorldFrame::NWU>(in_am, q0, q1, q2, q3);  \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
-  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }   \
-  TEST(MadgwickTest, Stationary_NWU_NM_ ## in_am){   \
-  double q0 = .5, q1 = .5, q2 = .5, q3 = .5;         \
-  filterStationary<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3);  \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);              \
-  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
-
-TEST_STATIONARY_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180)
-TEST_STATIONARY_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY)
-TEST_STATIONARY_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU)
-TEST_STATIONARY_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU)
-
-TEST_STATIONARY_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY)
-TEST_STATIONARY_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120)
-TEST_STATIONARY_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90)
-TEST_STATIONARY_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU)
-TEST_STATIONARY_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU)
-
-TEST_STATIONARY_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY)
-TEST_STATIONARY_NED(AM_NORTH_WEST_UP, QUAT_X_180)
-TEST_STATIONARY_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED)
-TEST_STATIONARY_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED)
-
-
-
-TEST(MadgwickTest, TestQuatEqNoZ) {
-
-  ASSERT_TRUE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_MZ_90));
-  ASSERT_FALSE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_X_180));
-
-}
-
-
-
-int main(int argc, char **argv){
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}

imu_tools/imu_filter_madgwick/test/stateless_orientation_test.cpp

@@ -1,117 +0,0 @@
-#include <imu_filter_madgwick/stateless_orientation.h>
-#include "test_helpers.h"
-
-
-template<WorldFrame::WorldFrame FRAME>
-bool computeOrientation(
-    float Ax, float Ay, float Az,
-    float Mx, float My, float Mz,
-    double& q0, double& q1, double& q2, double& q3) {
-
-  geometry_msgs::Vector3 A, M;
-  geometry_msgs::Quaternion orientation;
-  A.x = Ax; A.y = Ay; A.z = Az;
-  M.x = Mx; M.y = My; M.z = Mz;
-
-  bool res = StatelessOrientation::computeOrientation(FRAME, A, M, orientation);
-
-  q0 = orientation.w;
-  q1 = orientation.x;
-  q2 = orientation.y;
-  q3 = orientation.z;
-
-  return res;
-}
-
-template<WorldFrame::WorldFrame FRAME>
-bool computeOrientation(
-    float Ax, float Ay, float Az,
-    double& q0, double& q1, double& q2, double& q3) {
-
-  geometry_msgs::Vector3 A;
-  geometry_msgs::Quaternion orientation;
-  A.x = Ax; A.y = Ay; A.z = Az;
-
-  bool res = StatelessOrientation::computeOrientation(FRAME, A, orientation);
-
-  q0 = orientation.w;
-  q1 = orientation.x;
-  q2 = orientation.y;
-  q3 = orientation.z;
-
-  return res;
-}
-
-
-#define TEST_STATELESS_ENU(in_am, exp_result)       \
-  TEST(StatelessOrientationTest, Stationary_ENU_ ## in_am){      \
-  double q0, q1, q2, q3;                                         \
-  ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(in_am, q0, q1, q2, q3)); \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
-  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }               \
-  TEST(StatelessOrientationTest, Stationary_ENU_NM_ ## in_am){   \
-  double q0, q1, q2, q3;                                         \
-  ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
-  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
-
-#define TEST_STATELESS_NED(in_am, exp_result)       \
-  TEST(StatelessOrientationTest, Stationary_NED_ ## in_am){      \
-  double q0, q1, q2, q3;                                         \
-  ASSERT_TRUE(computeOrientation<WorldFrame::NED>(in_am, q0, q1, q2, q3));  \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
-  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }               \
-  TEST(StatelessOrientationTest, Stationary_NED_NM_ ## in_am){   \
-  double q0, q1, q2, q3;                                         \
-  ASSERT_TRUE(computeOrientation<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
-  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
-
-#define TEST_STATELESS_NWU(in_am, exp_result)       \
-  TEST(StatelessOrientationTest, Stationary_NWU_ ## in_am){      \
-  double q0, q1, q2, q3;                                         \
-  ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(in_am, q0, q1, q2, q3));  \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
-  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); }               \
-  TEST(StatelessOrientationTest, Stationary_NWU_NM_ ## in_am){   \
-  double q0, q1, q2, q3;                                         \
-  ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
-  ASSERT_IS_NORMALIZED(q0, q1, q2, q3);                          \
-  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
-
-TEST_STATELESS_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY)
-TEST_STATELESS_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120)
-TEST_STATELESS_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90)
-TEST_STATELESS_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU)
-TEST_STATELESS_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU)
-
-
-TEST_STATELESS_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY)
-TEST_STATELESS_NED(AM_WEST_NORTH_DOWN, QUAT_MZ_90)
-TEST_STATELESS_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED)
-TEST_STATELESS_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED)
-
-
-TEST_STATELESS_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180)
-TEST_STATELESS_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY)
-TEST_STATELESS_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU)
-TEST_STATELESS_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU)
-
-
-TEST(StatelessOrientationTest, Check_NoAccel){
-  double q0, q1, q2, q3;
-  ASSERT_FALSE(computeOrientation<WorldFrame::ENU>(
-    0.0, 0.0, 0.0,
-    0.0, 0.0005, -0.0005,
-    q0, q1, q2, q3));
-}
-
-TEST(StatelessOrientationTest, Check_NoMag){
-  double q0, q1, q2, q3;
-  ASSERT_FALSE(computeOrientation<WorldFrame::ENU>(
-    0.0, 0.0, 9.81,
-    0.0, 0.0, 0.0,
-    q0, q1, q2, q3));
-}
-
-

imu_tools/imu_filter_madgwick/test/test_helpers.h

@@ -1,102 +0,0 @@
-
-#ifndef TEST_TEST_HELPERS_H_
-#define TEST_TEST_HELPERS_H_
-
-#include <gtest/gtest.h>
-#include <tf2/LinearMath/Quaternion.h>
-
-#define MAX_DIFF 0.05
-
-template <typename T>
-static inline void normalize_quaternion(T& q0, T& q1, T& q2, T& q3) {
-  T invNorm = 1 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
-  T max = q0;
-  if (fabs(max) < fabs(q1)) max = q1;
-  if (fabs(max) < fabs(q2)) max = q2;
-  if (fabs(max) < fabs(q3)) max = q3;
-  if (max < 0) invNorm *= -1.0;
-
-  q0 *= invNorm;
-  q1 *= invNorm;
-  q2 *= invNorm;
-  q3 *= invNorm;
-}
-
-// Tests for normalization in the same way as tf2:
-// https://github.com/ros/geometry2/blob/bd490515b1434caeff521ea14901dfe04063ca27/tf2/src/buffer_core.cpp#L244-L247
-template <typename T>
-static inline bool is_normalized(T q0, T q1, T q2, T q3) {
-  return std::abs((q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) - 1.0f) < 10e-6;
-}
-
-template <typename T>
-static inline bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
-  normalize_quaternion(q0, q1, q2, q3);
-  normalize_quaternion(qr0, qr1, qr2, qr3);
-
-  return (fabs(q0 - qr0) < MAX_DIFF) &&
-         (fabs(q1 - qr1) < MAX_DIFF) &&
-         (fabs(q2 - qr2) < MAX_DIFF) &&
-         (fabs(q3 - qr3) < MAX_DIFF);
-}
-
-template <typename T>
-static inline bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
-  // assert q == qr * qz
-  tf2::Quaternion q(q1, q2, q3, q0);
-  tf2::Quaternion qr(qr1, qr2, qr3, qr0);
-  tf2::Quaternion qz = q * qr.inverse();
-
-  // remove x and y components.
-  qz.setX(0.0);
-  qz.setY(0.0);
-
-  tf2::Quaternion qr_ = qz * qr;
-
-  return quat_equal(q0, q1, q2, q3,
-      qr_.getW(),
-      qr_.getX(),
-      qr_.getY(),
-      qr_.getZ());
-}
-
-#define ASSERT_IS_NORMALIZED_(q0, q1, q2, q3) ASSERT_TRUE(is_normalized(q0, q1, q2, q3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
-#define ASSERT_IS_NORMALIZED(...) ASSERT_IS_NORMALIZED_(__VA_ARGS__)
-
-#define ASSERT_QUAT_EQUAL_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_equal(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
-#define ASSERT_QUAT_EQUAL(...) ASSERT_QUAT_EQUAL_(__VA_ARGS__)
-
-#define ASSERT_QUAT_EQUAL_EX_Z_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_eq_ex_z(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
-#define ASSERT_QUAT_EQUAL_EX_Z(...) ASSERT_QUAT_EQUAL_EX_Z_(__VA_ARGS__)
-
-// Well known states
-// scheme: AM_x_y_z
-#define AM_EAST_NORTH_UP    /* Acceleration */ 0.0, 0.0, 9.81,  /* Magnetic */ 0.0, 0.0005, -0.0005
-#define AM_NORTH_EAST_DOWN  /* Acceleration */ 0.0, 0.0, -9.81, /* Magnetic */ 0.0005, 0.0, 0.0005
-#define AM_NORTH_WEST_UP    /* Acceleration */ 0.0, 0.0, 9.81,  /* Magnetic */ 0.0005, 0.0, -0.0005
-#define AM_SOUTH_UP_WEST    /* Acceleration */ 0.0, 9.81, 0.0,  /* Magnetic */ -0.0005, -0.0005, 0.0
-#define AM_SOUTH_EAST_UP    /* Acceleration */ 0.0, 0.0, 9.81,  /* Magnetic */ -0.0005, 0.0, -0.0005
-#define AM_WEST_NORTH_DOWN  /* Acceleration */ 0.0, 0.0, -9.81, /* Magnetic */ 0.0, 0.0005, 0.0005
-
-// Real sensor data
-#define AM_WEST_NORTH_DOWN_RSD /* Acceleration */ 0.12, 0.29, -9.83, /* Magnetic */ 6.363e-06, 1.0908e-05, 4.2723e-05
-#define AM_NE_NW_UP_RSD   /* Acceleration */ 0.20, 0.55, 9.779, /* Magnetic */ 8.484e-06, 8.181e-06, -4.3329e-05
-
-// Strip accelration from am
-#define ACCEL_ONLY(ax,ay,az, mx,my,mz) ax, ay, az
-
-// Well known quaternion
-// scheme: QUAT_axis_angle
-#define QUAT_IDENTITY  1.0, 0.0, 0.0, 0.0
-#define QUAT_MZ_90 0.707107, 0.0, 0.0, -0.707107
-#define QUAT_X_180 0.0, 1.0, 0.0, 0.0
-#define QUAT_XMYMZ_120 0.5, 0.5, -0.5, -0.5
-#define QUAT_WEST_NORTH_DOWN_RSD_NWU 0.01, 0.86, 0.50, 0.012 /* axis: (0.864401, 0.502559, 0.0120614) | angle: 178.848deg */
-#define QUAT_WEST_NORTH_DOWN_RSD_ENU 0.0, -0.25, -0.97, -0.02/* Axis: (-0.2, -0.96, 0.02), Angle: 180deg */
-#define QUAT_WEST_NORTH_DOWN_RSD_NED 0.86, -0.01, 0.01, -0.50 /* Axis: -Z, Angle: 60deg */
-#define QUAT_NE_NW_UP_RSD_NWU 0.91, 0.03, -0.02, -0.41 /* axis: (0.0300376, -0.020025, -0.410513) | angle: 48.6734deg */
-#define QUAT_NE_NW_UP_RSD_ENU 0.93, 0.03, 0.0, 0.35 /* Axis: Z,  Angle: 41deg */
-#define QUAT_NE_NW_UP_RSD_NED 0.021, -0.91, -0.41, 0.02 /* Axis: (0.9, 0.4, 0.0), Angle: 180deg */
-
-
-#endif /* TEST_TEST_HELPERS_H_ */

imu_tools/imu_tools/CHANGELOG.rst

@@ -1,81 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package imu_tools
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.2.3 (2021-04-09)
-------------------
-* Fix "non standard content" warning in imu_tools metapackage
-  Fixes `#135 <https://github.com/ccny-ros-pkg/imu_tools/issues/135>`_.
-* Update maintainers in package.xml
-* Set cmake_policy CMP0048 to fix warning
-* Contributors: Martin Günther
-
-1.2.2 (2020-05-25)
-------------------
-
-1.2.1 (2019-05-06)
-------------------
-
-1.2.0 (2018-05-25)
-------------------
-
-1.1.5 (2017-05-24)
-------------------
-
-1.1.4 (2017-05-22)
-------------------
-
-1.1.3 (2017-03-10)
-------------------
-
-1.1.2 (2016-09-07)
-------------------
-
-1.1.1 (2016-09-07)
-------------------
-
-1.1.0 (2016-04-25)
-------------------
-
-1.0.11 (2016-04-22)
--------------------
-
-1.0.10 (2016-04-22)
--------------------
-
-1.0.9 (2015-10-16)
-------------------
-
-1.0.8 (2015-10-07)
-------------------
-* Add imu_complementary_filter to meta package
-* Contributors: Martin Günther
-
-1.0.7 (2015-10-07)
-------------------
-
-1.0.6 (2015-10-06)
-------------------
-
-1.0.5 (2015-06-24)
-------------------
-
-1.0.4 (2015-05-06)
-------------------
-
-1.0.3 (2015-01-29)
-------------------
-
-1.0.2 (2015-01-27)
-------------------
-
-1.0.1 (2014-12-10)
-------------------
-* add me as maintainer to package.xml
-* Contributors: Martin Günther
-
-1.0.0 (2014-09-03)
-------------------
-* First public release
-* catkinization of imu_tools metapackage
-* Contributors: Francisco Vina

imu_tools/imu_tools/CMakeLists.txt

@@ -1,4 +0,0 @@
-cmake_minimum_required(VERSION 3.5.1)
-project(imu_tools)
-find_package(catkin REQUIRED)
-catkin_metapackage()

imu_tools/imu_tools/package.xml

@@ -1,21 +0,0 @@
-<package>
-  <name> imu_tools </name>
-  <version>1.2.3</version>
-  <description>
-    Various tools for IMU devices
-  </description>
-  <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
-  <license>BSD, GPL</license>
-
-  <url>http://ros.org/wiki/imu_tools</url>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <run_depend>imu_complementary_filter</run_depend>
-  <run_depend>imu_filter_madgwick</run_depend>
-  <run_depend>rviz_imu_plugin</run_depend>
-
-  <export>
-    <metapackage/>
-  </export>
-</package>

imu_tools/rviz_imu_plugin/CHANGELOG.rst

@@ -1,100 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package rviz_imu_plugin
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.2.3 (2021-04-09)
-------------------
-* Fix "non standard content" warning in imu_tools metapackage
-  Fixes `#135 <https://github.com/ccny-ros-pkg/imu_tools/issues/135>`_.
-* Update maintainers in package.xml
-* Set cmake_policy CMP0048 to fix warning
-* Contributors: Martin Günther
-
-1.2.2 (2020-05-25)
-------------------
-* Export symbols so plugin can load
-* properly show/hide visualization when enabled/disabled
-* Contributors: CCNY Robotics Lab, Lou Amadio, Martin Günther, v4hn
-
-1.2.1 (2019-05-06)
-------------------
-* Fix includes, typos and log messages
-* print ros_warn and give unit quaternion to ogre to prevent rviz crash (`#90 <https://github.com/ccny-ros-pkg/imu_tools/issues/90>`_)
-* Contributors: Jackey-Huo, Martin Günther
-
-1.2.0 (2018-05-25)
-------------------
-
-1.1.5 (2017-05-24)
-------------------
-
-1.1.4 (2017-05-22)
-------------------
-* Add option to display orientation in world frame (`#69 <https://github.com/ccny-ros-pkg/imu_tools/issues/69>`_)
-  Per REP 145 IMU orientation is in the world frame. Rotating the
-  orientation data to transform into the sensor frame results in strange
-  behavior, such as double-rotation of orientation on a robot. Provide an
-  option to display orientation in the world frame, and enable it by
-  default. Continue to translate the position of the data to the sensor
-  frame.
-* Contributors: C. Andy Martin
-
-1.1.3 (2017-03-10)
-------------------
-
-1.1.2 (2016-09-07)
-------------------
-
-1.1.1 (2016-09-07)
-------------------
-
-1.1.0 (2016-04-25)
-------------------
-* Add qt5 dependencies to rviz_imu_plugin package.xml
-  This fixes the compilation errors on Kinetic for Debian Jessie.
-* Contributors: Martin Guenther
-
-1.0.11 (2016-04-22)
--------------------
-
-1.0.10 (2016-04-22)
--------------------
-* Support qt4/qt5 using rviz's exported qt version
-  Closes `#58 <https://github.com/ccny-ros-pkg/imu_tools/issues/58>`_ .
-  This fixes the build on Kinetic, where only Qt5 is available, and
-  is backwards compatible with Qt4 for Indigo.
-* Contributors: Martin Guenther
-
-1.0.9 (2015-10-16)
-------------------
-
-1.0.8 (2015-10-07)
-------------------
-
-1.0.7 (2015-10-07)
-------------------
-
-1.0.6 (2015-10-06)
-------------------
-
-1.0.5 (2015-06-24)
-------------------
-
-1.0.4 (2015-05-06)
-------------------
-
-1.0.3 (2015-01-29)
-------------------
-
-1.0.2 (2015-01-27)
-------------------
-
-1.0.1 (2014-12-10)
-------------------
-* add me as maintainer to package.xml
-* Contributors: Martin Günther
-
-1.0.0 (2014-09-03)
-------------------
-* First public release
-* Contributors: Ivan Dryanovski, Martin Günther, Davide Tateo, Francisco Vina, Lorenzo Riano

imu_tools/rviz_imu_plugin/CMakeLists.txt

@@ -1,66 +0,0 @@
-cmake_minimum_required(VERSION 3.5.1)
-project(rviz_imu_plugin)
-
-find_package(catkin REQUIRED COMPONENTS rviz)
-
-## This setting causes Qt's "MOC" generation to happen automatically.
-set(CMAKE_AUTOMOC ON)
-
-## This plugin includes Qt widgets, so we must include Qt.
-## We'll use the version that rviz used so they are compatible.
-if(rviz_QT_VERSION VERSION_LESS "5")
-  message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
-  find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
-  include(${QT_USE_FILE})
-else()
-  message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
-  find_package(Qt5Widgets REQUIRED)
-  find_package(Qt5Core REQUIRED)
-  find_package(Qt5OpenGL REQUIRED)
-  ## Set the QT_LIBRARIES variable for Qt5, so it can be used below.
-  set(QT_LIBRARIES Qt5::Widgets)
-endif()
-
-## I prefer the Qt signals and slots to avoid defining "emit", "slots",
-## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here.
-add_definitions(-DQT_NO_KEYWORDS)
-
-catkin_package(
-  DEPENDS 
-  CATKIN_DEPENDS rviz
-  INCLUDE_DIRS
-  LIBRARIES
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Here we specify the list of source files.
-## The generated MOC files are included automatically as headers.
-set(SRC_FILES  src/imu_display.cpp
-               src/imu_orientation_visual.cpp
-               src/imu_axes_visual.cpp
-               src/imu_acc_visual.cpp)
-
-## Build libraries
-add_library(${PROJECT_NAME} ${SRC_FILES})
-
-set_target_properties(${PROJECT_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
-
-## Link the library with whatever Qt libraries have been defined by
-## the ``find_package(Qt4 ...)`` line above, or by the
-## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries
-## catkin has included.
-target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES})
-
-install(TARGETS
-  ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(FILES plugin_description.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

imu_tools/rviz_imu_plugin/package.xml

@@ -1,28 +0,0 @@
-<package>
-  <name>rviz_imu_plugin</name>
-  <version>1.2.3</version>
-  <description>
-    RVIZ plugin for IMU visualization
-  </description>
-  <license>BSD</license>
-  <url>http://ros.org/wiki/rviz_imu_plugin</url>
-  <author email="ivan.dryanovski@gmail.com">Ivan Dryanovski</author>
-  <maintainer email="martin.guenther1980@gmail.com">Martin Günther</maintainer>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>qtbase5-dev</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rviz</build_depend>
-
-  <run_depend>libqt5-core</run_depend>
-  <run_depend>libqt5-gui</run_depend>
-  <run_depend>libqt5-widgets</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>rviz</run_depend>
-
-  <export>
-    <rosdoc config="${prefix}/rosdoc.yaml"/>
-    <rviz plugin="${prefix}/plugin_description.xml"/>
-  </export>
-</package>

imu_tools/rviz_imu_plugin/plugin_description.xml

@@ -1,13 +0,0 @@
-<library path="lib/librviz_imu_plugin">
-
-  <class name="rviz_imu_plugin/Imu"
-         type="rviz::ImuDisplay"
-         base_class_type="rviz::Display">
-
-    <description>
-      Displays the orientation and acceleration components of sensor_msgs/Imu messages.
-    </description>
-
-  </class>
-
-</library>

imu_tools/rviz_imu_plugin/rosdoc.yaml

@@ -1,2 +0,0 @@
-- builder: sphinx
-  sphinx_root_dir: src/doc

imu_tools/rviz_imu_plugin/src/imu_acc_visual.cpp

@@ -1,173 +0,0 @@
-/*
- * Copyright (c) 2012, Willow Garage, Inc.
- * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
- * 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 Willow Garage, Inc. 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.
- */
-
-#include "imu_acc_visual.h"
-
-namespace rviz
-{
-
-ImuAccVisual::ImuAccVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node):
-  acc_vector_(NULL),
-  arrow_length_(9.81),
-  arrow_radius_(0.50),
-  head_length_(1.00),
-  head_radius_(1.00),
-  scale_(0.05),
-  alpha_(1.0),
-  color_(1.0, 1.0, 0.0),
-  derotated_(true)
-{
-  scene_manager_ = scene_manager;
-
-  // Ogre::SceneNode s form a tree, with each node storing the
-  // transform (position and orientation) of itself relative to its
-  // parent.  Ogre does the math of combining those transforms when it
-  // is time to render.
-  //
-  // Here we create a node to store the pose of the Imu's header frame
-  // relative to the RViz fixed frame.
-  frame_node_ = parent_node->createChildSceneNode();
-}
-
-ImuAccVisual::~ImuAccVisual()
-{
-  hide();
-
-  // Destroy the frame node since we don't need it anymore.
-  scene_manager_->destroySceneNode(frame_node_);
-}
-
-void ImuAccVisual::show()
-{
-  if (!acc_vector_)
-  {
-    acc_vector_ = new Arrow(scene_manager_, frame_node_);
-    acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
-    acc_vector_->setDirection(direction_);
-    acc_vector_->set(
-      arrow_length_ * scale_, 
-      arrow_radius_ * scale_, 
-      head_length_  * scale_, 
-      head_radius_  * scale_);
-  }
-}
-
-void ImuAccVisual::hide()
-{
-  if (acc_vector_)
-  {
-    delete acc_vector_;
-    acc_vector_ = NULL;
-  }
-}
-
-void ImuAccVisual::setMessage(const sensor_msgs::Imu::ConstPtr& msg)
-{
-  direction_ = Ogre::Vector3(msg->linear_acceleration.x,
-                             msg->linear_acceleration.y,
-                             msg->linear_acceleration.z);
-
-
-  // Rotate the acceleration vector by the IMU orientation. This makes
-  // sense since the visualization of the IMU is also rotated by the 
-  // orientation. In this way, both appear in the inertial frame.
-  if (derotated_)
-  {
-    Ogre::Quaternion orientation(msg->orientation.w,
-                                 msg->orientation.x,
-                                 msg->orientation.y,
-                                 msg->orientation.z);
-
-    direction_ = orientation * direction_;
-  }
-
-  arrow_length_ = sqrt(
-    msg->linear_acceleration.x * msg->linear_acceleration.x +
-    msg->linear_acceleration.y * msg->linear_acceleration.y +
-    msg->linear_acceleration.z * msg->linear_acceleration.z);
-
-  if (acc_vector_)
-  {
-    acc_vector_->setDirection(direction_);
-    acc_vector_->set(
-      arrow_length_ * scale_, 
-      arrow_radius_ * scale_, 
-      head_length_  * scale_, 
-      head_radius_  * scale_);
-  }
-}
-
-void ImuAccVisual::setScale(float scale) 
-{ 
-  scale_ = scale; 
-  if (acc_vector_)
-  {
-    acc_vector_->setDirection(direction_);
-    acc_vector_->set(
-      arrow_length_ * scale_, 
-      arrow_radius_ * scale_, 
-      head_length_  * scale_, 
-      head_radius_  * scale_);
-  }
-}
-
-void ImuAccVisual::setColor(const QColor& color)
-{
-  color_ = color;
-  if (acc_vector_) 
-    acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
-}
-
-void ImuAccVisual::setAlpha(float alpha) 
-{ 
-  alpha_ = alpha; 
-  if (acc_vector_) 
-    acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(),alpha_);
-}
-
-void ImuAccVisual::setDerotated(bool derotated) 
-{ 
-  derotated_ = derotated; 
-  if (acc_vector_) 
-    acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(),alpha_);
-}
-
-void ImuAccVisual::setFramePosition(const Ogre::Vector3& position)
-{
-  frame_node_->setPosition(position);
-}
-
-void ImuAccVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
-{
-  frame_node_->setOrientation(orientation);
-}
-
-} // end namespace rviz
-

imu_tools/rviz_imu_plugin/src/imu_acc_visual.h

@@ -1,110 +0,0 @@
-/*
- * Copyright (c) 2012, Willow Garage, Inc.
- * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
- * 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 Willow Garage, Inc. 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 RVIZ_IMU_PLUGIN_IMU_ACC_VISUAL_H
-#define RVIZ_IMU_PLUGIN_IMU_ACC_VISUAL_H
-
-#include <sensor_msgs/Imu.h>
-#include <OGRE/OgreVector3.h>
-#include <OGRE/OgreSceneNode.h>
-#include <OGRE/OgreSceneManager.h>
-#include <rviz/ogre_helpers/arrow.h>
-#include <QColor>
-
-
-namespace rviz
-{
-
-class ImuAccVisual
-{
-  public:
-    // Constructor.  Creates the visual stuff and puts it into the
-    // scene, but in an unconfigured state.
-    ImuAccVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node);
-
-    // Destructor.  Removes the visual stuff from the scene.
-    virtual ~ImuAccVisual();
-
-    // Configure the visual to show the data in the message.
-    void setMessage(const sensor_msgs::Imu::ConstPtr& msg);
-
-    // Set the pose of the coordinate frame the message refers to.
-    // These could be done inside setMessage(), but that would require
-    // calls to FrameManager and error handling inside setMessage(),
-    // which doesn't seem as clean.  This way ImuVisual is only
-    // responsible for visualization.
-    void setFramePosition(const Ogre::Vector3& position);
-    void setFrameOrientation(const Ogre::Quaternion& orientation);
-
-    // Set the color and alpha of the visual, which are user-editable
-
-    void setScale(float scale);
-    void setColor(const QColor &color);
-    void setAlpha(float alpha);
-    void setDerotated(bool derotated);
-
-    float getScale() { return scale_; }
-    const QColor& getColor() { return color_; }
-    float getAlpha() { return alpha_; }
-    bool  getDerotated() { return derotated_; }
-
-    void show();
-    void hide();
-
-  private:
-
-    void create();
-
-    Arrow * acc_vector_;
-
-    Ogre::Vector3 direction_; // computed from IMU message
-
-    float arrow_length_; // computed from IMU message
-    float arrow_radius_;
-    float head_length_;
-    float head_radius_;
-
-    float scale_;
-    float alpha_;
-    QColor color_;
-
-    bool derotated_;
- 
-    // A SceneNode whose pose is set to match the coordinate frame of
-    // the Imu message header.
-    Ogre::SceneNode * frame_node_;
-
-    // The SceneManager, kept here only so the destructor can ask it to
-    // destroy the ``frame_node_``.
-    Ogre::SceneManager * scene_manager_;
-};
-
-} // end namespace rviz
-
-#endif // RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H

imu_tools/rviz_imu_plugin/src/imu_axes_visual.cpp

@@ -1,144 +0,0 @@
-/*
- * Copyright (c) 2012, Willow Garage, Inc.
- * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
- * 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 Willow Garage, Inc. 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.
- */
-
-#include "imu_axes_visual.h"
-
-#include <ros/ros.h>
-#include <cmath>
-
-namespace rviz
-{
-
-ImuAxesVisual::ImuAxesVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node):
-  orientation_axes_(NULL),
-  scale_(0.15), quat_valid_(true)
-{
-  scene_manager_ = scene_manager;
-
-  // Ogre::SceneNode s form a tree, with each node storing the
-  // transform (position and orientation) of itself relative to its
-  // parent.  Ogre does the math of combining those transforms when it
-  // is time to render.
-  //
-  // Here we create a node to store the pose of the Imu's header frame
-  // relative to the RViz fixed frame.
-  frame_node_ = parent_node->createChildSceneNode();
-}
-
-ImuAxesVisual::~ImuAxesVisual()
-{
-  hide();
-
-  // Destroy the frame node since we don't need it anymore.
-  scene_manager_->destroySceneNode(frame_node_);
-}
-
-void ImuAxesVisual::show()
-{
-  if (!orientation_axes_)
-  {
-    orientation_axes_ = new Axes(scene_manager_, frame_node_);
-    orientation_axes_->setScale(Ogre::Vector3(scale_, scale_, scale_));
-    orientation_axes_->setOrientation(orientation_);
-  }
-}
-
-void ImuAxesVisual::hide()
-{
-  if (orientation_axes_)
-  {
-    delete orientation_axes_;
-    orientation_axes_ = NULL;
-  }
-}
-
-void ImuAxesVisual::setMessage(const sensor_msgs::Imu::ConstPtr& msg)
-{
-  if (checkQuaternionValidity(msg)) {
-    if (!quat_valid_) {
-      ROS_INFO("rviz_imu_plugin got valid quaternion, "
-               "displaying true orientation");
-      quat_valid_ = true;
-    }
-    orientation_ = Ogre::Quaternion(msg->orientation.w,
-                                    msg->orientation.x,
-                                    msg->orientation.y,
-                                    msg->orientation.z);
-  } else {
-    if (quat_valid_) {
-      ROS_WARN("rviz_imu_plugin got invalid quaternion (%lf, %lf, %lf, %lf), "
-               "will display neutral orientation instead", msg->orientation.w,
-               msg->orientation.x,msg->orientation.y,msg->orientation.z);
-      quat_valid_ = false;
-    }
-    // if quaternion is invalid, give a unit quat to Ogre
-    orientation_ = Ogre::Quaternion();
-  }
-
-  if (orientation_axes_)
-    orientation_axes_->setOrientation(orientation_);
-}
-
-void ImuAxesVisual::setScale(float scale) 
-{ 
-  scale_ = scale; 
-  if (orientation_axes_) 
-   orientation_axes_->setScale(Ogre::Vector3(scale_, scale_, scale_));
-}
-
-void ImuAxesVisual::setFramePosition(const Ogre::Vector3& position)
-{
-  frame_node_->setPosition(position);
-}
-
-void ImuAxesVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
-{
-  frame_node_->setOrientation(orientation);
-}
-
-inline bool ImuAxesVisual::checkQuaternionValidity(
-    const sensor_msgs::Imu::ConstPtr& msg) {
-
-  double x = msg->orientation.x,
-         y = msg->orientation.y,
-         z = msg->orientation.z,
-         w = msg->orientation.w;
-  // OGRE can handle unnormalized quaternions, but quat's length extremely small;
-  // this may indicate that invalid (0, 0, 0, 0) quat is passed, this will lead ogre
-  // to crash unexpectly
-  if ( std::sqrt( x*x + y*y + z*z + w*w ) < 0.0001 ) {
-    return false;
-  }
-  return true;
-}
-
-
-} // end namespace rviz
-

imu_tools/rviz_imu_plugin/src/imu_axes_visual.h

@@ -1,98 +0,0 @@
-/*
- * Copyright (c) 2012, Willow Garage, Inc.
- * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
- * 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 Willow Garage, Inc. 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 RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H
-#define RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H
-
-#include <sensor_msgs/Imu.h>
-#include <OGRE/OgreSceneNode.h>
-#include <OGRE/OgreSceneManager.h>
-#include <rviz/ogre_helpers/axes.h>
-
-namespace rviz
-{
-
-class Axes;
-
-class ImuAxesVisual
-{
-  public:
-    // Constructor.  Creates the visual stuff and puts it into the
-    // scene, but in an unconfigured state.
-    ImuAxesVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node);
-
-    // Destructor.  Removes the visual stuff from the scene.
-    virtual ~ImuAxesVisual();
-
-    // Configure the visual to show the data in the message.
-    void setMessage(const sensor_msgs::Imu::ConstPtr& msg);
-
-    // Set the pose of the coordinate frame the message refers to.
-    // These could be done inside setMessage(), but that would require
-    // calls to FrameManager and error handling inside setMessage(),
-    // which doesn't seem as clean.  This way ImuVisual is only
-    // responsible for visualization.
-    void setFramePosition(const Ogre::Vector3& position);
-    void setFrameOrientation(const Ogre::Quaternion& orientation);
-
-    // Set the color and alpha of the visual, which are user-editable
-
-    void setScale(float scale);
-
-    float getScale() { return scale_; }
-
-    void show();
-    void hide();
-
-  private:
-
-    void create();
-    inline bool checkQuaternionValidity(
-        const sensor_msgs::Imu::ConstPtr& msg);
-
-    Ogre::Quaternion orientation_;
-
-    float scale_;
-    bool quat_valid_;
-
-    Axes * orientation_axes_;
-  
-    // A SceneNode whose pose is set to match the coordinate frame of
-    // the Imu message header.
-    Ogre::SceneNode * frame_node_;
-
-    // The SceneManager, kept here only so the destructor can ask it to
-    // destroy the ``frame_node_``.
-    Ogre::SceneManager * scene_manager_;
-};
-
-} // end namespace rviz
-
-#endif //  RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H

imu_tools/rviz_imu_plugin/src/imu_display.cpp

@@ -1,331 +0,0 @@
-/*
- * Copyright (c) 2012, Willow Garage, Inc.
- * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
- * 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 Willow Garage, Inc. 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.
- */
-
-#include "imu_display.h"
-
-#include <rviz/properties/status_property.h>
-
-namespace rviz
-{
-
-ImuDisplay::ImuDisplay():
-    fixed_frame_orientation_(true),
-    box_enabled_(false),
-    axes_enabled_(true),
-    acc_enabled_(false),
-    scene_node_(NULL),
-    messages_received_(0)
-{
-    createProperties();
-
-}
-
-void ImuDisplay::onEnable()
-{
-    MessageFilterDisplay<sensor_msgs::Imu>::onEnable();
-
-    if (box_enabled_)
-        box_visual_->show();
-    else
-        box_visual_->hide();
-
-    if (axes_enabled_)
-        axes_visual_->show();
-    else
-        axes_visual_->hide();
-
-    if (acc_enabled_)
-        acc_visual_->show();
-    else
-        acc_visual_->hide();
-}
-
-void ImuDisplay::onDisable()
-{
-    MessageFilterDisplay<sensor_msgs::Imu>::onDisable();
-
-    box_visual_->hide();
-    axes_visual_->hide();
-    acc_visual_->hide();
-}
-
-void ImuDisplay::onInitialize()
-{
-    MFDClass::onInitialize();
-
-    // Make an Ogre::SceneNode to contain all our visuals.
-    scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
-
-    // create orientation box visual
-    box_visual_ = new ImuOrientationVisual(
-                context_->getSceneManager(), scene_node_);
-
-    // create orientation axes visual
-    axes_visual_ = new ImuAxesVisual(
-                context_->getSceneManager(), scene_node_);
-
-    // create acceleration vector visual
-    acc_visual_ = new ImuAccVisual(
-                context_->getSceneManager(), scene_node_);
-}
-
-ImuDisplay::~ImuDisplay()
-{
-}
-
-void ImuDisplay::reset()
-{
-    MFDClass::reset();
-    messages_received_ = 0;
-    setStatus(rviz::StatusProperty::Warn, "Topic", "No messages received" );
-
-    box_visual_->hide();
-    axes_visual_->hide();
-    acc_visual_->hide();
-}
-
-void ImuDisplay::updateTop() {
-    fixed_frame_orientation_ = fixed_frame_orientation_property_->getBool();
-}
-
-void ImuDisplay::updateBox() {
-    box_enabled_ = box_enabled_property_->getBool();
-    if (isEnabled() && box_enabled_)
-        box_visual_->show();
-    else
-        box_visual_->hide();
-
-    box_visual_->setScaleX(box_scale_x_property_->getFloat());
-    box_visual_->setScaleY(box_scale_y_property_->getFloat());
-    box_visual_->setScaleZ(box_scale_z_property_->getFloat());
-    box_visual_->setColor(box_color_property_->getColor());
-    box_visual_->setAlpha(box_alpha_property_->getFloat());
-}
-
-void ImuDisplay::updateAxes() {
-    axes_enabled_ = axes_enabled_property_->getBool();
-    if (isEnabled() && axes_enabled_)
-        axes_visual_->show();
-    else
-        axes_visual_->hide();
-
-    axes_visual_->setScale(axes_scale_property_->getFloat());
-
-}
-
-void ImuDisplay::updateAcc() {
-    acc_enabled_ = acc_enabled_property_->getBool();
-    if (isEnabled() && acc_enabled_)
-        acc_visual_->show();
-    else
-        acc_visual_->hide();
-
-    acc_visual_->setScale(acc_scale_property_->getFloat());
-    acc_visual_->setColor(acc_color_property_->getColor());
-    acc_visual_->setAlpha(acc_alpha_property_->getFloat());
-    acc_visual_->setDerotated(acc_derotated_property_->getBool());
-}
-
-void ImuDisplay::processMessage( const sensor_msgs::Imu::ConstPtr& msg )
-{
-    if(!isEnabled())
-        return;
-
-    ++messages_received_;
-
-    std::stringstream ss;
-    ss << messages_received_ << " messages received";
-    setStatus( rviz::StatusProperty::Ok, "Topic", ss.str().c_str() );
-
-    Ogre::Quaternion orientation;
-    Ogre::Vector3 position;
-    if(!context_->getFrameManager()->getTransform(msg->header.frame_id,
-                                                  msg->header.stamp,
-                                                  position, orientation ))
-    {
-        ROS_ERROR("Error transforming from frame '%s' to frame '%s'",
-                  msg->header.frame_id.c_str(), fixed_frame_.toStdString().c_str());
-        return;
-    }
-
-    if (fixed_frame_orientation_) {
-        /* Display IMU orientation is in the world-fixed frame. */
-        Ogre::Vector3 unused;
-        if(!context_->getFrameManager()->getTransform(context_->getFrameManager()->getFixedFrame(),
-                                                      msg->header.stamp,
-                                                      unused, orientation ))
-        {
-            ROS_ERROR("Error getting fixed frame transform");
-            return;
-        }
-    }
-
-    if (box_enabled_)
-    {
-        box_visual_->setMessage(msg);
-        box_visual_->setFramePosition(position);
-        box_visual_->setFrameOrientation(orientation);
-        box_visual_->show();
-    }
-    if (axes_enabled_)
-    {
-        axes_visual_->setMessage(msg);
-        axes_visual_->setFramePosition(position);
-        axes_visual_->setFrameOrientation(orientation);
-        axes_visual_->show();
-    }
-    if (acc_enabled_)
-    {
-        acc_visual_->setMessage(msg);
-        acc_visual_->setFramePosition(position);
-        acc_visual_->setFrameOrientation(orientation);
-        acc_visual_->show();
-    }
-}
-
-void ImuDisplay::createProperties()
-{
-    // **** top level properties
-    fixed_frame_orientation_property_ = new rviz::BoolProperty("fixed_frame_orientation",
-                                                   fixed_frame_orientation_,
-                                                   "Use world fixed frame for display orientation instead of IMU reference frame",
-                                                   this,
-                                                   SLOT(updateTop()),
-                                                   this);
-
-    // **** box properties
-    box_category_ = new rviz::Property("Box properties",
-                                       QVariant(),
-                                       "The list of all the box properties",
-                                       this
-                                       );
-    box_enabled_property_ = new rviz::BoolProperty("Enable box",
-                                                   box_enabled_,
-                                                   "Enable the box display",
-                                                   box_category_,
-                                                   SLOT(updateBox()),
-                                                   this);
-    box_scale_x_property_ = new rviz::FloatProperty("x_scale",
-                                                    1.0,
-                                                    "Box length (x), in meters.",
-                                                    box_category_,
-                                                    SLOT(updateBox()),
-                                                    this);
-    box_scale_y_property_ = new rviz::FloatProperty("y_scale",
-                                                    1.0,
-                                                    "Box length (y), in meters.",
-                                                    box_category_,
-                                                    SLOT(updateBox()),
-                                                    this);
-    box_scale_z_property_ = new rviz::FloatProperty("z_scale",
-                                                    1.0,
-                                                    "Box length (z), in meters.",
-                                                    box_category_,
-                                                    SLOT(updateBox()),
-                                                    this);
-    box_color_property_  = new rviz::ColorProperty("Box color",
-                                                   Qt::red,
-                                                   "Color to draw IMU box",
-                                                   box_category_,
-                                                   SLOT(updateBox()),
-                                                   this);
-    box_alpha_property_ = new rviz::FloatProperty("Box alpha",
-                                                  1.0,
-                                                  "0 is fully transparent, 1.0 is fully opaque.",
-                                                  box_category_,
-                                                  SLOT(updateBox()),
-                                                  this);
-
-    // **** axes properties
-    axes_category_ = new rviz::Property("Axes properties",
-                                       QVariant(),
-                                       "The list of all the axes properties",
-                                       this
-                                       );
-    axes_enabled_property_ = new rviz::BoolProperty("Enable axes",
-                                                   axes_enabled_,
-                                                   "Enable the axes display",
-                                                   axes_category_,
-                                                   SLOT(updateAxes()),
-                                                   this);
-    axes_scale_property_ = new rviz::FloatProperty("Axes scale",
-                                                   true,
-                                                   "Axes size, in meters",
-                                                   axes_category_,
-                                                   SLOT(updateAxes()),
-                                                   this);
-
-    // **** acceleration vector properties
-    acc_category_ = new rviz::Property("Acceleration properties",
-                                       QVariant(),
-                                       "The list of all the acceleration properties",
-                                       this
-                                       );
-    acc_enabled_property_ = new rviz::BoolProperty("Enable acceleration",
-                                                   acc_enabled_,
-                                                   "Enable the acceleration display",
-                                                   acc_category_,
-                                                   SLOT(updateAcc()),
-                                                   this);
-
-    acc_derotated_property_ = new rviz::BoolProperty("Derotate acceleration",
-                                                     true,
-                                                     "If selected, the acceleration is derotated by the IMU orientation. Otherwise, the raw sensor reading is displayed.",
-                                                     acc_category_,
-                                                     SLOT(updateAcc()),
-                                                     this);
-    acc_scale_property_ = new rviz::FloatProperty("Acc. vector scale",
-                                                  true,
-                                                  "Acceleration vector size, in meters",
-                                                  acc_category_,
-                                                  SLOT(updateAcc()),
-                                                  this);
-    acc_color_property_ =  new rviz::ColorProperty("Acc. vector color",
-                                                   Qt::red,
-                                                   "Color to draw acceleration vector.",
-                                                   acc_category_,
-                                                   SLOT(updateAcc()),
-                                                   this);
-    acc_alpha_property_ = new rviz::FloatProperty("Acc. vector alpha",
-                                                 1.0,
-                                                 "0 is fully transparent, 1.0 is fully opaque.",
-                                                 acc_category_,
-                                                 SLOT(updateAcc()),
-                                                  this);
-}
-
-} // end namespace rviz
-
-// Tell pluginlib about this class.  It is important to do this in
-// global scope, outside our package's namespace.
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(rviz::ImuDisplay, rviz::Display)
-
-

imu_tools/rviz_imu_plugin/src/imu_display.h

@@ -1,171 +0,0 @@
-/*
- * Copyright (c) 2012, Willow Garage, Inc.
- * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
- * 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 Willow Garage, Inc. 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 RVIZ_IMU_PLUGIN_IMU_DISPLAY_H
-#define RVIZ_IMU_PLUGIN_IMU_DISPLAY_H
-
-#include <message_filters/subscriber.h>
-#include <tf/message_filter.h>
-#include <sensor_msgs/Imu.h>
-#include <rviz/display.h>
-#include <rviz/visualization_manager.h>
-#include <rviz/properties/property.h>
-#include <rviz/frame_manager.h>
-#include <rviz/message_filter_display.h>
-#include <OGRE/OgreSceneNode.h>
-#include <OGRE/OgreSceneManager.h>
-#include <tf/transform_listener.h>
-
-#include <rviz/properties/bool_property.h>
-#include <rviz/properties/float_property.h>
-#include <rviz/properties/color_property.h>
-#include <rviz/properties/ros_topic_property.h>
-#include <rviz/display_group.h>
-
-#include "imu_axes_visual.h"
-#include "imu_orientation_visual.h"
-#include "imu_acc_visual.h"
-
-namespace Ogre
-{
-class SceneNode;
-}
-
-namespace rviz
-{
-
-class ImuDisplay: public rviz::MessageFilterDisplay<sensor_msgs::Imu>
-{
-    Q_OBJECT
-public:
-
-    ImuDisplay();
-    virtual ~ImuDisplay();
-
-    virtual void onInitialize();
-    virtual void onEnable();
-    virtual void onDisable();
-    virtual void reset();
-    virtual void createProperties();
-
-    void setTopic(const std::string& topic);
-    const std::string& getTopic() { return topic_; }
-
-    void setBoxEnabled(bool enabled);
-    void setBoxScaleX(float x);
-    void setBoxScaleY(float y);
-    void setBoxScaleZ(float z);
-    void setBoxColor(const Color& color);
-    void setBoxAlpha(float alpha);
-
-    void setAxesEnabled(bool enabled);
-    void setAxesScale(float scale);
-
-    void setAccEnabled(bool enabled);
-    void setAccDerotated(bool derotated);
-    void setAccScale(float scale);
-    void setAccColor(const Color& color);
-    void setAccAlpha(float alpha);
-
-    bool  getBoxEnabled() { return box_enabled_; }
-    float getBoxScaleX()  { return box_visual_->getScaleX(); }
-    float getBoxScaleY()  { return box_visual_->getScaleY(); }
-    float getBoxScaleZ()  { return box_visual_->getScaleZ(); }
-    float getBoxAlpha()   { return box_visual_->getAlpha(); }
-    const QColor& getBoxColor() { return box_visual_->getColor(); }
-
-    bool  getAxesEnabled() { return axes_enabled_; }
-    float getAxesScale()   { return axes_visual_->getScale(); }
-
-    bool  getAccEnabled()   { return acc_enabled_; }
-    float getAccDerotated() { return acc_visual_->getDerotated(); }
-    float getAccScale()     { return acc_visual_->getScale(); }
-    float getAccAlpha()     { return acc_visual_->getAlpha(); }
-    const QColor& getAccColor() { return acc_visual_->getColor(); }
-
-protected Q_SLOTS:
-
-    void updateTop();
-    void updateBox();
-    void updateAxes();
-    void updateAcc();
-
-private:
-
-    // Property objects for user-editable properties.
-    rviz::BoolProperty*  fixed_frame_orientation_property_;
-
-    rviz::Property* box_category_;
-    rviz::Property* axes_category_;
-    rviz::Property* acc_category_;
-
-//    rviz::RosTopicProperty* topic_property_;
-    rviz::BoolProperty*  box_enabled_property_;
-    rviz::FloatProperty* box_scale_x_property_;
-    rviz::FloatProperty* box_scale_y_property_;
-    rviz::FloatProperty* box_scale_z_property_;
-    rviz::ColorProperty* box_color_property_;
-    rviz::FloatProperty* box_alpha_property_;
-
-    rviz::BoolProperty*  axes_enabled_property_;
-    rviz::FloatProperty* axes_scale_property_;
-
-    rviz::BoolProperty*  acc_enabled_property_;
-    rviz::BoolProperty*  acc_derotated_property_;
-    rviz::FloatProperty* acc_scale_property_;
-    rviz::ColorProperty* acc_color_property_;
-    rviz::FloatProperty* acc_alpha_property_;
-
-    // Differetn types of visuals
-    ImuOrientationVisual * box_visual_;
-    ImuAxesVisual        * axes_visual_;
-    ImuAccVisual         * acc_visual_;
-
-    // User-editable property variables.
-    std::string topic_;
-    bool fixed_frame_orientation_;
-    bool box_enabled_;
-    bool axes_enabled_;
-    bool acc_enabled_;
-
-    // A node in the Ogre scene tree to be the parent of all our visuals.
-    Ogre::SceneNode* scene_node_;
-
-    int messages_received_;
-
-    // Function to handle an incoming ROS message.
-    void processMessage( const sensor_msgs::Imu::ConstPtr& msg);
-
-};
-
-} // end namespace rviz
-
-#endif // IMU_DISPLAY_H
-

imu_tools/rviz_imu_plugin/src/imu_orientation_visual.cpp

@@ -1,179 +0,0 @@
-/*
- * Copyright (c) 2012, Willow Garage, Inc.
- * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
- * 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 Willow Garage, Inc. 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.
- */
-
-#include "imu_orientation_visual.h"
-
-#include <ros/ros.h>
-#include <cmath>
-
-namespace rviz
-{
-
-ImuOrientationVisual::ImuOrientationVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node):
-  orientation_box_(NULL),
-  scale_x_(0.07),
-  scale_y_(0.10),
-  scale_z_(0.03),
-  alpha_(1.0),
-  quat_valid_(true),
-  color_(0.5, 0.5, 0.5)
-{
-  scene_manager_ = scene_manager;
-
-  // Ogre::SceneNode s form a tree, with each node storing the
-  // transform (position and orientation) of itself relative to its
-  // parent.  Ogre does the math of combining those transforms when it
-  // is time to render.
-  //
-  // Here we create a node to store the pose of the Imu's header frame
-  // relative to the RViz fixed frame.
-  frame_node_ = parent_node->createChildSceneNode();
-}
-
-ImuOrientationVisual::~ImuOrientationVisual()
-{
-  hide();
-
-  // Destroy the frame node since we don't need it anymore.
-  scene_manager_->destroySceneNode(frame_node_);
-}
-
-void ImuOrientationVisual::show()
-{
-  if (!orientation_box_)
-  {
-    orientation_box_ = new Shape(Shape::Cube, scene_manager_, frame_node_);
-    orientation_box_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
-    orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_));
-    orientation_box_->setOrientation(orientation_);
-  }
-}
-
-void ImuOrientationVisual::hide()
-{
-  if (orientation_box_)
-  {
-    delete orientation_box_;
-    orientation_box_ = NULL;
-  }
-}
-
-void ImuOrientationVisual::setMessage(const sensor_msgs::Imu::ConstPtr& msg)
-{
-  if (checkQuaternionValidity(msg)) {
-    if (!quat_valid_) {
-      ROS_INFO("rviz_imu_plugin got valid quaternion, "
-               "displaying true orientation");
-      quat_valid_ = true;
-    }
-    orientation_ = Ogre::Quaternion(msg->orientation.w,
-                                    msg->orientation.x,
-                                    msg->orientation.y,
-                                    msg->orientation.z);
-  } else {
-    if (quat_valid_) {
-      ROS_WARN("rviz_imu_plugin got invalid quaternion (%lf, %lf, %lf, %lf), "
-               "will display neutral orientation instead", msg->orientation.w,
-               msg->orientation.x,msg->orientation.y,msg->orientation.z);
-      quat_valid_ = false;
-    }
-    // if quaternion is invalid, give a unit quat to Ogre
-    orientation_ = Ogre::Quaternion();
-  }
-
-  if (orientation_box_)
-    orientation_box_->setOrientation(orientation_);
-}
-
-void ImuOrientationVisual::setScaleX(float x) 
-{ 
-  scale_x_ = x; 
-  if (orientation_box_) 
-   orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_));
-}
-
-void ImuOrientationVisual::setScaleY(float y) 
-{ 
-  scale_y_ = y; 
-  if (orientation_box_) 
-    orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_));
-}
-
-void ImuOrientationVisual::setScaleZ(float z) 
-{ 
-  scale_z_ = z; 
-  if (orientation_box_) 
-    orientation_box_->setScale(Ogre::Vector3(scale_x_, scale_y_, scale_z_));
-
-}
-
-void ImuOrientationVisual::setColor(const QColor& color)
-{
-  color_ = color;
-  if (orientation_box_) 
-    orientation_box_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
-}
-
-void ImuOrientationVisual::setAlpha(float alpha) 
-{ 
-  alpha_ = alpha; 
-  if (orientation_box_) 
-    orientation_box_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_);
-}
-
-void ImuOrientationVisual::setFramePosition(const Ogre::Vector3& position)
-{
-  frame_node_->setPosition(position);
-}
-
-void ImuOrientationVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
-{
-  frame_node_->setOrientation(orientation);
-}
-
-inline bool ImuOrientationVisual::checkQuaternionValidity(
-    const sensor_msgs::Imu::ConstPtr& msg) {
-
-  double x = msg->orientation.x,
-         y = msg->orientation.y,
-         z = msg->orientation.z,
-         w = msg->orientation.w;
-  // OGRE can handle unnormalized quaternions, but quat's length extremely small;
-  // this may indicate that invalid (0, 0, 0, 0) quat is passed, this will lead ogre
-  // to crash unexpectly
-  if ( std::sqrt( x*x + y*y + z*z + w*w ) < 0.0001 ) {
-    return false;
-  }
-  return true;
-}
-
-
-} // end namespace rviz
-

imu_tools/rviz_imu_plugin/src/imu_orientation_visual.h

@@ -1,107 +0,0 @@
-/*
- * Copyright (c) 2012, Willow Garage, Inc.
- * Copyright (c) 2012, Ivan Dryanovski <ivan.dryanovski@gmail.com>
- * 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 Willow Garage, Inc. 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 RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H
-#define RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H
-
-#include <sensor_msgs/Imu.h>
-#include <OGRE/OgreSceneNode.h>
-#include <OGRE/OgreSceneManager.h>
-#include <rviz/ogre_helpers/shape.h>
-#include <rviz/helpers/color.h>
-#include <QColor>
-
-namespace rviz
-{
-
-class ImuOrientationVisual
-{
-  public:
-    // Constructor.  Creates the visual stuff and puts it into the
-    // scene, but in an unconfigured state.
-    ImuOrientationVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node);
-
-    // Destructor.  Removes the visual stuff from the scene.
-    virtual ~ImuOrientationVisual();
-
-    // Configure the visual to show the data in the message.
-    void setMessage(const sensor_msgs::Imu::ConstPtr& msg);
-
-    // Set the pose of the coordinate frame the message refers to.
-    // These could be done inside setMessage(), but that would require
-    // calls to FrameManager and error handling inside setMessage(),
-    // which doesn't seem as clean.  This way ImuVisual is only
-    // responsible for visualization.
-    void setFramePosition(const Ogre::Vector3& position);
-    void setFrameOrientation(const Ogre::Quaternion& orientation);
-
-    // Set the color and alpha of the visual, which are user-editable
-
-    void setScaleX(float x);
-    void setScaleY(float y);
-    void setScaleZ(float z);
-    void setColor(const QColor &color);
-    void setAlpha(float alpha);
-
-    float getScaleX() { return scale_x_; }
-    float getScaleY() { return scale_y_; }
-    float getScaleZ() { return scale_z_; }
-    const QColor& getColor() { return color_; }
-    float getAlpha() { return alpha_; }
-
-    void show();
-    void hide();
-
-  private:
-
-    void create();
-    inline bool checkQuaternionValidity(
-        const sensor_msgs::Imu::ConstPtr& msg);
-
-    Ogre::Quaternion orientation_;
-
-    float scale_x_, scale_y_, scale_z_;
-    QColor color_;
-    float alpha_;
-    bool quat_valid_;
-
-    Shape * orientation_box_;
-  
-    // A SceneNode whose pose is set to match the coordinate frame of
-    // the Imu message header.
-    Ogre::SceneNode * frame_node_;
-
-    // The SceneManager, kept here only so the destructor can ask it to
-    // destroy the ``frame_node_``.
-    Ogre::SceneManager * scene_manager_;
-};
-
-} // end namespace rviz
-
-#endif // RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H

rasp_imu_hat_6dof/CMakeLists.txt

@@ -1,55 +1,29 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(rasp_imu_hat_6dof)
 
-find_package(catkin REQUIRED COMPONENTS 
-    rospy
-    std_msgs
-    dynamic_reconfigure
-    message_generation)
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-add_service_files(
-    FILES
-    GetYawData.srv
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/GetYawData.srv"
 )
 
-generate_messages(
-    DEPENDENCIES
-    std_msgs
+install(PROGRAMS
+  nodes/imu_node.py
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-generate_dynamic_reconfigure_options(
-    cfg/imu.cfg
+install(FILES
+  nodes/imu_data.py
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-catkin_package(
-    CATKIN_DEPENDS
-    std_msgs
-    message_runtime
-)
-
-include_directories(
-    ${catkin_INCLUDE_DIRS}
-)
-
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY cfg
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY src
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-catkin_install_python(PROGRAMS
-	nodes/imu_node.py
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes
+install(DIRECTORY cfg launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

rasp_imu_hat_6dof/README.md

@@ -1,3 +1,21 @@
 # rasp_imu_hat_6dof
 
-基于树莓派制作的扩展板，可以直接插在树莓派上使用，这里代码为ROS上使用的软件包源码，可以下载直接在catkin_ws/src目录下，然后编译使用
+这是树莓派 6DOF IMU HAT 的 ROS2 Humble IIC 驱动包。
+
+节点 `imu_node.py` 通过 IIC 读取 IMU 的加速度、角速度、四元数、温度和 yaw 数据，并发布：
+
+- `imu_data`
+- `imu_temp`
+- `yaw_data`
+
+启动：
+
+```bash
+ros2 launch rasp_imu_hat_6dof imu_data_pub.launch.py
+```
+
+运行时 yaw 校准使用 ROS2 参数：
+
+```bash
+ros2 param set /imu_node yaw_calibration 10.0
+```

rasp_imu_hat_6dof/cfg/imu.cfg

@@ -1,9 +0,0 @@
-#!/usr/bin/env python
-PACKAGE = "rasp_imu_hat_6dof"
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("yaw_calibration", double_t, 0, "Yaw Calibration", 0, -180, 180)
-
-exit(gen.generate(PACKAGE, "rasp_imu_hat_6dof", "imu"))

rasp_imu_hat_6dof/cfg/imu_display.rviz

@@ -1,5 +1,5 @@
 Panels:
-  - Class: rviz/Displays
+  - Class: rviz_common/Displays
     Help Height: 123
     Name: Displays
     Property Tree Widget:
@@ -8,21 +8,21 @@ Panels:
         - /Grid1/Offset1
       Splitter Ratio: 0.6222222447395325
     Tree Height: 344
-  - Class: rviz/Selection
+  - Class: rviz_common/Selection
     Name: Selection
-  - Class: rviz/Tool Properties
+  - Class: rviz_common/Tool Properties
     Expanded:
       - /2D Pose Estimate1
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
     Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
+  - Class: rviz_common/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
-  - Class: rviz/Time
+  - Class: rviz_common/Time
     Experimental: false
     Name: Time
     SyncMode: 0
@@ -36,7 +36,7 @@ Visualization Manager:
   Displays:
     - Alpha: 0.699999988079071
       Cell Size: 1
-      Class: rviz/Grid
+      Class: rviz_default_plugins/Grid
       Color: 50; 115; 54
       Enabled: true
       Line Style:
@@ -83,26 +83,26 @@ Visualization Manager:
     Frame Rate: 30
   Name: root
   Tools:
-    - Class: rviz/Interact
+    - Class: rviz_default_plugins/Interact
       Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+    - Class: rviz_default_plugins/SetInitialPose
       Theta std deviation: 0.2617993950843811
       Topic: /initialpose
       X std deviation: 0.5
       Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
+    - Class: rviz_default_plugins/SetGoal
+      Topic: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
       Single click: true
       Topic: /clicked_point
   Value: true
   Views:
     Current:
-      Class: rviz/Orbit
+      Class: rviz_default_plugins/Orbit
       Distance: 0.9545544385910034
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549

rasp_imu_hat_6dof/cfg/param.yaml

@@ -1,20 +1,17 @@
 ###################################################
-# Copyright: 2016-2021 www.corvin.cn ROS小课堂
-# Description:使用IIC总线来读取IMU模块的数据,并
-#   通过话题将imu数据发送出来.这里是可以配置的
-#   一些参数.各参数意义如下:
-#   pub_data_topic:发布imu数据的话题名.
-#   pub_temp_topic:发布当前imu模块温度的话题名.
-#   yaw_topic:发布当前yaw偏航角的话题,单位弧度.
-#   link_name:imu模块的urdf中link名字.
-#   pub_hz:上述各话题发布的频率.
-# Author: corvin
-# History:
-#   20200406:init this file.
-#   20200410:更新默认发布imu话题数据频率为20hz.
+# ROS2 Humble参数文件。节点名必须与launch里的name保持一致,
+# 参数需要放在ros__parameters下面,否则不会被ROS2加载。
 ###################################################
-pub_data_topic: imu_data
-pub_temp_topic: imu_temp
-yaw_topic: yaw_data
-link_name: base_imu_link
-pub_hz: 20
+imu_node:
+  ros__parameters:
+    iic_bus: 1
+    # 0x50的十进制值为80。
+    iic_addr: 80
+    pub_data_topic: imu_data
+    pub_temp_topic: imu_temp
+    yaw_topic: yaw_data
+    link_name: base_imu_link
+    pub_hz: 20.0
+    # 替代ROS1 dynamic_reconfigure的yaw_calibration,运行时可用ros2 param set修改。
+    yaw_calibration: 0.0
+    get_yaw_service: /imu_node/GetYawData

rasp_imu_hat_6dof/launch/imu_data_pub.launch

@@ -1,17 +0,0 @@
-<!---
-  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
-  Description:该launch启动文件是为了启动扩展板,使其进入正常工作状态.启动后就会在
-  /imu_data话题上发布imu传感器信息.需要该信息的节点,只需订阅该话题即可.同时我们还可
-  以使用dynamic_reconfigure来动态实时的修正z轴的角度.
-  Author: corvin
-  History:
-    20191031:Initial this launch file.
--->
-<launch>
-  <arg name="imu_cfg_file" default="$(find rasp_imu_hat_6dof)/cfg/param.yaml"/>
-
-  <node pkg="rasp_imu_hat_6dof" type="imu_node.py" name="imu_node" output="screen">
-      <rosparam file="$(arg imu_cfg_file)" command="load"/>
-  </node>
-</launch>
-

rasp_imu_hat_6dof/launch/imu_data_pub.launch.py

@@ -0,0 +1,30 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # 默认加载IIC IMU的ROS2参数文件。
+    default_config = PathJoinSubstitution([
+        FindPackageShare('rasp_imu_hat_6dof'),
+        'cfg',
+        'param.yaml',
+    ])
+
+    config_arg = DeclareLaunchArgument(
+        'imu_cfg_file',
+        default_value=default_config,
+        description='rasp_imu_hat_6dof ROS2 parameter file',
+    )
+
+    imu_node = Node(
+        package='rasp_imu_hat_6dof',
+        executable='imu_node.py',
+        name='imu_node',
+        output='screen',
+        parameters=[LaunchConfiguration('imu_cfg_file')],
+    )
+
+    return LaunchDescription([config_arg, imu_node])

rasp_imu_hat_6dof/launch/imu_rviz_display.launch

@@ -1,13 +0,0 @@
-<!---
-  Copyright:2016-2021 https://www.corvin.cn ROS小课堂
-  Description:该launch启动文件是为了在启动扩展板,使其进入正常工作状态后,
-    可以在rviz中可视化查看imu扩展板当前的姿态信息.
-  Author: corvin
-  History:
-    20191031:Initial this launch file.
--->
-<launch>
-  <node pkg="rviz" type="rviz" name="imu_rviz_node"
-        args="-d $(find rasp_imu_hat_6dof)/cfg/imu_display.rviz">
-  </node>
-</launch>

rasp_imu_hat_6dof/launch/imu_rviz_display.launch.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # 启动RViz2并加载IMU显示配置文件。
+    rviz_config = PathJoinSubstitution([
+        FindPackageShare('rasp_imu_hat_6dof'),
+        'cfg',
+        'imu_display.rviz',
+    ])
+
+    return LaunchDescription([
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='imu_rviz_node',
+            output='screen',
+            arguments=['-d', rviz_config],
+        )
+    ])

rasp_imu_hat_6dof/nodes/imu_data.py

@@ -1,69 +1,90 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: UTF-8 -*-
 
-# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
-# Author: corvin
-# Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
-#    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
-#    中读取IMU模块的三轴加速度、三轴角速度、四元数。
-# History:
-#    20191031: Initial this file.
-#    20191209：Add get imu chip temperature function-get_temp().
-#    20200702: 读取各数据后需要使用np.short转换才能进行后续数据处理.
-#    20210319:从寄存器地址读取数据时,直接一次性读取多个字节,不是依次
-#      读取多个寄存器地址,每个地址读取２字节.
+# Copyright: 2016-2026 https://www.corvin.cn ROS小课堂
+# Description: 通过IIC读取IMU扩展板的原始寄存器数据,并转换为物理量。
+
 import smbus
-import rospy
-import numpy as np
+
+
+def _to_int16(low_byte, high_byte):
+    """将IMU寄存器的低字节+高字节转换为有符号16位整数。"""
+    value = (high_byte << 8) | low_byte
+    if value >= 0x8000:
+        value -= 0x10000
+    return value
+
 
 class MyIMU(object):
-    def __init__(self, addr):
+    def __init__(self, addr, iic_bus=1, logger=None):
         self.addr = addr
-        self.i2c = smbus.SMBus(1)
+        self.logger = logger
+        self.i2c = smbus.SMBus(iic_bus)
+
+        # 初始化缓存值,读取失败时节点可以保留上一次有效数据。
+        self.raw_roll = 0.0
+        self.raw_pitch = 0.0
+        self.raw_yaw = 0.0
+        self.raw_ax = 0.0
+        self.raw_ay = 0.0
+        self.raw_az = 0.0
+        self.raw_gx = 0.0
+        self.raw_gy = 0.0
+        self.raw_gz = 0.0
+        self.raw_q0 = 1.0
+        self.raw_q1 = 0.0
+        self.raw_q2 = 0.0
+        self.raw_q3 = 0.0
+        self.temp = 0.0
+
+    def _log_error(self, message):
+        if self.logger is not None:
+            self.logger.error(message)
 
     def get_YPRAG(self):
+        """读取欧拉角、加速度和角速度,成功返回True。"""
         try:
-            rpy_data  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
+            rpy_data = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
             axyz_data = self.i2c.read_i2c_block_data(self.addr, 0x34, 6)
             gxyz_data = self.i2c.read_i2c_block_data(self.addr, 0x37, 6)
-        except IOError:
-            rospy.logerr("Read IMU RPYAG date error !")
-        else:
-            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])/32768.0*180.0)
-            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])/32768.0*180.0)
-            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])/32768.0*180.0)
+        except OSError:
+            self._log_error("Read IMU RPYAG data error !")
+            return False
 
-            self.raw_ax = float(np.short(np.short(axyz_data[1]<<8)|axyz_data[0])/32768.0*16.0)
-            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])/32768.0*16.0)
-            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])/32768.0*16.0)
+        self.raw_roll = _to_int16(rpy_data[0], rpy_data[1]) / 32768.0 * 180.0
+        self.raw_pitch = _to_int16(rpy_data[2], rpy_data[3]) / 32768.0 * 180.0
+        self.raw_yaw = _to_int16(rpy_data[4], rpy_data[5]) / 32768.0 * 180.0
 
-            self.raw_gx = float(np.short(np.short(gxyz_data[1]<<8)|gxyz_data[0])/32768.0*2000.0)
-            self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])/32768.0*2000.0)
-            self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])/32768.0*2000.0)
+        self.raw_ax = _to_int16(axyz_data[0], axyz_data[1]) / 32768.0 * 16.0
+        self.raw_ay = _to_int16(axyz_data[2], axyz_data[3]) / 32768.0 * 16.0
+        self.raw_az = _to_int16(axyz_data[4], axyz_data[5]) / 32768.0 * 16.0
+
+        self.raw_gx = _to_int16(gxyz_data[0], gxyz_data[1]) / 32768.0 * 2000.0
+        self.raw_gy = _to_int16(gxyz_data[2], gxyz_data[3]) / 32768.0 * 2000.0
+        self.raw_gz = _to_int16(gxyz_data[4], gxyz_data[5]) / 32768.0 * 2000.0
+        return True
 
     def get_quatern(self):
+        """读取IMU模块直接输出的四元数,成功返回True。"""
         try:
             quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)
-        except IOError:
-            rospy.logerr("Read IMU quaternion date error !")
-        else:
-            self.raw_q0 = float((np.short(np.short(quat_data[1]<<8)|quat_data[0]))/32768.0)
-            self.raw_q1 = float((np.short(np.short(quat_data[3]<<8)|quat_data[2]))/32768.0)
-            self.raw_q2 = float((np.short(np.short(quat_data[5]<<8)|quat_data[4]))/32768.0)
-            self.raw_q3 = float((np.short(np.short(quat_data[7]<<8)|quat_data[6]))/32768.0)
-
-    def get_two_float(self, data, n):
-        data = str(data)
-        a, b, c = data.partition('.')
-        c = (c+"0"*n)[:n]
-        return ".".join([a,c])
+        except OSError:
+            self._log_error("Read IMU quaternion data error !")
+            return False
+
+        self.raw_q0 = _to_int16(quat_data[0], quat_data[1]) / 32768.0
+        self.raw_q1 = _to_int16(quat_data[2], quat_data[3]) / 32768.0
+        self.raw_q2 = _to_int16(quat_data[4], quat_data[5]) / 32768.0
+        self.raw_q3 = _to_int16(quat_data[6], quat_data[7]) / 32768.0
+        return True
 
     def get_temp(self):
+        """读取IMU芯片温度,成功返回True。"""
         try:
-            temp = self.i2c.read_i2c_block_data(self.addr, 0x40, 2)
-        except IOError:
-            rospy.logerr("Read IMU temperature data error !")
-        else:
-            self.temp = float((temp[1]<<8)|temp[0])/100.0
-            #self.temp = float(self.get_two_float(self.temp, 2)) #keep 2 decimal places
+            temp_data = self.i2c.read_i2c_block_data(self.addr, 0x40, 2)
+        except OSError:
+            self._log_error("Read IMU temperature data error !")
+            return False
 
+        self.temp = ((temp_data[1] << 8) | temp_data[0]) / 100.0
+        return True

rasp_imu_hat_6dof/nodes/imu_node.py

@@ -1,139 +1,151 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: UTF-8 -*-
 
-# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
-# Author: corvin
-# Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
-#    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
-#    中读取IMU模块的三轴加速度、角度、四元数，然后组装成ROS
-#    中的IMU消息格式，发布到/imu话题中，这样有需要的节点可以
-#    直接订阅该话题即可获取到imu扩展板当前的数据。
-# History:
-#    20191031:Initial this file.
-#    20191209:新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
-#    20200406:增加可以直接发布yaw数据的话题.
-#    20200410:增加通过service方式发布yaw数据,方便其他节点调用.
-#    20200703:修改三轴线性加速度的方向,沿着各轴正方向数据为正,
-#      否则加速度就为负值,单位m/s2.
-#    20210319:计算四元素时,不用自己使用rpy数据来计算,直接使用imu模块提供的.
-import rospy
+# Copyright: 2016-2026 https://www.corvin.cn ROS小课堂
+# Description: ROS2版IIC IMU节点,发布sensor_msgs/Imu、温度和yaw数据。
+
 import math
 
-from rasp_imu_hat_6dof.srv import GetYawData,GetYawDataResponse
-from dynamic_reconfigure.server import Server
-from rasp_imu_hat_6dof.cfg import imuConfig
-from std_msgs.msg import Float32
+import rclpy
+from rcl_interfaces.msg import SetParametersResult
+from rclpy.node import Node
 from sensor_msgs.msg import Imu
-from imu_data import MyIMU
-
-
-degrees2rad = math.pi/180.0
-imu_yaw_calibration = 0.0
-yaw = 0.0
-
-# ros server return Yaw data
-def return_yaw_data(req):
-    return GetYawDataResponse(yaw)
-
-# Callback for dynamic reconfigure requests
-def reconfig_callback(config, level):
-    global imu_yaw_calibration
-    imu_yaw_calibration = config['yaw_calibration']
-    rospy.loginfo("Set imu_yaw_calibration to %f" % (imu_yaw_calibration))
-    return config
-
-rospy.init_node("imu_node")
-
-# Get DIY config param
-data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
-temp_topic_name = rospy.get_param('~pub_temp_topic', 'imu_temp')
-link_name  = rospy.get_param('~link_name', 'base_imu_link')
-yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
-pub_hz = rospy.get_param('~pub_hz', '20')
-
-data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)
-temp_pub = rospy.Publisher(temp_topic_name, Float32, queue_size=1)
-yaw_pub = rospy.Publisher(yaw_topic_name, Float32, queue_size=1)
-srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
-yaw_srv = rospy.Service('imu_node/GetYawData', GetYawData, return_yaw_data)
-
-imuMsg = Imu()
-yawMsg = Float32()
-
-# Orientation covariance estimation
-imuMsg.orientation_covariance = [
-0.0025 , 0 , 0,
-0, 0.0025, 0,
-0, 0, 0.0025
-]
-
-# Angular velocity covariance estimation
-imuMsg.angular_velocity_covariance = [
-0.02, 0 , 0,
-0 , 0.02, 0,
-0 , 0 , 0.02
-]
-
-# linear acceleration covariance estimation
-imuMsg.linear_acceleration_covariance = [
-0.04 , 0 , 0,
-0 , 0.04, 0,
-0 , 0 , 0.04
-]
-
-seq=0
-# sensor accel g convert to m/s^2.
-accel_factor = 9.806
-
-myIMU = MyIMU(0x50)
-rate = rospy.Rate(pub_hz)
-
-rospy.loginfo("IMU Module is working ...")
-while not rospy.is_shutdown():
-    myIMU.get_YPRAG()
-
-    #rospy.loginfo("yaw:%f pitch:%f roll:%f", myIMU.raw_yaw,
-    #              myIMU.raw_pitch, myIMU.raw_roll)
-    yaw_deg = float(myIMU.raw_yaw)
-    yaw_deg = yaw_deg + imu_yaw_calibration
-    if yaw_deg >= 180.0:
-        yaw_deg -= 360.0
-
-    #rospy.loginfo("yaw_deg: %f", yaw_deg)
-    yaw = yaw_deg*degrees2rad
-    pitch = float(myIMU.raw_pitch)*degrees2rad
-    roll  = float(myIMU.raw_roll)*degrees2rad
-
-    yawMsg.data = yaw
-    yaw_pub.publish(yawMsg)
-
-    # Publish imu message
-    #rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
-    #              myIMU.raw_ay, myIMU.raw_az)
-    imuMsg.linear_acceleration.x = -float(myIMU.raw_ax)*accel_factor
-    imuMsg.linear_acceleration.y = -float(myIMU.raw_ay)*accel_factor
-    imuMsg.linear_acceleration.z = -float(myIMU.raw_az)*accel_factor
-
-    imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
-    imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
-    imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad
-
-    # From IMU module get quatern param
-    myIMU.get_quatern()
-    imuMsg.orientation.x = myIMU.raw_q1
-    imuMsg.orientation.y = myIMU.raw_q2
-    imuMsg.orientation.z = myIMU.raw_q3
-    imuMsg.orientation.w = myIMU.raw_q0
-
-    imuMsg.header.stamp= rospy.Time.now()
-    imuMsg.header.frame_id = link_name
-    imuMsg.header.seq = seq
-    seq = seq + 1
-    data_pub.publish(imuMsg)
-
-    # Get imu module temperature, then publish to topic
-    myIMU.get_temp()
-    temp_pub.publish(myIMU.temp)
-
-    rate.sleep()
+from std_msgs.msg import Float32
 
+from imu_data import MyIMU
+from rasp_imu_hat_6dof.srv import GetYawData
+
+
+DEGREES_TO_RAD = math.pi / 180.0
+ACCEL_FACTOR = 9.806
+
+
+def _normalize_degree(degree):
+    """将角度限制到[-180, 180)范围,避免yaw校准后跳变到异常范围。"""
+    while degree >= 180.0:
+        degree -= 360.0
+    while degree < -180.0:
+        degree += 360.0
+    return degree
+
+
+class RaspImuHatNode(Node):
+    def __init__(self):
+        super().__init__('imu_node')
+
+        self.declare_parameter('iic_bus', 1)
+        self.declare_parameter('iic_addr', 0x50)
+        self.declare_parameter('pub_data_topic', 'imu_data')
+        self.declare_parameter('pub_temp_topic', 'imu_temp')
+        self.declare_parameter('yaw_topic', 'yaw_data')
+        self.declare_parameter('link_name', 'base_imu_link')
+        self.declare_parameter('pub_hz', 20.0)
+        self.declare_parameter('yaw_calibration', 0.0)
+        self.declare_parameter('get_yaw_service', '/imu_node/GetYawData')
+
+        iic_bus = int(self.get_parameter('iic_bus').value)
+        iic_addr = int(self.get_parameter('iic_addr').value)
+        data_topic_name = self.get_parameter('pub_data_topic').value
+        temp_topic_name = self.get_parameter('pub_temp_topic').value
+        yaw_topic_name = self.get_parameter('yaw_topic').value
+        self.link_name = self.get_parameter('link_name').value
+        self.pub_hz = float(self.get_parameter('pub_hz').value)
+        self.yaw_calibration = float(self.get_parameter('yaw_calibration').value)
+        get_yaw_service = self.get_parameter('get_yaw_service').value
+
+        if self.pub_hz <= 0.0:
+            self.get_logger().warn('pub_hz must be > 0, reset to 20.0 Hz')
+            self.pub_hz = 20.0
+
+        self.imu = MyIMU(iic_addr, iic_bus=iic_bus, logger=self.get_logger())
+        self.yaw = 0.0
+
+        self.data_pub = self.create_publisher(Imu, data_topic_name, 10)
+        self.temp_pub = self.create_publisher(Float32, temp_topic_name, 10)
+        self.yaw_pub = self.create_publisher(Float32, yaw_topic_name, 10)
+        self.yaw_srv = self.create_service(GetYawData, get_yaw_service, self._handle_get_yaw)
+        self.add_on_set_parameters_callback(self._on_parameter_update)
+
+        # ROS2用定时器替代ROS1的while + rospy.Rate循环。
+        self.timer = self.create_timer(1.0 / self.pub_hz, self._timer_callback)
+        self.get_logger().info('IMU Module is working ...')
+
+    def _on_parameter_update(self, params):
+        for param in params:
+            if param.name == 'yaw_calibration':
+                self.yaw_calibration = float(param.value)
+                self.get_logger().info(
+                    'Set imu_yaw_calibration to %.3f' % self.yaw_calibration
+                )
+        return SetParametersResult(successful=True)
+
+    def _handle_get_yaw(self, request, response):
+        del request
+        response.yaw = float(self.yaw)
+        return response
+
+    def _make_imu_msg(self):
+        imu_msg = Imu()
+        imu_msg.header.stamp = self.get_clock().now().to_msg()
+        imu_msg.header.frame_id = self.link_name
+
+        imu_msg.orientation_covariance = [
+            0.0025, 0.0, 0.0,
+            0.0, 0.0025, 0.0,
+            0.0, 0.0, 0.0025,
+        ]
+        imu_msg.angular_velocity_covariance = [
+            0.02, 0.0, 0.0,
+            0.0, 0.02, 0.0,
+            0.0, 0.0, 0.02,
+        ]
+        imu_msg.linear_acceleration_covariance = [
+            0.04, 0.0, 0.0,
+            0.0, 0.04, 0.0,
+            0.0, 0.0, 0.04,
+        ]
+
+        # 沿用ROS1版本的坐标方向处理: IIC读取到的加速度在发布前取反。
+        imu_msg.linear_acceleration.x = -float(self.imu.raw_ax) * ACCEL_FACTOR
+        imu_msg.linear_acceleration.y = -float(self.imu.raw_ay) * ACCEL_FACTOR
+        imu_msg.linear_acceleration.z = -float(self.imu.raw_az) * ACCEL_FACTOR
+
+        imu_msg.angular_velocity.x = float(self.imu.raw_gx) * DEGREES_TO_RAD
+        imu_msg.angular_velocity.y = float(self.imu.raw_gy) * DEGREES_TO_RAD
+        imu_msg.angular_velocity.z = float(self.imu.raw_gz) * DEGREES_TO_RAD
+
+        # 四元数直接使用IMU模块输出值,顺序映射为ROS的x/y/z/w。
+        imu_msg.orientation.x = float(self.imu.raw_q1)
+        imu_msg.orientation.y = float(self.imu.raw_q2)
+        imu_msg.orientation.z = float(self.imu.raw_q3)
+        imu_msg.orientation.w = float(self.imu.raw_q0)
+        return imu_msg
+
+    def _timer_callback(self):
+        if not self.imu.get_YPRAG():
+            return
+        if not self.imu.get_quatern():
+            return
+
+        yaw_degree = _normalize_degree(float(self.imu.raw_yaw) + self.yaw_calibration)
+        self.yaw = yaw_degree * DEGREES_TO_RAD
+
+        self.yaw_pub.publish(Float32(data=float(self.yaw)))
+        self.data_pub.publish(self._make_imu_msg())
+
+        if self.imu.get_temp():
+            self.temp_pub.publish(Float32(data=float(self.imu.temp)))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RaspImuHatNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

rasp_imu_hat_6dof/package.xml

@@ -1,29 +1,31 @@
-<package>
+<?xml version="1.0"?>
+<package format="3">
   <name>rasp_imu_hat_6dof</name>
   <version>1.2.0</version>
-  <description>
-     rasp_imu_hat_6dof is a package that provides a ROS driver
-      for the corvin 6dof imu hat.
-  </description>
+  <description>ROS2 driver for the Corvin Raspberry Pi 6DOF IMU HAT.</description>
 
   <maintainer email="corvin_zhang@corvin.cn">Corvin Zhang</maintainer>
-
   <license>BSD</license>
   <author>Corvin Zhang</author>
-
   <url type="website">https://www.corvin.cn</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>std_msgs</build_depend>
-  <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>message_generation</build_depend>
+  <depend>rclpy</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
 
-  <run_depend>rospy</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>message_runtime</run_depend>
-</package>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>python3-smbus</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>rviz_imu_plugin</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

serial_imu_hat_6dof/CMakeLists.txt

@@ -1,187 +1,46 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(serial_imu_hat_6dof)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  dynamic_reconfigure
-  roscpp
-  sensor_msgs
-  std_msgs
-  message_generation
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/GetYawData.srv"
+  "srv/SetBaudRate.srv"
+  "srv/SetPinOutHL.srv"
+  "srv/SetYawZero.srv"
 )
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-# )
-
-## Generate services in the 'srv' folder
-add_service_files(
-  FILES
-  setYawZero.srv
-  setBaudRate.srv
-  setPinOutHL.srv
-  getYawData.srv
+add_executable(serial_imu_node
+  src/proc_serial_data.cpp
+  src/serial_imu_node.cpp
 )
-
-## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
+target_include_directories(serial_imu_node PRIVATE include)
+target_compile_features(serial_imu_node PUBLIC cxx_std_17)
+ament_target_dependencies(serial_imu_node
+  rclcpp
   sensor_msgs
   std_msgs
 )
 
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-  INCLUDE_DIRS include
-#  LIBRARIES serial_imu_hat_6dof
-#  CATKIN_DEPENDS dynamic_reconfigure roscpp sensor_msgs
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
+# 本节点直接使用同包生成的srv类型,需要链接C++ typesupport目标。
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+target_link_libraries(serial_imu_node "${cpp_typesupport_target}")
 
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
+install(TARGETS serial_imu_node
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/serial_imu_hat_6dof.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-add_executable(serial_imu_node src/proc_serial_data.cpp src/serial_imu_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-add_dependencies(serial_imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(serial_imu_node
-  ${catkin_LIBRARIES}
+install(DIRECTORY cfg launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_serial_imu_hat_6dof.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

serial_imu_hat_6dof/cfg/param.yaml

@@ -1,56 +1,20 @@
 ###########################################################
-# Copyright: 2016-2021 www.corvin.cn ROS小课堂
-# Description:使用串口与IMU模块进行通信时的配置信息,可以根据需要修改
-# 可以配置的各参数如下,这些话题名或者服务名,发布频率这些都
-# 可以根据需要来修改,这里需要注意参数名称和参数之间要有空格隔开:
-#  imu_dev:串口设备在linux系统的挂载点,如果使用usb转串口
-#     模块进行通信,都是写ttyUSB0,1,2等.如果使用树莓派的
-#     GPIO排针处串口,则使用ttyAMA0.需要注意是这个ttyAMA0
-#     默认是给树莓派蓝牙使用的,所以要想使用该硬件串口,
-#     就需要在raspi-config中关闭串口登录功能,打开硬件
-#     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
-#  baud_rate:串口通信波特率,默认为57600,如果查看发布的
-#  　　话题中没有IMU数据可以尝试换波特率为9600或115200.
-#  pub_topic_hz:话题发布的数据频率,默认设置为100Hz.
-#  imu_link_name:imu模块的link名,该名称一般在urdf模型中定义.
-#  pub_data_topic:发布imu数据的ROS话题名.
-#  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
-#  yaw_pub_topic:将yaw信息通过该话题发送出来.
-#  pitch_pub_topic:将pitch数据通过该话题发布出来.
-#  roll_pub_topic:将roll数据通过该话题发布出来.
-#  yaw_zero_topic:通过向该话题发布空消息即可将yaw角度归零.
-#  yaw_zero_service:通过向该服务发送空请求即可将yaw归零.
-#  baud_update_srv:通过向该服务发送1,2,3可以将波特率更新为
-#    9600bps,57600bps,115200bps,三种通信波特率.
-#  pin_outHL_srv:控制IMU板上D0-D4的输出电平高低的服务.
-#  yaw_data_srv:可以获取到yaw当前角度的服务.
-# Author: corvin
-# History:
-#   20200402:init this file.
-#   20200406:增加直接发布yaw数据的ROS话题.
-#   20210317:去掉数据位,奇偶校验,停止位参数,这几个
-#     参数模块已经固定死了,无需修改.
-#   20210318:增加yaw角度归零的话题配置参数yaw_zero_topic.
-#     增加使用服务方式将yaw归零的名称yaw_zero_service.
-#   20210319:增加修改串口波特率的配置参数baud_update_srv.
-#   20210321:增加可以设置D0-D4共4个引脚输出数字高低电平的
-#     服务配置参数pin_outHL_srv.增加获取yaw数据的服务.
-#   20210910:更改默认话题发布imu数据的频率为50hz.
-#   20211003:更新发布imu数据话题的默认频率为100hz;
-#     新增发布俯仰角数据的话题参数:pitch_pub_topic;
-#     新增发布横滚角数据的话题参数:roll_pub_topic;
+# ROS2 Humble参数文件。节点名必须与launch里的name保持一致,
+# 参数需要放在ros__parameters下面,否则不会被ROS2加载。
 ###########################################################
-imu_dev: /dev/ttyAMA0
-baud_rate: 57600
-pub_topic_hz: 100
+serial_imu_node:
+  ros__parameters:
+    imu_dev: /dev/ttyAMA0
+    baud_rate: 57600
+    pub_topic_hz: 100.0
 
-imu_link_name: base_imu_link
-pub_data_topic: imu_data
-yaw_pub_topic: yaw_data
-pitch_pub_topic: pitch_data
-roll_pub_topic: roll_data
-yaw_zero_topic: yaw_zero_topic
-yaw_zero_service: yaw_zero_service
-baud_update_srv: baud_update_service
-pin_outHL_srv: pin_outHL_service
-yaw_data_srv: /imu_node/GetYawData
+    imu_link_name: base_imu_link
+    pub_data_topic: imu_data
+    yaw_pub_topic: yaw_data
+    pitch_pub_topic: pitch_data
+    roll_pub_topic: roll_data
+    yaw_zero_topic: yaw_zero_topic
+    yaw_zero_service: yaw_zero_service
+    baud_update_srv: baud_update_service
+    pin_outHL_srv: pin_outHL_service
+    yaw_data_srv: /imu_node/GetYawData

serial_imu_hat_6dof/include/imu_data.h

@@ -1,11 +1,6 @@
 /*******************************************************
- * Copyright:2016-2021 www.corvin.cn ROS小课堂
+ * Copyright:2016-2026 www.corvin.cn ROS小课堂
  * Description:使用串口操作读取imu模块的代码头文件.
- * Author: corvin
- * History:
- *   20210318:init this file.
- *   20210319:增加修改串口通信波特率的函数.
- *   20210321:增加控制IMU引脚数字高低电平的函数.
  *******************************************************/
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
@@ -22,5 +17,6 @@ float getQuat(int flag);
 
 int makeYawZero(void);
 int setIMUBaudRate(const int flag);
-int setIMUPinOutHL(const int d0,const int d1,const int d2,const int d3);
+int setIMUPinOutHL(const int d0, const int d1, const int d2, const int d3);
+
 #endif

serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -1,14 +0,0 @@
-<!--
-  Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
-  Description:使用串口来读取imu模块的数据,并在ros中发布使用话题发布出来.
-  Author: corvin
-  History:
-    20200401: init this file.
--->
-<launch>
-  <arg name="imu_cfg_file" default="$(find serial_imu_hat_6dof)/cfg/param.yaml" />
-
-  <node pkg="serial_imu_hat_6dof" type="serial_imu_node" name="serial_imu_node" output="screen">
-    <rosparam file="$(arg imu_cfg_file)" command="load" />
-  </node>
-</launch>

serial_imu_hat_6dof/launch/serial_imu_hat.launch.py

@@ -0,0 +1,30 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # 默认加载串口IMU的ROS2参数文件。
+    default_config = PathJoinSubstitution([
+        FindPackageShare('serial_imu_hat_6dof'),
+        'cfg',
+        'param.yaml',
+    ])
+
+    config_arg = DeclareLaunchArgument(
+        'imu_cfg_file',
+        default_value=default_config,
+        description='serial_imu_hat_6dof ROS2 parameter file',
+    )
+
+    imu_node = Node(
+        package='serial_imu_hat_6dof',
+        executable='serial_imu_node',
+        name='serial_imu_node',
+        output='screen',
+        parameters=[LaunchConfiguration('imu_cfg_file')],
+    )
+
+    return LaunchDescription([config_arg, imu_node])

serial_imu_hat_6dof/package.xml

@@ -1,69 +1,23 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>serial_imu_hat_6dof</name>
   <version>0.0.1</version>
-  <description>The serial_imu_hat_6dof package</description>
+  <description>Read Raspberry Pi IMU HAT data from serial port and publish ROS2 topics/services.</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <maintainer email="corvin_zhang@corvin.cn">corvin_zhang</maintainer>
-
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
   <license>TODO</license>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/serial_imu_hat_6dof</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>message_generation</build_depend>
-
-  <build_export_depend>dynamic_reconfigure</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>sensor_msgs</build_export_depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
 
-  <exec_depend>dynamic_reconfigure</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
-  <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <!-- Other tools can request additional information be placed here -->
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -1,440 +1,413 @@
 /*******************************************************************
- * Copyright:2016-2022 www.corvin.cn ROS小课堂
+ * Copyright:2016-2026 www.corvin.cn ROS小课堂
  * Description:使用串口方式读取和控制IMU模块信息.
- * Author: corvin
- * History:
- *   20200401:init this file.
- *   20200702:将读取到的各数据的高低自己合成后要转换为short类型,
- *     这样才能进行/32768.0*16.0运算.
- *   20210318:增加可以将yaw角度归零的函数yawZeroCmd().
- *   20210319:增加可以修改串口通信波特率的函数setIMUBaudRate().
- *   20210321:增加控制引脚输出数字高低电平的函数setIMUPinOutHL().
-******************************************************************/
-#include<ros/ros.h>
-#include<stdio.h>
-#include<stdlib.h>
-#include<fcntl.h>
-#include<unistd.h>
-#include<termios.h>
-#include<string.h>
-#include<sys/time.h>
-#include<sys/types.h>
-
-#define   BYTE_CNT      55            //每次从串口中读取的总字节数
-#define   ACCE_CONST    0.000488281   //用于计算加速度的常量16.0/32768.0
-#define   ANGULAR_CONST 0.061035156   //用于计算角速度的常量2000.0/32768.0
-#define   ANGLE_CONST   0.005493164   //用于计算欧拉角的常量180.0/32768.0
-
-static unsigned char r_buf[BYTE_CNT];  //一次从串口中读取的数据存储缓冲区
-static int port_fd = -1; //串口打开时的文件描述符
-static float acce[3],angular[3],angle[3],quater[4];
+ ******************************************************************/
+#include <imu_data.h>
+
+#include <cerrno>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+
+namespace
+{
+constexpr int READ_BUF_SIZE = 256;
+constexpr int FRAME_LEN = 11;
+constexpr float ACCE_CONST = 16.0f / 32768.0f;
+constexpr float ANGULAR_CONST = 2000.0f / 32768.0f;
+constexpr float ANGLE_CONST = 180.0f / 32768.0f;
+
+constexpr unsigned int ACC_MASK = 1 << 0;
+constexpr unsigned int GYRO_MASK = 1 << 1;
+constexpr unsigned int ANGLE_MASK = 1 << 2;
+constexpr unsigned int QUAT_MASK = 1 << 3;
+constexpr unsigned int FULL_DATA_MASK = ACC_MASK | GYRO_MASK | ANGLE_MASK | QUAT_MASK;
+
+int port_fd = -1;
+float acce[3] = {0.0f, 0.0f, 0.0f};
+float angular[3] = {0.0f, 0.0f, 0.0f};
+float angle[3] = {0.0f, 0.0f, 0.0f};
+float quater[4] = {1.0f, 0.0f, 0.0f, 0.0f};
+
+speed_t baud_to_constant(const int baud)
+{
+    switch (baud)
+    {
+        case 9600:
+            return B9600;
+        case 57600:
+            return B57600;
+        case 115200:
+            return B115200;
+        default:
+            return B57600;
+    }
+}
+
+int16_t to_int16(const unsigned char low_byte, const unsigned char high_byte)
+{
+    const uint16_t value = (static_cast<uint16_t>(high_byte) << 8) | low_byte;
+    return static_cast<int16_t>(value);
+}
+
+int write_all(const int fd, const unsigned char* data, const int length)
+{
+    int written = 0;
+    while (written < length)
+    {
+        const ssize_t ret = write(fd, data + written, length - written);
+        if (ret < 0)
+        {
+            if (errno == EINTR)
+            {
+                continue;
+            }
+            return -1;
+        }
+        written += static_cast<int>(ret);
+    }
+    return 0;
+}
+
+bool parse_serial_data(const unsigned char chr)
+{
+    static unsigned char frame[FRAME_LEN];
+    static int frame_pos = 0;
+    static unsigned int parsed_mask = 0;
+
+    // 先同步帧头0x55,串口中偶发杂字节时不会污染后续完整帧。
+    if (frame_pos == 0 && chr != 0x55)
+    {
+        return false;
+    }
+
+    frame[frame_pos++] = chr;
+    if (frame_pos < FRAME_LEN)
+    {
+        return false;
+    }
+
+    unsigned char frame_sum = 0;
+    for (int i = 0; i < FRAME_LEN - 1; ++i)
+    {
+        frame_sum = static_cast<unsigned char>(frame_sum + frame[i]);
+    }
+
+    if (frame[0] != 0x55 || frame_sum != frame[10])
+    {
+        // 校验失败时在当前缓冲中继续寻找下一个0x55,减少重新同步耗时。
+        int next_head = -1;
+        for (int i = 1; i < FRAME_LEN; ++i)
+        {
+            if (frame[i] == 0x55)
+            {
+                next_head = i;
+                break;
+            }
+        }
+
+        if (next_head >= 0)
+        {
+            frame_pos = FRAME_LEN - next_head;
+            std::memmove(frame, frame + next_head, frame_pos);
+        }
+        else
+        {
+            frame_pos = 0;
+        }
+        return false;
+    }
+
+    switch (frame[1])
+    {
+        case 0x51: // x,y,z轴加速度输出,单位g。
+            acce[0] = to_int16(frame[2], frame[3]) * ACCE_CONST;
+            acce[1] = to_int16(frame[4], frame[5]) * ACCE_CONST;
+            acce[2] = to_int16(frame[6], frame[7]) * ACCE_CONST;
+            parsed_mask |= ACC_MASK;
+            break;
+        case 0x52: // x,y,z轴角速度输出,单位deg/s。
+            angular[0] = to_int16(frame[2], frame[3]) * ANGULAR_CONST;
+            angular[1] = to_int16(frame[4], frame[5]) * ANGULAR_CONST;
+            angular[2] = to_int16(frame[6], frame[7]) * ANGULAR_CONST;
+            parsed_mask |= GYRO_MASK;
+            break;
+        case 0x53: // 欧拉角输出,顺序为roll,pitch,yaw,单位deg。
+            angle[0] = to_int16(frame[2], frame[3]) * ANGLE_CONST;
+            angle[1] = to_int16(frame[4], frame[5]) * ANGLE_CONST;
+            angle[2] = to_int16(frame[6], frame[7]) * ANGLE_CONST;
+            parsed_mask |= ANGLE_MASK;
+            break;
+        case 0x59: // 四元数输出,顺序为q0,q1,q2,q3。
+            quater[0] = to_int16(frame[2], frame[3]) / 32768.0f;
+            quater[1] = to_int16(frame[4], frame[5]) / 32768.0f;
+            quater[2] = to_int16(frame[6], frame[7]) / 32768.0f;
+            quater[3] = to_int16(frame[8], frame[9]) / 32768.0f;
+            parsed_mask |= QUAT_MASK;
+            break;
+        default:
+            // 模块可能开启了其他数据帧,这里忽略未知帧,不影响四类IMU数据同步。
+            break;
+    }
+
+    frame_pos = 0;
+    if ((parsed_mask & FULL_DATA_MASK) == FULL_DATA_MASK)
+    {
+        parsed_mask = 0;
+        return true;
+    }
+    return false;
+}
+}  // namespace
 
 /******************************************************
- * Description:打开IMU模块串口,输入两个参数即可连接.
- *   open()打开失败时,返回-1,成功打开时返回文件描述符.
- *   各参数意义如下:
- *   const char *path:IMU设备的挂载地址;
- *   const int baud:串口通讯的波特率;
+ * Description:打开IMU模块串口,输入设备路径和波特率即可连接.
  *****************************************************/
-int openSerialPort(const char *path, const int baud)
+int openSerialPort(const char* path, const int baud)
 {
-    int fd = 0;
-    struct termios terminfo;
-    bzero(&terminfo, sizeof(terminfo));
-
-    fd = open(path, O_RDWR|O_NOCTTY);
-    if (-1 == fd)
+    const int fd = open(path, O_RDWR | O_NOCTTY);
+    if (fd < 0)
     {
-        ROS_ERROR("Open IMU dev error:%s, baud:%d", path, baud);
+        std::fprintf(stderr, "Open IMU dev error:%s, baud:%d, errno:%s\n",
+                     path, baud, std::strerror(errno));
         return -1;
     }
 
-    //判断当前连接的设备是否为终端设备
-    if (isatty(STDIN_FILENO) == 0)
+    if (isatty(fd) == 0)
     {
-        ROS_ERROR("IMU dev isatty error !");
+        std::fprintf(stderr, "IMU dev is not tty:%s\n", path);
+        close(fd);
         return -2;
     }
 
-    /*设置串口通信波特率-bps*/
-    switch(baud)
+    termios terminfo;
+    if (tcgetattr(fd, &terminfo) != 0)
     {
-        case 9600:
-           cfsetispeed(&terminfo, B9600); //设置输入速度
-           cfsetospeed(&terminfo, B9600); //设置输出速度
-           break;
-
-        case 57600:
-           cfsetispeed(&terminfo, B57600);
-           cfsetospeed(&terminfo, B57600);
-           break;
-
-       case 115200:
-           cfsetispeed(&terminfo, B115200);
-           cfsetospeed(&terminfo, B115200);
-           break;
-
-       default: //设置默认波特率9600
-           cfsetispeed(&terminfo, B9600);
-           cfsetospeed(&terminfo, B9600);
-           break;
+        std::fprintf(stderr, "Get imu serial port attr error:%s\n", std::strerror(errno));
+        close(fd);
+        return -3;
     }
 
-    //设置数据位 - 8 bit
-    terminfo.c_cflag |= CLOCAL|CREAD;
+    cfmakeraw(&terminfo);
+    const speed_t baud_const = baud_to_constant(baud);
+    cfsetispeed(&terminfo, baud_const);
+    cfsetospeed(&terminfo, baud_const);
+
+    terminfo.c_cflag |= CLOCAL | CREAD;
     terminfo.c_cflag &= ~CSIZE;
     terminfo.c_cflag |= CS8;
-
-    //不设置奇偶校验位 - N
     terminfo.c_cflag &= ~PARENB;
-
-    //设置停止位 - 1
     terminfo.c_cflag &= ~CSTOPB;
+#ifdef CRTSCTS
+    terminfo.c_cflag &= ~CRTSCTS;
+#endif
+    terminfo.c_iflag &= ~(IXON | IXOFF | IXANY);
 
-    //设置等待时间和最小接收字符
+    // VMIN=1让read在收到至少1字节后立即返回,避免固定读55字节造成额外等待。
     terminfo.c_cc[VTIME] = 0;
-    terminfo.c_cc[VMIN]  = 1;
+    terminfo.c_cc[VMIN] = 1;
 
-    //清除串口缓存的数据
     tcflush(fd, TCIFLUSH);
-
-    if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
+    if (tcsetattr(fd, TCSANOW, &terminfo) != 0)
     {
-        ROS_ERROR("set imu serial port attr error !");
-        return -3;
+        std::fprintf(stderr, "Set imu serial port attr error:%s\n", std::strerror(errno));
+        close(fd);
+        return -4;
     }
 
     return fd;
 }
 
-/**************************************
- * Description:关闭串口文件描述符.
- *************************************/
 int closeSerialPort(void)
 {
-    int ret = close(port_fd);
+    if (port_fd < 0)
+    {
+        return 0;
+    }
+
+    const int ret = close(port_fd);
+    port_fd = -1;
     return ret;
 }
 
 /*****************************************************************
- * Description:向IMU模块的串口中发送数据,第一步就是先解锁,其次才能
- *   发送控制命令,最后就是需要发送命令保存刚才的操作.解锁命令是固
- *   定的0xFF 0xAA 0x69 0x88 0xB5,保存命令也是固定的0xFF 0xAA 0x00
- *   0x00 0x00.
+ * Description:向IMU模块发送控制命令:先解锁,再发送命令,最后保存配置.
  *****************************************************************/
-int send_data(int fd, unsigned char *send_buffer, int length)
+int send_data(int fd, unsigned char* send_buffer, int length)
 {
-    int ret = -1;
-    unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
-    unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
-
-    ret = write(fd, unLockCmd, sizeof(unLockCmd));
-    if(ret != sizeof(unLockCmd))
+    if (fd < 0)
     {
-        ROS_ERROR("Send IMU module unlock cmd error !");
         return -1;
     }
-    ros::Duration(0.01).sleep(); //delay 10ms才能发送其他配置命令
 
-    #if 0 //打印发送给IMU模块的数据,用于调试
-    printf("Send %d byte to IMU:", length);
-    for(int i=0; i<length; i++)
+    unsigned char un_lock_cmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
+    unsigned char save_cmd[5] = {0xFF, 0xAA, 0x00, 0x00, 0x00};
+
+    if (write_all(fd, un_lock_cmd, sizeof(un_lock_cmd)) != 0)
     {
-        printf("0x%02x ", send_buffer[i]);
+        std::fprintf(stderr, "Send IMU module unlock cmd error !\n");
+        return -1;
     }
-    printf("\n");
-    #endif
+    usleep(10 * 1000);
 
-    //发送控制命令
-    ret = write(fd, send_buffer, length*sizeof(unsigned char));
-    if(ret != length)
+    if (write_all(fd, send_buffer, length) != 0)
     {
-        ROS_ERROR("Send IMU module control cmd error !");
+        std::fprintf(stderr, "Send IMU module control cmd error !\n");
         return -2;
     }
 
-    //发送保存命令
-    ret = write(fd, saveCmd, sizeof(saveCmd));
-    if(ret != sizeof(saveCmd))
+    if (write_all(fd, save_cmd, sizeof(save_cmd)) != 0)
     {
-        ROS_ERROR("Send IMU module save cmd error !");
+        std::fprintf(stderr, "Send IMU module save cmd error !\n");
         return -3;
     }
-    ros::Duration(0.1).sleep(); //delay 100ms才能进行其他操作
+    usleep(100 * 1000);
 
     return 0;
 }
 
-/**************************************************************
- * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
- *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
- *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
- *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
- *************************************************************/
-void parse_serialData(unsigned char chr)
+int makeYawZero(void)
 {
-    static unsigned char chrBuf[BYTE_CNT];
-    static unsigned char chrCnt = 0;  //记录读取的第几个字节
-
-    signed short sData[4]; //save 8 Byte有效信息
-    unsigned char i = 0;   //用于for循环
-    unsigned char frameSum = 0;  //存储数据帧的校验和
-
-    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
-
-    //判断是否读取满一个完整数据帧11个字节,若没有则返回不进行解析
-    if(chrCnt < 11)
-        return;
-
-    //读取满完整一帧数据,计算数据帧的前十个字节的校验和,累加即可得到
-    for(i=0; i<10; i++)
-    {
-        frameSum += chrBuf[i];
-    }
-
-    //找到数据帧第一个字节是0x55,同时判断校验和是否正确,若两者有一个不正确,
-    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
-    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
-    {
-        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
-        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
-        return;
-    }
+    unsigned char yaw_zero_cmd[5] = {0xFF, 0xAA, 0x01, 0x04, 0x00};
+    return send_data(port_fd, yaw_zero_cmd, sizeof(yaw_zero_cmd));
+}
 
-    #if 0 //打印出完整的带帧头尾的数据帧
-    for(i=0; i<11; i++)
-      printf("0x%02x ",chrBuf[i]);
-    printf("\n");
-    #endif
+int setIMUBaudRate(const int flag)
+{
+    unsigned char baud_rate_cmd[5] = {0xFF, 0xAA, 0x04, 0x00, 0x00};
 
-    memcpy(&sData[0], &chrBuf[2], 8);
-    switch(chrBuf[1])  //根据数据帧不同类型来进行解析数据
+    switch (flag)
     {
-        case 0x51: //x,y,z轴 加速度输出
-            acce[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ACCE_CONST;
-            acce[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ACCE_CONST;
-            acce[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ACCE_CONST;
-            break;
-        case 0x52: //角速度输出
-            angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGULAR_CONST;
-            angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGULAR_CONST;
-            angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGULAR_CONST;
+        case 1:
+            baud_rate_cmd[3] = 0x02; // 9600 bps
             break;
-        case 0x53: //欧拉角度输出, roll, pitch, yaw
-            angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGLE_CONST;
-            angle[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGLE_CONST;
-            angle[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGLE_CONST;
+        case 2:
+            baud_rate_cmd[3] = 0x05; // 57600 bps
             break;
-        case 0x59: //四元素输出
-            quater[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0;
-            quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
-            quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
-            quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
+        case 3:
+            baud_rate_cmd[3] = 0x06; // 115200 bps
             break;
         default:
-            ROS_ERROR("parse imu data frame type error !");
+            baud_rate_cmd[3] = 0x05; // 默认57600 bps
             break;
     }
-    chrCnt = 0;
+
+    return send_data(port_fd, baud_rate_cmd, sizeof(baud_rate_cmd));
 }
 
-/********************************************************************
- * Description:将yaw角度归零,这里需要通过串口发送的命令如下所示,
- *  发送z轴角度归零的固定命令:0xFF 0xAA 0x01 0x04 0x00;
- *******************************************************************/
-int makeYawZero(void)
+int setIMUPinOutHL(const int d0, const int d1, const int d2, const int d3)
 {
-    int ret = 0;
-    unsigned char yawZeroCmd[5] = {0xFF, 0xAA, 0x01, 0x04, 0x00};
+    unsigned char pin_d0_cmd[5] = {0xFF, 0xAA, 0x0E, 0x03, 0x00};
+    unsigned char pin_d1_cmd[5] = {0xFF, 0xAA, 0x0F, 0x03, 0x00};
+    unsigned char pin_d2_cmd[5] = {0xFF, 0xAA, 0x10, 0x03, 0x00};
+    unsigned char pin_d3_cmd[5] = {0xFF, 0xAA, 0x11, 0x03, 0x00};
 
-    ret = send_data(port_fd, yawZeroCmd, sizeof(yawZeroCmd));
-    if(ret != 0) //通过串口发送命令失败
+    if (d0 == 1)
     {
-        ROS_ERROR("Send yaw zero control cmd error !");
-        return -1;
+        pin_d0_cmd[3] = 0x02;
     }
-    ROS_INFO("set yaw to zero radian successfully !");
-
-    return 0;
-}
-
-/******************************************************************
- * Description:更新串口通信波特率,根据输入参数的不同,修改为不同的波特率,
- *   1修改为9600,2修改为57600,3修改为115200.修改波特率就是发送3条指令,
- *   第一条命令是解除锁定,然后是发送串口波特率更新命令,最后是保存操作的指令.
- ******************************************************************/
-int setIMUBaudRate(const int flag)
-{
-    int ret = 0;
-    int baud = 0;
-    unsigned char baudRate[5] = {0xFF, 0xAA, 0x04, 0x00, 0x00};
-
-    switch(flag)
+    if (d1 == 1)
     {
-        case 1: //set baud 9600 bps
-            baudRate[3] = 0x02;
-            baud = 9600;
-            break;
-        case 2: //set baud 57600 bps
-            baudRate[3] = 0x05;
-            baud = 57600;
-            break;
-        case 3: //set baud 115200 bps
-            baudRate[3] = 0x06;
-            baud = 115200;
-            break;
-        default: //default set baud 57600 bps
-            baudRate[3] = 0x05;
-            baud = 57600;
-            break;
+        pin_d1_cmd[3] = 0x02;
     }
-    ret = send_data(port_fd, baudRate, sizeof(baudRate));
-    if(ret != 0)
+    if (d2 == 1)
     {
-        ROS_ERROR("Send baud rate update cmd error !");
-        return -1;
+        pin_d2_cmd[3] = 0x02;
+    }
+    if (d3 == 1)
+    {
+        pin_d3_cmd[3] = 0x02;
     }
-    ROS_INFO("Set imu new baud rate:[ %d bps] successfully !", baud);
-    ROS_INFO("Please reconnect IMU module with new baud !");
-
-    return 0;
-}
 
-/*********************************************************************
- * Description:要想控制引脚输出数字高低电平,其实就是发送固定的控制命令,
- *   这里的高低电平的控制字节是命令中的第4个字节,0x03表明是输出低电平,
- *   要想输出高电平就是将该字节改为0x02即可.剩下就是依次发送这4个引脚
- *   的控制命令就可以了.
- ********************************************************************/
-int setIMUPinOutHL(const int d0,const int d1,const int d2,const int d3)
-{
-    int ret = -1;
-    unsigned char pinD0CtlCmd[5] = {0xFF, 0xAA, 0x0E, 0x03, 0x00};
-    unsigned char pinD1CtlCmd[5] = {0xFF, 0xAA, 0x0F, 0x03, 0x00};
-    unsigned char pinD2CtlCmd[5] = {0xFF, 0xAA, 0x10, 0x03, 0x00};
-    unsigned char pinD3CtlCmd[5] = {0xFF, 0xAA, 0x11, 0x03, 0x00};
-
-    if(d0 == 1)
-        pinD0CtlCmd[3] = 0x02;
-    if(d1 == 1)
-        pinD1CtlCmd[3] = 0x02;
-    if(d2 == 1)
-        pinD2CtlCmd[3] = 0x02;
-    if(d3 == 1)
-        pinD3CtlCmd[3] = 0x02;
-
-    ret = send_data(port_fd, pinD0CtlCmd, sizeof(pinD0CtlCmd));
-    if(ret != 0)
+    if (send_data(port_fd, pin_d0_cmd, sizeof(pin_d0_cmd)) != 0)
     {
-        ROS_ERROR("Send pin D0 control cmd error !");
         return -1;
     }
-    ret = send_data(port_fd, pinD1CtlCmd, sizeof(pinD1CtlCmd));
-    if(ret != 0)
+    if (send_data(port_fd, pin_d1_cmd, sizeof(pin_d1_cmd)) != 0)
     {
-        ROS_ERROR("Send pin D1 control cmd error !");
         return -2;
     }
-    ret = send_data(port_fd, pinD2CtlCmd, sizeof(pinD2CtlCmd));
-    if(ret != 0)
+    if (send_data(port_fd, pin_d2_cmd, sizeof(pin_d2_cmd)) != 0)
     {
-        ROS_ERROR("Send pin D2 control cmd error !");
         return -3;
     }
-    ret = send_data(port_fd, pinD3CtlCmd, sizeof(pinD3CtlCmd));
-    if(ret != 0)
+    if (send_data(port_fd, pin_d3_cmd, sizeof(pin_d3_cmd)) != 0)
     {
-        ROS_ERROR("Send pin D3 control cmd error !");
         return -4;
     }
 
     return 0;
 }
 
-/*****************************************************************
- * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
- *   两个输入参数的意义如下:
- *   const char* path:IMU设备的挂载地址;
- *   const int baud:IMU模块的串口通信波特率;
- ****************************************************************/
 int initSerialPort(const char* path, const int baud)
 {
     port_fd = openSerialPort(path, baud);
-    if(port_fd < 0) //打开IMU模块串口失败
+    if (port_fd < 0)
     {
-        ROS_ERROR("Open IMU Module Serial Port error !");
+        std::fprintf(stderr, "Open IMU Module Serial Port error !\n");
         return -1;
     }
-
     return 0;
 }
 
-/*****************************************
- * Description:得到三轴加速度信息,输入参数可以
- * 　为0,1,2分别代表x,y,z轴的加速度信息.
- *****************************************/
 float getAcc(int flag)
 {
     return acce[flag];
 }
 
-/******************************************
- * Description:得到角速度信息,输入参数可以为
- * 　0,1,2,分别代表x,y,z三轴的角速度信息.
- *****************************************/
 float getAngular(int flag)
 {
     return angular[flag];
 }
 
-/********************************************
- * Description:获取yaw,pitch,roll,输入参数0,
- * 1,2可以分别获取到roll,pitch,yaw的数据.
- *******************************************/
 float getAngle(int flag)
 {
     return angle[flag];
 }
 
-/********************************************
- * Description:输入参数0,1,2,3可以分别获取到
- * 　四元素的q0,q1,q2,q3.但是这里对应的ros中
- * 　的imu_msg.orientation.w,x,y,z的顺序,
- * 　这里的q0是对应w参数,1,2,3对应x,y,z.
- ********************************************/
 float getQuat(int flag)
 {
     return quater[flag];
 }
 
-/*************************************************************
- * Description:从串口读取数据,然后解析出各有效数据段,这里
- *  一次性从串口中读取55个字节,然后需要从这些字节中进行
- *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
- *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
- *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
- *************************************************************/
 int getImuData(void)
 {
-    int data_len = 0;
-    bzero(r_buf, sizeof(r_buf)); //存储新数据前,将缓冲区清零
+    static unsigned char read_buf[READ_BUF_SIZE];
+    static int buf_pos = 0;
+    static int buf_len = 0;
 
-    //开始从串口中读取数据,并将其保存到r_buf缓冲区中
-    data_len = read(port_fd, r_buf, sizeof(r_buf));
-    if(data_len <= 0) //从串口中读取数据失败
+    while (true)
     {
-        ROS_ERROR("read serial port data failed !\n");
-        closeSerialPort();
-        return -1;
-    }
-
-    //printf("recv data len:%d\n", data_len); //一次性从串口中读取的总数据字节数
-    for (int i=0; i<data_len; i++) //将读取到的数据进行解析
-    {
-        //printf("0x%02x ", r_buf[i]);
-        parse_serialData(r_buf[i]);
+        if (buf_pos >= buf_len)
+        {
+            const ssize_t data_len = read(port_fd, read_buf, sizeof(read_buf));
+            if (data_len < 0)
+            {
+                if (errno == EINTR)
+                {
+                    continue;
+                }
+                std::fprintf(stderr, "Read serial port data failed:%s\n", std::strerror(errno));
+                return -1;
+            }
+            if (data_len == 0)
+            {
+                continue;
+            }
+            buf_pos = 0;
+            buf_len = static_cast<int>(data_len);
+        }
+
+        while (buf_pos < buf_len)
+        {
+            if (parse_serial_data(read_buf[buf_pos++]))
+            {
+                // 已凑齐一组IMU数据,立即返回给ROS发布层,剩余字节保留到下次继续解析。
+                return 0;
+            }
+        }
     }
-    //printf("\n\n");
-
-    return 0;
 }

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,215 +1,274 @@
 /*****************************************************************
- * Copyright:2016-2022 www.corvin.cn ROS小课堂
- * Description:使用串口来读取IMU模块的数据,并通过ROS话题topic将
- *   数据发布出来.芯片中默认读取出来的加速度数据单位是g,需要将其
- *   转换为ROS中加速度规定的m/s2才能发布.
- * Author: corvin
- * History:
- *   20200403:init this file.
- *   20200406:增加发布yaw数据的话题.
- *   20200703:修改三轴加速度数据正负,按照ROS规定沿着各轴
- *     正方向时数据为正,否则为负.单位m/s2。
- *   20210317:去掉从配置文件中读取的停止位,数据位这些无法
- *     修改的配置参数,因为都是固定的,无需自己修改配置.
- *   20210318:增加订阅话题,话题消息格式Empty,可以将yaw角度归零.
- *     增加使用service方式可将yaw角度归零.
- *   20210319:增加使用service方式可以修改imu模块串口通信波特率.
- *     注意修改了波特率后,需要将IMU模块短开使用新波特率连接.
- *   20210321:增加可以设置D0,D1,D2,D3共4个IO引脚输出数字高低电平
- *     的服务,这里的高电平为3.3V.增加通过服务方式获取yaw数据.
- *   20211003:增加发布pitch和roll数据的话题.
- *   20220220:修改三轴加速度数据正负值,ROS中使用ENU坐标系,规定Z轴线
- *     加速度的正方向为下,所以IMU静止时G为正值.
-*****************************************************************/
-#include <ros/ros.h>
-#include <tf/tf.h>
+ * Copyright:2016-2026 www.corvin.cn ROS小课堂
+ * Description: ROS2版串口IMU节点,读取IMU模块数据并发布话题/服务.
+ ****************************************************************/
 #include <imu_data.h>
-#include <sensor_msgs/Imu.h>
-#include <std_msgs/Float32.h>
-#include <std_msgs/Empty.h>
-#include "serial_imu_hat_6dof/setYawZero.h"
-#include "serial_imu_hat_6dof/setBaudRate.h"
-#include "serial_imu_hat_6dof/setPinOutHL.h"
-#include "serial_imu_hat_6dof/getYawData.h"
-
-static float g_yawData; //全局变量,存储当前yaw值,可以通过服务来得到该值
-
-/***********************************************************
- * Description:将yaw角度归零的话题回调函数,只要往话题中发布一条
- *   std_msgs::Empty类型消息,即可将yaw角度归零.
- **********************************************************/
-void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
-{
-    makeYawZero();
-}
 
-/******************************************************************
- * Description:使用service方式来进行yaw角度归零,这里是服务的回调函数,
- *   当有客户端发送yaw归零的服务时,自动调用该函数,如果正确执行了yaw归零,
- *   则response反馈为0,如果为其他负数则表明yaw归零命令执行失败.
- ******************************************************************/
-bool yawZeroService(serial_imu_hat_6dof::setYawZero::Request &req,
-                    serial_imu_hat_6dof::setYawZero::Response &res)
-{
-    res.status = makeYawZero();
-    return true;
-}
+#include <functional>
+#include <memory>
+#include <string>
 
-/********************************************************************
- * Description:使用服务调用方式来修改与IMU模块通信的波特率,这里根据
- *   客户端发送过来的request来决定修改的波特率,请求内容对应的波特率:
- *   请求发送1,修改波特率为9600;
- *   请求发送2,修改波特率为57600;
- *   请求发送3,修改波特率为115200;
- *   当修改IMU模块的波特率成功后函数会返回0,若返回其他负值,则表示
- *   修改波特率失败,可以重新调用服务来修改波特率.
- *******************************************************************/
-bool setBaudService(serial_imu_hat_6dof::setBaudRate::Request &req,
-                    serial_imu_hat_6dof::setBaudRate::Response &res)
-{
-    res.status = setIMUBaudRate(req.index);
-    return true;
-}
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/float32.hpp>
 
-/********************************************************************
- * Description:控制IMU板的D0,D1,D2,D3共4个引脚的输出数字高低电平信号,
- *   这里的服务request一次性控制4个引脚的状态,对应引脚0,则输出低电平,
- *   请求为1,则输出高电平.根据反馈状态status,可以得知控制的结果,如果
- *   反馈0,表明控制成功,负值的话控制失败.这里的高电平为3.3V.
- ********************************************************************/
-bool pinOutHLService(serial_imu_hat_6dof::setPinOutHL::Request &req,
-                     serial_imu_hat_6dof::setPinOutHL::Response &res)
-{
-    res.status = setIMUPinOutHL(req.D0, req.D1, req.D2, req.D3);
-    return true;
-}
+#include "serial_imu_hat_6dof/srv/get_yaw_data.hpp"
+#include "serial_imu_hat_6dof/srv/set_baud_rate.hpp"
+#include "serial_imu_hat_6dof/srv/set_pin_out_hl.hpp"
+#include "serial_imu_hat_6dof/srv/set_yaw_zero.hpp"
 
-/**********************************************************************
- * Description:通过service服务调用方式来获取到yaw角度信息.
- **********************************************************************/
-bool getYawDataService(serial_imu_hat_6dof::getYawData::Request &req,
-                       serial_imu_hat_6dof::getYawData::Response &res)
+namespace
 {
-    res.yaw = g_yawData;
-    return true;
-}
+constexpr float DEGREE_TO_RAD = 0.017453292519943295f;
+constexpr float ACC_FACTOR = 9.806f;
+}  // namespace
 
-int main(int argc, char **argv)
+class SerialImuHatNode : public rclcpp::Node
 {
-    float yaw, pitch, roll;
-    std::string imu_dev;
-    int baud = 0;
-
-    std::string imu_link_name;
-    std::string imu_topic_name;
-    std::string yaw_pub_topic;
-    std::string pitch_pub_topic;
-    std::string roll_pub_topic;
-    std::string yaw_zero_topic;
-    std::string yaw_zero_service;
-    std::string baud_update_service;
-    std::string pin_outHL_service;
-    std::string yaw_data_service;
-
-    int pub_topic_hz = 0;  //话题发布imu数据的频率
-    const float degree2Rad = 0.017453292; //3.1415926/180.0 角度转换为弧度的系数
-    const float acc_factor = 9.806; //重力加速度常量
-
-    ros::init(argc, argv, "imu_data_pub_node");
-    ros::NodeHandle handle;
-
-    //launch文件中加载yaml配置文件，然后从配置文件中读取参数
-    ros::param::get("~imu_dev",          imu_dev);
-    ros::param::get("~baud_rate",        baud);
-    ros::param::get("~imu_link_name",    imu_link_name);
-    ros::param::get("~pub_topic_hz",     pub_topic_hz);
-    ros::param::get("~pub_data_topic",   imu_topic_name);
-    ros::param::get("~yaw_zero_topic",   yaw_zero_topic);
-    ros::param::get("~yaw_zero_service", yaw_zero_service);
-    ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
-    ros::param::get("~pitch_pub_topic",  pitch_pub_topic);
-    ros::param::get("~roll_pub_topic",   roll_pub_topic);
-    ros::param::get("~baud_update_srv",  baud_update_service);
-    ros::param::get("~pin_outHL_srv",    pin_outHL_service);
-    ros::param::get("~yaw_data_srv",     yaw_data_service);
-
-    //初始化imu模块串口,根据设备号和波特率建立连接
-    int ret = initSerialPort(imu_dev.c_str(), baud);
-    if(ret < 0) //通过串口连接IMU模块失败
+public:
+    SerialImuHatNode()
+    : Node("serial_imu_node")
+    {
+        declare_parameter<std::string>("imu_dev", "/dev/ttyAMA0");
+        declare_parameter<int>("baud_rate", 57600);
+        declare_parameter<double>("pub_topic_hz", 100.0);
+        declare_parameter<std::string>("imu_link_name", "base_imu_link");
+        declare_parameter<std::string>("pub_data_topic", "imu_data");
+        declare_parameter<std::string>("yaw_pub_topic", "yaw_data");
+        declare_parameter<std::string>("pitch_pub_topic", "pitch_data");
+        declare_parameter<std::string>("roll_pub_topic", "roll_data");
+        declare_parameter<std::string>("yaw_zero_topic", "yaw_zero_topic");
+        declare_parameter<std::string>("yaw_zero_service", "yaw_zero_service");
+        declare_parameter<std::string>("baud_update_srv", "baud_update_service");
+        declare_parameter<std::string>("pin_outHL_srv", "pin_outHL_service");
+        declare_parameter<std::string>("yaw_data_srv", "/imu_node/GetYawData");
+
+        imu_dev_ = get_parameter("imu_dev").as_string();
+        baud_rate_ = static_cast<int>(get_parameter("baud_rate").as_int());
+        pub_topic_hz_ = get_parameter("pub_topic_hz").as_double();
+        imu_link_name_ = get_parameter("imu_link_name").as_string();
+        const std::string imu_topic_name = get_parameter("pub_data_topic").as_string();
+        const std::string yaw_pub_topic = get_parameter("yaw_pub_topic").as_string();
+        const std::string pitch_pub_topic = get_parameter("pitch_pub_topic").as_string();
+        const std::string roll_pub_topic = get_parameter("roll_pub_topic").as_string();
+        const std::string yaw_zero_topic = get_parameter("yaw_zero_topic").as_string();
+        const std::string yaw_zero_service = get_parameter("yaw_zero_service").as_string();
+        const std::string baud_update_service = get_parameter("baud_update_srv").as_string();
+        const std::string pin_out_hl_service = get_parameter("pin_outHL_srv").as_string();
+        const std::string yaw_data_service = get_parameter("yaw_data_srv").as_string();
+
+        if (pub_topic_hz_ <= 0.0)
+        {
+            RCLCPP_WARN(get_logger(), "pub_topic_hz must be > 0, reset to 100.0 Hz");
+            pub_topic_hz_ = 100.0;
+        }
+
+        serial_ready_ = (initSerialPort(imu_dev_.c_str(), baud_rate_) == 0);
+        if (!serial_ready_)
+        {
+            RCLCPP_ERROR(get_logger(), "init IMU module serial port error !");
+            closeSerialPort();
+            return;
+        }
+        RCLCPP_INFO(get_logger(), "Now IMU module is working...");
+
+        imu_pub_ = create_publisher<sensor_msgs::msg::Imu>(imu_topic_name, 10);
+        yaw_pub_ = create_publisher<std_msgs::msg::Float32>(yaw_pub_topic, 10);
+        pitch_pub_ = create_publisher<std_msgs::msg::Float32>(pitch_pub_topic, 10);
+        roll_pub_ = create_publisher<std_msgs::msg::Float32>(roll_pub_topic, 10);
+
+        // 话题方式将yaw归零: ros2 topic pub --once /yaw_zero_topic std_msgs/msg/Empty "{}"
+        yaw_zero_sub_ = create_subscription<std_msgs::msg::Empty>(
+            yaw_zero_topic,
+            10,
+            std::bind(&SerialImuHatNode::yaw_zero_callback, this, std::placeholders::_1));
+
+        yaw_zero_srv_ = create_service<serial_imu_hat_6dof::srv::SetYawZero>(
+            yaw_zero_service,
+            std::bind(&SerialImuHatNode::handle_yaw_zero, this,
+                      std::placeholders::_1, std::placeholders::_2));
+        baud_srv_ = create_service<serial_imu_hat_6dof::srv::SetBaudRate>(
+            baud_update_service,
+            std::bind(&SerialImuHatNode::handle_set_baud, this,
+                      std::placeholders::_1, std::placeholders::_2));
+        pin_out_srv_ = create_service<serial_imu_hat_6dof::srv::SetPinOutHL>(
+            pin_out_hl_service,
+            std::bind(&SerialImuHatNode::handle_pin_out, this,
+                      std::placeholders::_1, std::placeholders::_2));
+        yaw_data_srv_ = create_service<serial_imu_hat_6dof::srv::GetYawData>(
+            yaw_data_service,
+            std::bind(&SerialImuHatNode::handle_get_yaw, this,
+                      std::placeholders::_1, std::placeholders::_2));
+    }
+
+    ~SerialImuHatNode() override
     {
-        ROS_ERROR("init IMU module serial port error !");
         closeSerialPort();
-        return -1;
     }
-    ROS_INFO("Now IMU module is working...");
-
-    //定义四个服务,分别是更新波特率,yaw角度归零,控制引脚输出高低电平和获取yaw当前角度
-    ros::ServiceServer setBaudSrv= handle.advertiseService(baud_update_service, setBaudService);
-    ros::ServiceServer setyawSrv = handle.advertiseService(yaw_zero_service,  yawZeroService);
-    ros::ServiceServer pinOutSrv = handle.advertiseService(pin_outHL_service, pinOutHLService);
-    ros::ServiceServer getYawSrv = handle.advertiseService(yaw_data_service,  getYawDataService);
-
-    ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
-    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic_name, 2);
-    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 2);
-    ros::Publisher pitch_pub = handle.advertise<std_msgs::Float32>(pitch_pub_topic, 2);
-    ros::Publisher roll_pub  = handle.advertise<std_msgs::Float32>(roll_pub_topic, 2);
-    ros::Rate loop_rate(pub_topic_hz);
-
-    sensor_msgs::Imu imu_msg;
-    imu_msg.header.frame_id = imu_link_name;
-    std_msgs::Float32 yaw_msg;
-    std_msgs::Float32 pitch_msg;
-    std_msgs::Float32 roll_msg;
-    while(ros::ok())
+
+    bool initialized() const
     {
-        if(getImuData() < 0)
-            break;
+        return serial_ready_;
+    }
+
+    double publish_rate_hz() const
+    {
+        return pub_topic_hz_;
+    }
+
+    bool read_and_publish_once()
+    {
+        if (!serial_ready_)
+        {
+            return false;
+        }
+
+        if (getImuData() < 0)
+        {
+            RCLCPP_ERROR(get_logger(), "read imu serial data failed !");
+            closeSerialPort();
+            serial_ready_ = false;
+            return false;
+        }
+
+        sensor_msgs::msg::Imu imu_msg;
+        imu_msg.header.stamp = get_clock()->now();
+        imu_msg.header.frame_id = imu_link_name_;
 
-        imu_msg.header.stamp = ros::Time::now();
-        roll  = getAngle(0)*degree2Rad;
-        pitch = getAngle(1)*degree2Rad;
-        yaw   = getAngle(2)*degree2Rad;
-        if(yaw >= 3.1415926)
-            yaw -= 6.2831852;
+        float roll = getAngle(0) * DEGREE_TO_RAD;
+        float pitch = getAngle(1) * DEGREE_TO_RAD;
+        float yaw = getAngle(2) * DEGREE_TO_RAD;
+        if (yaw >= 3.1415926f)
+        {
+            yaw -= 6.2831852f;
+        }
+        yaw_data_ = yaw;
 
-        g_yawData = yaw; //获取yaw值,可以通过服务来得到该值
-        yaw_msg.data   = yaw;
+        std_msgs::msg::Float32 yaw_msg;
+        std_msgs::msg::Float32 pitch_msg;
+        std_msgs::msg::Float32 roll_msg;
+        yaw_msg.data = yaw;
         pitch_msg.data = pitch;
-        roll_msg.data  = roll;
-        yaw_pub.publish(yaw_msg);     //将yaw值通过话题发布出去
-        pitch_pub.publish(pitch_msg); //将pitch值通过话题发布出去
-        roll_pub.publish(roll_msg);   //将roll值通过话题发布出去
+        roll_msg.data = roll;
+        yaw_pub_->publish(yaw_msg);
+        pitch_pub_->publish(pitch_msg);
+        roll_pub_->publish(roll_msg);
 
-        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
+        // IMU模块四元数顺序是q0,q1,q2,q3,ROS消息顺序是x,y,z,w。
         imu_msg.orientation.x = getQuat(1);
         imu_msg.orientation.y = getQuat(2);
         imu_msg.orientation.z = getQuat(3);
         imu_msg.orientation.w = getQuat(0);
-        imu_msg.orientation_covariance = {0.0025, 0, 0,
-                                          0, 0.0025, 0,
-                                          0, 0, 0.0025};
-        //三轴角速度数据
-        imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
-        imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
-        imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
-        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
-                                               0, 0.02, 0,
-                                               0, 0, 0.02};
-
-        //三轴线加速度数据,这里都为正值
-        imu_msg.linear_acceleration.x = getAcc(0)*acc_factor;
-        imu_msg.linear_acceleration.y = getAcc(1)*acc_factor;
-        imu_msg.linear_acceleration.z = getAcc(2)*acc_factor;
-        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
-                                                   0, 0.04, 0,
-                                                   0, 0, 0.04};
-
-        imu_pub.publish(imu_msg); //将imu数据通过话题发布出去
-        ros::spinOnce();
+        imu_msg.orientation_covariance = {
+            0.0025, 0.0, 0.0,
+            0.0, 0.0025, 0.0,
+            0.0, 0.0, 0.0025,
+        };
+
+        imu_msg.angular_velocity.x = getAngular(0) * DEGREE_TO_RAD;
+        imu_msg.angular_velocity.y = getAngular(1) * DEGREE_TO_RAD;
+        imu_msg.angular_velocity.z = getAngular(2) * DEGREE_TO_RAD;
+        imu_msg.angular_velocity_covariance = {
+            0.02, 0.0, 0.0,
+            0.0, 0.02, 0.0,
+            0.0, 0.0, 0.02,
+        };
+
+        // 串口版沿用原ROS1代码的ENU坐标处理: z轴静止时G为正。
+        imu_msg.linear_acceleration.x = getAcc(0) * ACC_FACTOR;
+        imu_msg.linear_acceleration.y = getAcc(1) * ACC_FACTOR;
+        imu_msg.linear_acceleration.z = getAcc(2) * ACC_FACTOR;
+        imu_msg.linear_acceleration_covariance = {
+            0.04, 0.0, 0.0,
+            0.0, 0.04, 0.0,
+            0.0, 0.0, 0.04,
+        };
+
+        imu_pub_->publish(imu_msg);
+        return true;
+    }
+
+private:
+    void yaw_zero_callback(const std_msgs::msg::Empty::SharedPtr msg)
+    {
+        (void)msg;
+        const int ret = makeYawZero();
+        if (ret != 0)
+        {
+            RCLCPP_ERROR(get_logger(), "set yaw to zero failed, status:%d", ret);
+        }
+    }
+
+    void handle_yaw_zero(
+        const std::shared_ptr<serial_imu_hat_6dof::srv::SetYawZero::Request> request,
+        std::shared_ptr<serial_imu_hat_6dof::srv::SetYawZero::Response> response)
+    {
+        (void)request;
+        response->status = makeYawZero();
+    }
+
+    void handle_set_baud(
+        const std::shared_ptr<serial_imu_hat_6dof::srv::SetBaudRate::Request> request,
+        std::shared_ptr<serial_imu_hat_6dof::srv::SetBaudRate::Response> response)
+    {
+        response->status = setIMUBaudRate(request->index);
+    }
+
+    void handle_pin_out(
+        const std::shared_ptr<serial_imu_hat_6dof::srv::SetPinOutHL::Request> request,
+        std::shared_ptr<serial_imu_hat_6dof::srv::SetPinOutHL::Response> response)
+    {
+        response->status = setIMUPinOutHL(request->d0, request->d1, request->d2, request->d3);
+    }
+
+    void handle_get_yaw(
+        const std::shared_ptr<serial_imu_hat_6dof::srv::GetYawData::Request> request,
+        std::shared_ptr<serial_imu_hat_6dof::srv::GetYawData::Response> response)
+    {
+        (void)request;
+        response->yaw = yaw_data_;
+    }
+
+    std::string imu_dev_;
+    int baud_rate_ = 57600;
+    double pub_topic_hz_ = 100.0;
+    std::string imu_link_name_;
+    float yaw_data_ = 0.0f;
+    bool serial_ready_ = false;
+
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr yaw_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pitch_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr roll_pub_;
+    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr yaw_zero_sub_;
+    rclcpp::Service<serial_imu_hat_6dof::srv::SetYawZero>::SharedPtr yaw_zero_srv_;
+    rclcpp::Service<serial_imu_hat_6dof::srv::SetBaudRate>::SharedPtr baud_srv_;
+    rclcpp::Service<serial_imu_hat_6dof::srv::SetPinOutHL>::SharedPtr pin_out_srv_;
+    rclcpp::Service<serial_imu_hat_6dof::srv::GetYawData>::SharedPtr yaw_data_srv_;
+};
+
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<SerialImuHatNode>();
+    if (!node->initialized())
+    {
+        rclcpp::shutdown();
+        return 1;
+    }
+
+    rclcpp::Rate loop_rate(node->publish_rate_hz());
+    while (rclcpp::ok())
+    {
+        if (!node->read_and_publish_once())
+        {
+            break;
+        }
+        rclcpp::spin_some(node);
         loop_rate.sleep();
     }
 
-    closeSerialPort(); //关闭与IMU模块的串口连接
+    rclcpp::shutdown();
     return 0;
 }

serial_imu_hat_6dof/srv/SetPinOutHL.srv

@@ -0,0 +1,6 @@
+int32 d0
+int32 d1
+int32 d2
+int32 d3
+---
+int32 status

serial_imu_hat_6dof/srv/setPinOutHL.srv

@@ -1,6 +0,0 @@
-int32 D0
-int32 D1
-int32 D2
-int32 D3
----
-int32 status