Publish odom

ROS needs the accumulated odometry to be published in the tf. This implies changes in:

• ProcessorMotion
• wolf_ros_node: Enable/disable the publication of odom tf via parameter.

added To Do (release) label

closed

why was this issue closed?

Because Pep and I forgot exactly why it was open, if you know why it was open why can modify the description and reopen it

The description is ok.. ROS needs the accumulated odometry to be published in the tf, so we have to publish it.

reopened

I will be addressing this issue in the following days.

Ideas to produce odometry trajectories related to each particular ProcessorMotion:

1. We already have a buffer MotionBuffer storing the motion data and the integrated deltas

2. Store, somewhere in the CaptureMotion at origin, the value of the fully-integrated odometry at the origin timestamp.

3. Have getOdometry() and getOdometry(ts) methods returning the composition of the odometry at origin plus the delta at the requested timestamp.

4. For IMU, I see two options (I think the good one is the second one):

   • Store the odometry state at origin in the form of an accumulated delta -- this contains huge terms in the z-direction due to the free-fall accumulating in all deltas.
   • Store the odometry at origin in the form of a state -- this does not contain the free-fall terms

One comment about 2. above:

The integrated odometry at the origin timestamp is necessary to break the odometry history into chunks, each chunk stored in the buffer in the CaptureMotions.

• Sotring the odomtery in the Processor is not good because we need to store all the history of odometries at each KF timestamp.

• Storing the odometry in the Frame I think is not good because multiple processors need to have different odomtery values

Therefore the odometry needs to be stored also in the CaptureMotion, alongside the buffer.

A simpler solution discussed with @joanvallve is what is going to be initially implemented:

Objective: provide a PM->getOdometry() for the present time stamp only

Implementation:

• In PM: VectorComposite odometry_;

• In PM::constructor --> odometry_ = Problem->stateZero( getStateStructure() );

• In PM::processCapture() --> odometry_ = statePlusDelta(delta_);

• In PM:: getOdomtery() {return odometry_;}

created merge request !387 (merged) to address this issue

mentioned in merge request !387 (merged)

assigned to @jsola

closed via merge request !387 (merged)

mentioned in commit 820eac18

@jcasals This issue is now merged in !387 (merged). It is accompanied by commit 3a01d91b in laser implementing the odometry integration for ProcessorOdomIcp.

Since I could not produce nice gtests due to lack of real scan data, can you please test if ProcessorOdomIcp() is producing reasonable odometry trajectories?

In ROS, you just need to access the processor and call

processor_odom_icp->getOdometry();

with the result of a VectorComposite. If you want an Eigen vector, use:

processor_odom_icp->getOdometry().vector("PO");

The result needs to be fed to ROS odometry to get the odom frame of the TF updated... I dont know how to do it though.

reopened

This functionality is still pending to be implemented in PublisherTf in order to publish odometry in ROS optionally. It was implemented by @jcasals in the wolf_ros_node in a branch: here.

assigned to @joanvallve and unassigned @jsola

Implementing this I realized that since the problem can have some IsMotion processors, maybe a method getOdometry() should be implemented in Problem analogous to getState() in which all processors are iterated to fill a VectorComposite. Is this overkilling? Anyway, somewhere, there should be a code that decides which/how processors are considered..

Maybe Problem::getOdometry() would be the most coherent solution and it could allow the user to specify some options (if only one processor should be used or one complemented with the others, which processor is the main one...) I don't have a strong opinion on this.. @jsola what do you think?

removed To Do (release) label

added Doing label

added Agreement required label

Implementing this Problem::getCovariance() in !414 (merged)

closed