some testing

include/LidarOdometry.h

@@ -6,11 +6,14 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <nav_msgs/Odometry.h>
 
 #include <fub_feature_common/Poles.h>
 #include <fub_feature_common/Edges.h>
 #include <fub_feature_common/Features.h>
 
+#include <pole_based_localization/PoleArray.h>
+
 
 namespace fub_lidar_odometry {
 
@@ -22,12 +25,15 @@ public:
     virtual void onInit() override;
 
 protected:
-    void callbackFeatures(const fub_feature_common::Features& features);
+    //void callbackFeatures(const fub_feature_common::Features& features);
+    void callbackFeatures(const pole_based_localization::PoleArray& features);
 
     ros::Subscriber mFeaturesSubscriber;
-    std::map<long, fub_feature_common::Pole> mLastPoles;
-    std::map<long, fub_feature_common::Corner> mLastCorners;
-    std::map<long, fub_feature_common::Edge> mLastEdges;
+    ros::Publisher mOdometryPublisher;
+    std::map<std::string, pole_based_localization::Pole> mLastPoles;
+    //std::map<long, fub_feature_common::Pole> mLastPoles;
+    std::map<std::string, fub_feature_common::Corner> mLastCorners;
+    std::map<std::string, fub_feature_common::Edge> mLastEdges;
 };
 }
 

src/LidarOdometry.cpp

@@ -25,11 +25,13 @@ LidarOdometry::~LidarOdometry()
 void LidarOdometry::onInit()
 {
     ros::NodeHandle nh;
-    mFeaturesSubscriber = nh.subscribe(nh.resolveName("/pole_based_localization/detected_features"), 11, &LidarOdometry::callbackFeatures, this);
+  //  mFeaturesSubscriber = nh.subscribe(nh.resolveName("/pole_based_localization/detected_features"), 11, &LidarOdometry::callbackFeatures, this);
+    mOdometryPublisher = nh.advertise<nav_msgs::Odometry>("/localization/lidar_odom",1);
+    mFeaturesSubscriber = nh.subscribe(nh.resolveName("/pole_recognition/trackedPolesArray"), 11, &LidarOdometry::callbackFeatures, this);
 }
 
 
-void LidarOdometry::callbackFeatures(const fub_feature_common::Features& features)
+void LidarOdometry::callbackFeatures(const pole_based_localization::PoleArray& features)
 {   
 /*
     // get own position
@@ -54,18 +56,21 @@ void LidarOdometry::callbackFeatures(const fub_feature_common::Features& feature
     psIn.header = header;
     tf2::doTransform(psIn, posUTM, transformStamped);*/
 
-    ROS_INFO("got features");
     pcl::PointCloud<pcl::PointXYZ> cloud_old;
     pcl::PointCloud<pcl::PointXYZ> cloud_new;
+    std::map<std::string, pole_based_localization::Pole> newPoles;
 
     // correlate last features with current features
-    for(auto pole : features.poles.poles){
-        if(mLastPoles.find(pole.trackingId) != mLastPoles.end()){
-            pcl::PointXYZ po(mLastPoles.at(pole.trackingId).position.x, mLastPoles.at(pole.trackingId).position.y, 0);
-            pcl::PointXYZ pn(pole.position.x, pole.position.y, 0);
+    for(auto pole : features.poles){
+        if(pole.type > 1)
+            continue;
+        if(mLastPoles.find(pole.ID) != mLastPoles.end()){
+            pcl::PointXYZ po(mLastPoles.at(pole.ID).pose.position.x, mLastPoles.at(pole.ID).pose.position.y, 0);
+            pcl::PointXYZ pn(pole.pose.position.x, pole.pose.position.y, 0);
             cloud_old.push_back(po);
             cloud_new.push_back(pn);
         }
+        newPoles[pole.ID] = pole;
     }
 
     //TODO: edges and walls
@@ -88,11 +93,22 @@ void LidarOdometry::callbackFeatures(const fub_feature_common::Features& feature
 
     result.orientation.x = 0.0;
     result.orientation.y = 0.0;
-    result.orientation.z = 0.0;
+    //result.orientation.z = 0.0;
     result.orientation.w = 1.0;
 
     // publish odometry
-    ROS_ERROR_STREAM("x, y, yaw: " << result.position.x << ", " << result.position.y << ", " << result.orientation.z);
+    //ROS_ERROR_STREAM("x, y, yaw: " << result.position.x << ", " << result.position.y << ", " << result.orientation.z);
+    nav_msgs::Odometry odom;
+    odom.header = features.header;
+    odom.header.frame_id = "base_link";
+    odom.pose.pose.position.x = result.position.x;
+    odom.pose.pose.position.y = result.position.y;
+    odom.pose.pose.orientation.z = result.position.z;
+    odom.pose.covariance[0] += 0.1;
+    odom.pose.covariance[7] += 0.1;
+    odom.pose.covariance[35] += 0.1;
+    mOdometryPublisher.publish(odom);
+    mLastPoles = newPoles;
 }