-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
project(imufusion_ros)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
-## 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
- eigen_conversions
- geometry_msgs
-# imufusion
- roscpp
- sensor_msgs
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-add_library(imufusion STATIC IMPORTED) # or STATIC instead of SHARED
-set_target_properties(imufusion PROPERTIES
- IMPORTED_LOCATION "$ENV{HOME}/lib/libimufusion.a"
- INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/include"
-)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## 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 add_*_files sections below as needed
-## and list every .msg/.srv/.action file to be processed
-## * 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
-# Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-# FILES
-# Action1.action
-# Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# geometry_msgs# sensor_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
-# cfg/DynReconf2.cfg
-# )
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(geometry_msgs REQUIRED)
+#find_package(imufusion REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
-###################################
-## 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 imufusion_ros
-# CATKIN_DEPENDS eigen_conversions geometry_msgs imufusion roscpp sensor_msgs
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/imufusion_ros.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(ekf src/ekf.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(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(ekf
- ${catkin_LIBRARIES}
- imufusion
+target_include_directories(ekf PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+ $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+target_compile_features(ekf PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
+ament_target_dependencies(
+ ekf
+ "rclcpp"
+ "tf2_eigen"
+ "geometry_msgs"
+# "imufusion"
+ "sensor_msgs"
)
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## 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
-# )
+target_include_directories(ekf PUBLIC $ENV{HOME}/include)
+target_link_libraries(ekf $ENV{HOME}/lib/libimufusion.a)
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+install(TARGETS ekf
+ DESTINATION lib/${PROJECT_NAME})
-#############
-## Testing ##
-#############
+install(DIRECTORY config launch
+ DESTINATION share/${PROJECT_NAME})
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_imufusion_ros.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_package()
--- /dev/null
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /TF1
+ - /TF1/Frames1
+ Splitter Ratio: 0.5
+ Tree Height: 549
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: Temperature
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/RobotModel
+ Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /robot_description
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ bno055_shuttle_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bno055_shuttle_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bno055_shuttle_measurement_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ bno055_shuttle_origin:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ bno055_shuttle_socket_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Mass Properties:
+ Inertia: false
+ Mass: false
+ Name: RobotModel
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Alpha: 0.30000001192092896
+ Autocompute Intensity Bounds: false
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: temperature
+ Class: rviz_default_plugins/Temperature
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: true
+ Max Color: 255; 255; 255
+ Max Intensity: 100
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: Temperature
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.10000000149011612
+ Style: Flat Squares
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /imu/temperature
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Filter (blacklist): ""
+ Filter (whitelist): ""
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ bno055_shuttle_base_link:
+ Value: false
+ bno055_shuttle_link:
+ Value: false
+ bno055_shuttle_measurement_frame:
+ Value: true
+ bno055_shuttle_origin:
+ Value: false
+ bno055_shuttle_socket_link:
+ Value: false
+ enu_link:
+ Value: true
+ imu_link:
+ Value: false
+ map:
+ Value: false
+ Marker Scale: 0.20000000298023224
+ Name: TF
+ Show Arrows: false
+ Show Axes: true
+ Show Names: false
+ Tree:
+ map:
+ enu_link:
+ imu_link:
+ bno055_shuttle_origin:
+ bno055_shuttle_base_link:
+ bno055_shuttle_socket_link:
+ bno055_shuttle_link:
+ bno055_shuttle_measurement_frame:
+ {}
+ Update Interval: 0
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 255; 255; 255
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 0.1501678079366684
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.785398006439209
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 0.785398006439209
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1200
+ X: 622
+ Y: 87
-# Floating point numbers in YAML require the dot, even if using "e".
+ekf:
+ ros__parameters:
-Pw: [7.47601e-07, 7.47601e-07, 7.47601e-07, 7.47601e-07, 0.00274156, 0.00274156, 0.00274156]
-Pv: [0.000173706, 0.000173706, 0.000173706, 1.0e-12, 1.0e-12, 1.96e-12]
+ # Floating point numbers in YAML require the dot, even if using "e".
+
+ Pw: [7.47601e-07, 7.47601e-07, 7.47601e-07, 7.47601e-07, 0.00274156, 0.00274156, 0.00274156]
+ Pv: [0.000173706, 0.000173706, 0.000173706, 1.0e-12, 1.0e-12, 1.96e-12]
+++ /dev/null
-<launch>
- <arg name="config" default="$(find imufusion_ros)/config/ekf.yaml"/>
-
- <include file="$(find imufusion_ros)/launch/ekf.launch">
- <arg name="config" value="$(arg config)"/>
- </include>
-
- <node pkg="tf2_ros" type="static_transform_publisher"
- name="enu_publisher"
- args="0 0 0 0 0 0.9238795 0.3826834 map enu_link" />
- <node pkg="imu_odometry" type="imu2tf" name="imu2tf"
- args="enu_link" output="screen" />
- <node name="rviz" pkg="rviz" type="rviz"
- args="-d $(find imufusion_ros)/rviz/display.rviz" />
-</launch>
--- /dev/null
+<launch>
+ <arg name="config" default="$(find-pkg-share imufusion_ros)/config/ekf.yaml"/>
+
+ <include file="$(find-pkg-share imufusion_ros)/launch/ekf.launch.xml">
+ <arg name="config" value="$(var config)"/>
+ </include>
+
+ <node pkg="tf2_ros" exec="static_transform_publisher"
+ name="enu_publisher"
+ args="0 0 0 0 0 0.9238795 0.3826834 map enu_link" />
+ <node pkg="imu_odometry" exec="imu2tf" name="imu2tf"
+ args="enu_link" output="screen" />
+ <node name="rviz" pkg="rviz2" exec="rviz2"
+ args="-d $(find-pkg-share imufusion_ros)/config/display.rviz" />
+</launch>
+++ /dev/null
-<launch>
- <arg name="config" default="$(find imufusion_ros)/config/ekf.yaml"/>
-
- <node pkg="imufusion_ros" type="ekf" name="ekf" args="ekf_link">
- <rosparam file="$(arg config)" command="load" />
- </node>
-</launch>
--- /dev/null
+<launch>
+ <arg name="config" default="$(find-pkg-share imufusion_ros)/config/ekf.yaml"/>
+
+ <node pkg="imufusion_ros" exec="ekf" name="ekf" args="ekf_link">
+ <param from="$(var config)"/>
+ </node>
+</launch>
<?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
<name>imufusion_ros</name>
- <version>0.1.0</version>
- <description>The imufusion_ros package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <version>0.0.0</version>
+ <description>ROS wrapper for imufusion.</description>
<maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+ <license>GPL-3.0-only</license>
+ <buildtool_depend>ament_cmake</buildtool_depend>
- <!-- 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>GPLv3</license>
-
-
- <!-- 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/imufusion_ros</url> -->
-
+ <depend>rclcpp</depend>
+ <!--depend>eigen_conversions</depend-->
+ <depend>geometry_msgs</depend>
+ <depend>imufusion</depend>
+ <depend>sensor_msgs</depend>
+ <depend>tf2_eigen</depend>
- <!-- 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> -->
- <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
-
- <!-- 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>eigen_conversions</build_depend>
- <build_depend>geometry_msgs</build_depend>
- <build_depend>imufusion</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>sensor_msgs</build_depend>
- <build_export_depend>eigen_conversions</build_export_depend>
- <build_export_depend>geometry_msgs</build_export_depend>
- <build_export_depend>imufusion</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>sensor_msgs</build_export_depend>
- <exec_depend>eigen_conversions</exec_depend>
- <exec_depend>geometry_msgs</exec_depend>
- <exec_depend>imufusion</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>sensor_msgs</exec_depend>
-
-
- <!-- 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>
+++ /dev/null
-Panels:
- - Class: rviz/Displays
- Help Height: 78
- Name: Displays
- Property Tree Widget:
- Expanded: ~
- Splitter Ratio: 0.4301801919937134
- Tree Height: 555
- - Class: rviz/Selection
- Name: Selection
- - Class: rviz/Tool Properties
- Expanded:
- - /2D Pose Estimate1
- - /2D Nav Goal1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz/Time
- Experimental: false
- Name: Time
- SyncMode: 0
- SyncSource: ""
-Preferences:
- PromptSaveOnExit: true
-Toolbars:
- toolButtonStyle: 2
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.029999999329447746
- Value: Lines
- Name: Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: 0
- Plane: XY
- Plane Cell Count: 10
- Reference Frame: <Fixed Frame>
- Value: true
- - Alpha: 1
- Class: rviz_plugin_tutorials/Imu
- Color: 204; 51; 204
- Enabled: true
- History Length: 1
- Name: Imu (data)
- Queue Size: 10
- Topic: /imu/data
- Unreliable: false
- Value: true
- - Alpha: 0.10000000149011612
- Class: rviz_plugin_tutorials/Imu
- Color: 57; 52; 204
- Enabled: true
- History Length: 1
- Name: Imu (raw data)
- Queue Size: 10
- Topic: /imu/data_raw
- Unreliable: false
- Value: true
- - Alpha: 1
- Class: rviz/Axes
- Enabled: true
- Length: 5
- Name: Axes (Fixed Frame)
- Radius: 0.10000000149011612
- Reference Frame: <Fixed Frame>
- Value: true
- - Alpha: 1
- Class: rviz/Axes
- Enabled: true
- Length: 5
- Name: Axes (enu_link)
- Radius: 0.10000000149011612
- Reference Frame: enu_link
- Value: true
- - Alpha: 1
- Class: rviz/Axes
- Enabled: true
- Length: 5
- Name: Axes (imu_link)
- Radius: 0.10000000149011612
- Reference Frame: imu_link
- Value: true
- - Alpha: 1
- Class: rviz/Axes
- Enabled: true
- Length: 5
- Name: Axes (ekf_link)
- Radius: 0.10000000149011612
- Reference Frame: ekf_link
- Value: true
- Enabled: true
- Global Options:
- Background Color: 249; 249; 249
- Default Light: true
- Fixed Frame: map
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz/Interact
- Hide Inactive Objects: true
- - Class: rviz/MoveCamera
- - Class: rviz/Select
- - Class: rviz/FocusCamera
- - Class: rviz/Measure
- - Class: rviz/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
- Single click: true
- Topic: /clicked_point
- Value: true
- Views:
- Current:
- Class: rviz/Orbit
- Distance: 24.478668212890625
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Field of View: 0.7853981852531433
- Focal Point:
- X: -1.5147979259490967
- Y: -1.228423833847046
- Z: 1.93975031375885
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.785398006439209
- Target Frame: <Fixed Frame>
- Yaw: 0.785398006439209
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 846
- Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd0000000400000000000001ba000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055c0000003efc0100000002fb0000000800540069006d006501000000000000055c0000023f00fffffffb0000000800540069006d0065010000000000000450000000000000000000000296000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1372
- X: 0
- Y: 0
/******************************************************************************
IMU Fusion Library ROS Wrapper
Extended Kalman Filter
- Copyright (C) 2019, 2020 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2019..2025 Walter Fetter Lages <w.fetter@ieee.org>
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
*******************************************************************************/
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/Imu.h>
-#include <sensor_msgs/MagneticField.h>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/magnetic_field.hpp>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.hpp>
#include <ekf.hpp>
-class EkfNode
+class EkfNode: public rclcpp::Node
{
public:
- EkfNode(const ros::NodeHandle &node,const char *frameId);
+ EkfNode(const char *name,const char *frameId);
~EkfNode(void);
void update(void);
void publish(void);
private:
- ros::NodeHandle node_;
- ros::Subscriber imuSubscriber_;
- ros::Subscriber magSubscriber_;
- ros::Publisher imuPublisher_;
+ rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSubscriber_;
+ rclcpp::Subscription<sensor_msgs::msg::MagneticField>::SharedPtr magSubscriber_;
+ rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imuPublisher_;
std::string frameId_;
Eigen::Vector3d accel_;
Eigen::Matrix3d magCovariance_;
Eigen::Matrix3d gyroCovariance_;
- imufusion::Ekf *ekf;
+ imufusion::Ekf *ekf_;
- bool imuAvailable;
- bool magAvailable;
+ bool imuAvailable_;
+ bool magAvailable_;
- void imuCB(const sensor_msgs::Imu::ConstPtr &imuMsg);
- void magCB(const sensor_msgs::MagneticField::ConstPtr &magMsg);
+ void imuCB(const sensor_msgs::msg::Imu::SharedPtr imuMsg);
+ void magCB(const sensor_msgs::msg::MagneticField::SharedPtr magMsg);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
+EkfNode::EkfNode(const char *name,const char *frameId):Node(name)
{
- node_=node;
frameId_=frameId;
accel_.setZero();
accelCovariance_.setZero();
gyroCovariance_.setZero();
- imuAvailable=false;
- magAvailable=false;
+ imuAvailable_=false;
+ magAvailable_=false;
- imuSubscriber_=node_.subscribe("imu/data_raw",10,&EkfNode::imuCB,this);
- magSubscriber_=node_.subscribe("imu/mag",10,&EkfNode::magCB,this);
- imuPublisher_=node_.advertise<sensor_msgs::Imu>("imu/data",10);
+ imuSubscriber_=create_subscription<sensor_msgs::msg::Imu>("imu/data_raw",10,std::bind(&EkfNode::imuCB,this,std::placeholders::_1));
+ magSubscriber_=create_subscription<sensor_msgs::msg::MagneticField>("imu/mag",10,std::bind(&EkfNode::magCB,this,std::placeholders::_1));
+ imuPublisher_=create_publisher<sensor_msgs::msg::Imu>("imu/data",10);
Eigen::MatrixXd Pw(7,7);
Pw.setIdentity();
std::vector<double> PwDiag;
- ros::NodeHandle nh("~");
- if(nh.getParam("Pw",PwDiag))
+ declare_parameter("Pw",rclcpp::PARAMETER_DOUBLE_ARRAY);
+ if(this->get_parameter("Pw",PwDiag))
{
Eigen::Map<Eigen::VectorXd> diag(PwDiag.data(),Pw.rows());
Pw=diag.asDiagonal();
Eigen::MatrixXd Pv(6,6);
Pv.setIdentity();
std::vector<double> PvDiag;
- if(nh.getParam("Pv",PvDiag))
+ declare_parameter("Pv",rclcpp::PARAMETER_DOUBLE_ARRAY);
+ if(this->get_parameter("Pv",PvDiag))
{
Eigen::Map<Eigen::VectorXd> diag(PvDiag.data(),Pv.rows());
Pv=diag.asDiagonal();
const double T=0.02; // 0.01 is too fast for an UART at 115200 bps
- ekf=new imufusion::Ekf(Pw,Pv,T,100);
+ ekf_=new imufusion::Ekf(Pw,Pv,T,100);
}
EkfNode::~EkfNode(void)
{
- imuSubscriber_.shutdown();
- magSubscriber_.shutdown();
- imuPublisher_.shutdown();
-
- delete ekf;
+ delete ekf_;
}
-void EkfNode::imuCB(const sensor_msgs::Imu::ConstPtr &imuMsg)
+void EkfNode::imuCB(const sensor_msgs::msg::Imu::SharedPtr imuMsg)
{
- tf::vectorMsgToEigen(imuMsg->linear_acceleration,accel_);
- tf::vectorMsgToEigen(imuMsg->angular_velocity,gyro_);
+ tf2::fromMsg(imuMsg->linear_acceleration,accel_);
+ tf2::fromMsg(imuMsg->angular_velocity,gyro_);
for(int i=0;i < 3;i++) for(int j=0;j < 3;j++)
{
accelCovariance_(i,j)=imuMsg->linear_acceleration_covariance[i*3+j];
gyroCovariance_(i,j)=imuMsg->angular_velocity_covariance[i*3+j];
}
- imuAvailable=true;
+ imuAvailable_=true;
}
-void EkfNode::magCB(const sensor_msgs::MagneticField::ConstPtr &magMsg)
+void EkfNode::magCB(const sensor_msgs::msg::MagneticField::SharedPtr magMsg)
{
- tf::vectorMsgToEigen(magMsg->magnetic_field,mag_);
+ tf2::fromMsg(magMsg->magnetic_field,mag_);
for(int i=0;i < 3;i++) for(int j=0;j < 3;j++)
magCovariance_(i,j)=magMsg->magnetic_field_covariance[i*3+j];
- magAvailable=true;
+ magAvailable_=true;
}
void EkfNode::update(void)
{
- if(imuAvailable && magAvailable) ekf->update(accel_,mag_,gyro_);
+ if(imuAvailable_ && magAvailable_) ekf_->update(accel_,mag_,gyro_);
}
void EkfNode::publish(void)
{
- ros::Time time=ros::Time::now();
-
- sensor_msgs::Imu dataMsg;
- dataMsg.header.stamp=time;
+ sensor_msgs::msg::Imu dataMsg;
+ dataMsg.header.stamp=now();
dataMsg.header.frame_id=frameId_;
- tf::quaternionEigenToMsg(ekf->quaternion(),dataMsg.orientation);
- tf::vectorEigenToMsg(gyro_,dataMsg.angular_velocity);
- tf::vectorEigenToMsg(accel_,dataMsg.linear_acceleration);
+ dataMsg.orientation=tf2::toMsg(ekf_->quaternion());
+ tf2::toMsg(gyro_,dataMsg.angular_velocity);
+ tf2::toMsg(accel_,dataMsg.linear_acceleration);
- Eigen::MatrixXd orientationCovariance=ekf->covariance();
+ Eigen::MatrixXd orientationCovariance=ekf_->covariance();
for(int i=0;i < 3;i++) for(int j=0;j < 3;j++)
{
dataMsg.orientation_covariance[i*3+j]=orientationCovariance(i,j);
dataMsg.angular_velocity_covariance[i*3+j]=gyroCovariance_(i,j);
}
- imuPublisher_.publish(dataMsg);
+ imuPublisher_->publish(dataMsg);
}
int main(int argc,char* argv[])
{
- ros::init(argc,argv,"ekf");
- ros::NodeHandle node;
-
- if(argc != 2)
+ rclcpp::init(argc,argv);
+
+ if(argc < 2)
{
- ROS_ERROR_STREAM("ekf: No frame id.\n");
+ std::cerr << "ekf: No frame id.\n";
return -1;
}
- EkfNode ekfNode(node,argv[1]);
+ EkfNode ekfNode("ekf",argv[1]);
- ros::Rate loop(50);
- while(ros::ok())
+ rclcpp::Rate loop(50);
+ while(rclcpp::ok())
{
ekfNode.update();
ekfNode.publish();
- ros::spinOnce();
+ rclcpp::spin_some(ekfNode.get_node_base_interface());
loop.sleep();
}
return 0;
projects / imufusion_ros.git / commitdiff