--- /dev/null
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+#*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(imufusion_ros)
+
+## 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
+ geometry_msgs
+# imufusion
+ roscpp
+ sensor_msgs
+ eigen_conversions
+)
+
+## 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
+# )
+
+###################################
+## 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 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
+)
+
+#############
+## 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
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_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
+# )
+
+## 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_imufusion_ros.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)
--- /dev/null
+<?xml version="1.0"?>
+<package format="2">
+ <name>imufusion_ros</name>
+ <version>0.0.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> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</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>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> -->
+
+
+ <!-- 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>
+
+
+ <!-- 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>geometry_msgs</build_depend>
+ <build_depend>imufusion</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>eigen_convertions</build_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>
+ <build_export_depend>eigen_convertions</build_export_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>imufusion</exec_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>sensor_msgs</exec_depend>
+ <exec_depend>eigen_convertions</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+/******************************************************************************
+ IMU Fusion Library ROS Wrapper
+ Extended Kalman Filter
+ Copyright (C) 2019 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
+ 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 <ros/ros.h>
+
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/MagneticField.h>
+
+#include <eigen_conversions/eigen_msg.h>
+
+#include <ekf.hpp>
+
+class EkfNode
+{
+ public:
+ EkfNode(const ros::NodeHandle &node,const char *frameId);
+ ~EkfNode(void);
+ void update(void);
+ void publish(void);
+
+ private:
+ ros::NodeHandle node_;
+ ros::Subscriber imuSubscriber_;
+ ros::Subscriber magSubscriber_;
+ ros::Publisher imuPublisher_;
+ std::string frameId_;
+
+ Eigen::Vector3d accel_;
+ Eigen::Vector3d mag_;
+ Eigen::Vector3d gyro_;
+
+ Eigen::Matrix3d accelCovariance_;
+ Eigen::Matrix3d magCovariance_;
+ Eigen::Matrix3d gyroCovariance_;
+
+ imufusion::Ekf *ekf;
+
+ void imuCB(const sensor_msgs::Imu::ConstPtr &imuMsg);
+ void magCB(const sensor_msgs::MagneticField::ConstPtr &magMsg);
+
+ public:
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
+{
+ node_=node;
+ frameId_=frameId;
+ 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);
+
+ Eigen::Quaterniond q0;
+ q0.setIdentity();
+
+ Eigen::MatrixXd P0(7,7);
+ P0.setIdentity();
+ std::vector<double> P0Diag;
+ if(node_.getParam("P",P0Diag))
+ {
+ Eigen::Map<Eigen::VectorXd> diag(P0Diag.data(),P0.rows());
+ P0=diag.asDiagonal();
+ }
+ else P0*=1e-5;
+
+ Eigen::MatrixXd Pw(7,7);
+ Pw.setIdentity();
+ std::vector<double> PwDiag;
+ ros::NodeHandle nh("~");
+ if(node_.getParam("/ekf/Pw",PwDiag))
+ {
+ Eigen::Map<Eigen::VectorXd> diag(PwDiag.data(),Pw.rows());
+ Pw=diag.asDiagonal();
+ }
+ else Pw*=1e-3*1e-3;
+
+ Eigen::MatrixXd Pv(6,6);
+ Pv.setIdentity();
+ std::vector<double> PvDiag;
+ if(nh.getParam("Pv",PvDiag))
+ {
+ Eigen::Map<Eigen::VectorXd> diag(PvDiag.data(),Pv.rows());
+ Pv=diag.asDiagonal();
+ }
+ else Pv*=1e-3*1e-3;
+
+ const double T=0.02; // 0.01 is too fast for an UART at 115200 bps
+
+ ekf=new imufusion::Ekf(q0,P0,Pw,Pv,T);
+}
+
+EkfNode::~EkfNode(void)
+{
+ imuSubscriber_.shutdown();
+ magSubscriber_.shutdown();
+ imuPublisher_.shutdown();
+
+ delete ekf;
+}
+
+void EkfNode::imuCB(const sensor_msgs::Imu::ConstPtr &imuMsg)
+{
+ tf::vectorMsgToEigen(imuMsg->linear_acceleration,accel_);
+ tf::vectorMsgToEigen(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];
+ }
+}
+
+void EkfNode::magCB(const sensor_msgs::MagneticField::ConstPtr &magMsg)
+{
+ tf::vectorMsgToEigen(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];
+}
+
+void EkfNode::update(void)
+{
+ ekf->update(accel_,mag_,gyro_);
+}
+
+void EkfNode::publish(void)
+{
+ ros::Time time=ros::Time::now();
+
+ sensor_msgs::Imu dataMsg;
+ dataMsg.header.stamp=time;
+ dataMsg.header.frame_id=frameId_;
+
+ tf::quaternionEigenToMsg(ekf->quaternion(),dataMsg.orientation);
+ tf::vectorEigenToMsg(gyro_,dataMsg.angular_velocity);
+ tf::vectorEigenToMsg(accel_,dataMsg.linear_acceleration);
+
+ 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.linear_acceleration_covariance[i*3+j]=accelCovariance_(i,j);
+ dataMsg.angular_velocity_covariance[i*3+j]=gyroCovariance_(i,j);
+ }
+
+ imuPublisher_.publish(dataMsg);
+}
+
+int main(int argc,char* argv[])
+{
+ ros::init(argc,argv,"ekf");
+ ros::NodeHandle node;
+
+ if(argc != 2)
+ {
+ ROS_ERROR_STREAM("ekf: No base frame id.\n");
+ return -1;
+ }
+
+ EkfNode ekfNode(node,argv[1]);
+
+ ros::Rate loop(50);
+ while(ros::ok())
+ {
+ ekfNode.update();
+ ekfNode.publish();
+
+ ros::spinOnce();
+ loop.sleep();
+ }
+ return 0;
+}
projects / imufusion_ros.git / commitdiff