Create a plugin

Create a plugin

---

The laser_odometry scheme package relies on pluginlib.
The plugin is a simple wrapper between a scan-matcher and the laser_odometry scheme.

To create a plugin for your scan-matcher (say my_scan_matcher), first create a new ROS package laser_odometry_my_scan_matcher that depends both on laser_odometry_core and your package my_scan_matcher.

In this package we then create the wrapper class LaserOdometrMyScanMatcher.

Let's first make sure to set everything up for pluginlib to 'see' our plugin. To do so there are only three steps :

• First, your LaserOdometrMyScanMatcher.cpp file should contains at its end the following :
  (let us assume your class is declared in the namespace laser_odometry)

// Declare this class as a plugin of type LaserOdometrMyScanMatcher,
// with base class of type LaserOdometryBase
// for pluginlib to know.

#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(laser_odometry::LaserOdometrMyScanMatcher, laser_odometry::LaserOdometryBase);

• Second, create a laser_odometry_plugin.xml file in the root directory.
  This file should be along the line:

<library path="lib/liblaser_odometry_my_scan_matcher">
  <class type="laser_odometry::LaserOdometrMyScanMatcher" base_class_type="laser_odometry::LaserOdometryBase">
   <description>
     A laser_odometry plugin for my scan matcher.
   </description>
  </class>
</library>

• Third, export the plugin in your package.xml :

  <export>
    <laser_odometry_core plugin="${prefix}/laser_odometry_plugins.xml" />
  </export>

For more details, please refer to pluginlib documentation.

Now that the package is set-up, we can come to the actual wrapper.
The wrapper has very little requirements, actually a single function is required to be implemented.
However a couple others are available shall you need for instance to initialize your scan matcher on the first scan received or do some post-processing. The overridable functions are detailed below and somewhat summarized in the pseudo-code

The minimal wrapper should be something along the line :

#include <laser_odometry_core/laser_odometry_core.h>
namespace laser_odometry
{

class LaserOdometryMyScanMatcher : public LaserOdometryBase
{
  ...

protected:

  bool processImpl(const sensor_msgs::LaserScanConstPtr& laser_msg,
                   const laser_odometry::Transform& prediction) override;
  ...

  MyScanMatcher matcher_;
}

} /* namespace laser_odometry */

Note:
The laser_odometry package is meant to work for both 2d-laser and 3d-laser.
Thus it supports both message types

• sensor_msgs::LaserScanConstPtr
• sensor_msgs::PointCloud2ConstPtr
  All detailed functions above and below that rely on the input message exists for both message type.

Here is a list of overridable functions :

#include <laser_odometry_core/laser_odometry_core.h>
namespace laser_odometry
{

class LaserOdometryMyScanMatcher : public LaserOdometryBase
{
public:

  OdomType odomType() const noexcept override
  {
    // return a laser_odometry::OdomType enum. 
    // Choice is among the following :
    // Unknown   - default
    // Odom2D    - the scan-matcher estimates a 2D odometry
    // Odom2DCov - the scan-matcher estimates a 2D odometry with covariance
    // Odom3D    - the scan-matcher estimates a 3D odometry
    // Odom3DCov - the scan-matcher estimates a 3D odometry with covariance
  }

protected:

  bool processImpl(const sensor_msgs::LaserScanConstPtr& laser_msg,
                   const laser_odometry::Transform& prediction) override
  {
    // Derived class **must define this function**

    // This function is the core of the plugin as the main computation takes place here.
    // Given a new reading ' laser_msg ' and the previous key-scan ' reference_scan_ ' 
    // the scan matching should be something along the line :

    increment_ = my_scan_matching(reference_scan_, laser_msg);

    // Where ' increment_ ' is a laser_odometry::Transform that contains 
    // the estimated relative transform in the laser frame.
    // If available, the covariance associated to ' increment_ ' 
    // should be copied to ' increment_covariance_ ' of type
    // laser_odometry::Covariance.
    //
    // note : laser_odometry::Transform  = Eigen::Transform<Scalar, 3, Eigen::Isometry>
    //        laser_odometry::Covariance = Eigen::Matrix<Scalar, 6, 6>
    //        with Scalar = double

    increment_covariance_ = my_scan_matching_get_covariance();
  }

  //////////////////////////////////////////////////////////////////////////////////
  // The following functions are available if needed, hence completely optional.  //
  // They are presented in execution order. The execution order is also recalled  //
  // in the pesudo-code page of this wiki.                                        //
  //////////////////////////////////////////////////////////////////////////////////

  bool configureImpl() override
  {
    // This function is called within the LaserOdometryBase::configure() function
    // which itself should be called right after the object instantiation.
    // It allows the scan-matcher to perform some configuration steps such as
    // retrieving ros-parameters and such.

    return true; // configuration went fine.
  }

  bool initialize(const sensor_msgs::LaserScanConstPtr& scan_msg) override
  {
    // This function is called upon the very first reading.
    // It allows the scan-matcher to perform some initialization that depends on
    // the data processed. e.g. the need of knowing the number of scan's range-readings etc.

    // Notice that the first reading is also used internally as the first key-scan.

    return true; // initialization went fine.
  }

  void preProcessing() override
  {
    // Allows the scan-matcher to perform some pre-processing.
  }

  laser_odometry::Transform predict(const laser_odometry::Transform& last_increment_in_base) override
  {
    // Given the previously estimated increment in the base frame,
    // allows the scan-matcher to estimate
    // a transform for the yet to come matching.

    // Default : return Identity.

    return increment_in_base_prior;
  }

  bool processImpl(const sensor_msgs::LaserScanConstPtr& laser_msg,
                   const laser_odometry::Transform& prediction) override
  {
    // See above.

    return true; // matching went fine.
  }

  bool isKeyFrame(const laser_odometry::Transform& increment_in_base) override
  {
    // Allows the scan-matcher to draw some heuristics from the estimated correction
    // (in the base frame) about whether or not to consider
    // the current reading as a new key-scan.
    // Such heuristic could be :

    if (std::abs(laser_odometry::getYaw(increment_in_base.rotation())) > some_angular_threshold) return true;

    const double x = correction.getOrigin().getX();
    const double y = correction.getOrigin().getY();

    if (increment.translation().squaredNorm() > some_linear_threshold_squared) return true;

    return false;

    // if we want to create a new key-scan every times the robot moves more
    // than some linear / angular threshold.

    // Default : return true.
    // Every new reading is a new key-scan.
  }

  void isKeyFrame() override
  {
    // Do something in the case of a new key-scan.
  }

  void isNotKeyFrame() override
  {
    // Do something in the case of NO new key-scan.
  }

  void postProcessing()
  {
    // Allows the matcher to perform some post-processing.
  }
}

} /* namespace laser_odometry */