robot_pose_ekf with mocap

edit

Hello Balto,

First off all, yes you can repack it, but as you can see in nav_msgs/Odometry you also need the covariance matrix of the position/orientation (float64[36] covariance). This includes (x|y|z|roll|pitch|yaw) * (x|y|z|roll|pitch|yaw)^T.

Copied from message description: Row-major representation of the 6x6 covariance matrix. The orientation parameters use a fixed-axis representation. In order, the parameters are: (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)

You can directly fill the pose part of the nav_msgs/Odometry message with your geometry_msgs/Pose message. Now you have to add your covariances into the pose-covariance-matrix, this covariances normally grows during time. So every time you get a new message from mocap you must also modify your covariances. If every covariance in the matrix is zero, the filter thinks that your odometry doesn't produce any error and no correction/filtering is done at this momement because the position at this moment will be 100% correct for the filter and it didn't have to correct it!

The twist part of the nav_msgs/Odometry message can be empty because as you can see in the source code of the robot_pose_ekf (odom_estimation_node.cpp lines 164-205) the robot_pose_ekf only uses position x,y the orientation and of course the covariance matrix of pose. Also the covariance matrix of Twist can be empty for this filter.

Best regards, Thomas