linatypes.cpp

Go to the documentation of this file.

#include <galaxis/linatypes.hpp>
 
#include <cstdlib>
 
using namespace galaxis::lina;
 
std::shared_ptr<DebugPolynomType> pointcloud_to_ros_message(std::vector<Point2f> const& p_pointCloud) {
  std::shared_ptr<DebugPolynomType> message = std::make_shared<DebugPolynomType>();
 
  for(auto& p: p_pointCloud) {
    Vector2fType v2f;
    v2f.x = p(0);
    v2f.y = p(1);
    message->points.track.push_back(v2f);
  }
 
  return message;
}
 
std::shared_ptr<std::vector<Point2f>> ros_message_to_pointcloud(std::shared_ptr<DebugPolynomType>& p_debugPolynomMessageSPtr) {
  std::shared_ptr<std::vector<Point2f>> pointCloud = std::make_shared<std::vector<Point2f>>();
 
  for(auto& p: p_debugPolynomMessageSPtr->points.track) {
    Vector2f vec(p.x, p.y);
    pointCloud->push_back(vec);
  }
 
  return pointCloud;
}