Do a SLERP to average two quaternions (3D space)

[rfzeg]

safe_flight/path_planner/src/path_planner/search_algorithm.cpp

Line 271 in 5c3be53

// TODO: Do something fancy with quaternions such that it works in 3D

There are many libraries that can average quaternions for you. One of them is the Eigen library (see http://eigen.tuxfamily.org).

To use Eigen with ROS, one must include the associated header files in the cpp source code and add lines to CmakeLists.txt. Specifically, add the following lines in the CMakeLists txt file:

find_package(cmake_modules REQUIRED) 
find_package(Eigen3 REQUIRED) 
include_directories(${EIGEN3_INCLUDE_DIR}) 
add_definitions(${EIGEN_DEFINITIONS}) 

See https://github.com/ros/cmake_modules/blob/0.3-devel/README.md# usage for an explanation of cmake_modules in ROS.

Implementation:

// In the source code, include the header files, for instance:
#include <Eigen/Core>
#include <Eigen/Geometry>
...

// Function to convert from ROS geometry_msgs::Quaternion
Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion & q) {
  return Eigen::Quaterniond(q.w, q.x, q.y, q.z);
}

geometry_msgs::PoseStamped pose1;
geometry_msgs::PoseStamped pose2;

// initialize q1, q2 with values
Eigen::Quaterniond q1 = 
        ros_to_eigen_quat(pose1.pose.orientation);
Eigen::Quaterniond q2 = 
        ros_to_eigen_quat(pose2.pose.orientation);

Doing a SLERP between two quaternions is just a matter of calling the slerp method.
In this example SLERP spherically interpolates between rot1 and rot2 by the interpolation coefficient t=0.5.

Eigen::Quaterniond qSlerp = q1.slerp(0.5, q2);