Devel

.gitignore

@@ -6,3 +6,4 @@
 build-debug/
 build-release/
 build/
+.clang-format

include/subscriber_landmarks.h

@@ -41,8 +41,9 @@ namespace wolf
 class SubscriberLandmarks : public Subscriber
 {
    protected:
-      SizeEigen dim; 
+      SizeEigen dim;
       bool inverse_detections_;
+      unsigned int type_offset_, id_offset_;
       Eigen::Vector3d sensor_p_;
       Eigen::Quaterniond sensor_q_;
 

include/subscriber_odom2d.h

@@ -46,7 +46,9 @@ class SubscriberOdom2d : public Subscriber
 {
    protected:
       ros::Time last_odom_stamp_;
+      Eigen::Vector3d last_pose_2d_;
       SensorOdom2dPtr sensor_odom_;
+      bool get_odom_from_pose_;
 
    public:
 

msg/LandmarkDetection.msg

 geometry_msgs/PoseWithCovariance pose
 int32 id
-float32 quality
\ No newline at end of file
+int32 type
+float32 quality

src/subscriber_landmarks.cpp

@@ -39,84 +39,92 @@ using namespace Eigen;
 
 namespace wolf
 {
-SubscriberLandmarks::SubscriberLandmarks(const std::string& _unique_name,
-                                         const ParamsServer& _server,
+SubscriberLandmarks::SubscriberLandmarks(const std::string  &_unique_name,
+                                         const ParamsServer &_server,
                                          const SensorBasePtr _sensor_ptr)
-  : Subscriber(_unique_name, _server, _sensor_ptr)
+    : Subscriber(_unique_name, _server, _sensor_ptr)
 {
     assert(_sensor_ptr);
-    dim = _sensor_ptr->getProblem()->getDim();
-    inverse_detections_ = _server.getParam<bool>(prefix_ + "/inverse_detections");
+    dim                    = _sensor_ptr->getProblem()->getDim();
+    inverse_detections_    = _server.getParam<bool>(prefix_ + "/inverse_detections");
+    type_offset_           = _server.getParam<unsigned int>(prefix_ + "/type_offset");
+    id_offset_             = _server.getParam<unsigned int>(prefix_ + "/id_offset");
     auto sensor_extrinsics = _server.getParam<VectorXd>(prefix_ + "/sensor_extrinsics");
-    sensor_p_ = sensor_extrinsics.head<3>();
-    sensor_q_ = Quaterniond(Vector4d(sensor_extrinsics.tail<4>()));
+    sensor_p_              = sensor_extrinsics.head<3>();
+    sensor_q_              = Quaterniond(Vector4d(sensor_extrinsics.tail<4>()));
 }
 
-void SubscriberLandmarks::initialize(ros::NodeHandle& nh, const std::string& topic)
+void SubscriberLandmarks::initialize(ros::NodeHandle &nh, const std::string &topic)
 {
     sub_ = nh.subscribe(topic, 100, &SubscriberLandmarks::callback, this);
 }
 
-void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr& msg)
+void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr &msg)
 {
-    ROS_INFO("SubscriberLandmarks::callback: %lu detections", msg->detections.size());
+    ROS_DEBUG("SubscriberLandmarks::callback: %lu detections", msg->detections.size());
 
     updateLastHeader(msg->header);
 
-    auto cap = std::make_shared<CaptureLandmarksExternal>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_);
+    auto cap = std::make_shared<CaptureLandmarksExternal>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
+                                                          sensor_ptr_);
     // Extract detections from msg
     for (auto i = 0; i < msg->detections.size(); i++)
     {
         // 3D measurement from msg
         VectorXd meas(7);
-        meas << msg->detections.at(i).pose.pose.position.x,
-                msg->detections.at(i).pose.pose.position.y,
-                msg->detections.at(i).pose.pose.position.z,
-                msg->detections.at(i).pose.pose.orientation.x,
-                msg->detections.at(i).pose.pose.orientation.y,
-                msg->detections.at(i).pose.pose.orientation.z,
-                msg->detections.at(i).pose.pose.orientation.w;
-        
+        meas << msg->detections.at(i).pose.pose.position.x, msg->detections.at(i).pose.pose.position.y,
+            msg->detections.at(i).pose.pose.position.z, msg->detections.at(i).pose.pose.orientation.x,
+            msg->detections.at(i).pose.pose.orientation.y, msg->detections.at(i).pose.pose.orientation.z,
+            msg->detections.at(i).pose.pose.orientation.w;
+
         meas.head<3>() = sensor_p_ + sensor_q_ * meas.head<3>();
         meas.tail<4>() = (sensor_q_ * Quaterniond(Vector4d(meas.tail<4>()))).coeffs();
 
         // PoseWithCovariance documentation:
         //   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)
-        MatrixXd cov = Eigen::Map<const Eigen::Matrix6d>(msg->detections.at(i).pose.covariance.data());
-        auto R = q2R(meas.tail<4>());
-        cov.topLeftCorner<3,3>() = R * cov.topLeftCorner<3,3>() * R.transpose();
-        //TODO: rotate covariance for orientation
+        //   In order, the parameters are: (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z
+        //   axis)
+        MatrixXd cov              = Eigen::Map<const Eigen::Matrix6d>(msg->detections.at(i).pose.covariance.data());
+        auto     R                = q2R(meas.tail<4>());
+        cov.topLeftCorner<3, 3>() = R * cov.topLeftCorner<3, 3>() * R.transpose();
+        // TODO: rotate covariance for orientation
 
         // inverse measurement
         if (inverse_detections_)
         {
-            meas = SE3::inverse(meas);
-            cov.topLeftCorner<3,3>() = R.transpose() * cov.topLeftCorner<3,3>() * R;
-            //TODO: rotate covariance for orientation
+            meas                      = SE3::inverse(meas);
+            cov.topLeftCorner<3, 3>() = R.transpose() * cov.topLeftCorner<3, 3>() * R;
+            // TODO: rotate covariance for orientation
         }
 
         // 2D
         if (dim == 2)
         {
             VectorXd meas2d = meas.head<3>();
-            meas2d(2) = getYaw(q2R(meas.tail<4>()));
+            meas2d(2)       = getYaw(q2R(meas.tail<4>()));
 
-            MatrixXd cov2d(3,3);
-            cov2d << cov(0,0), cov(0,1), cov(0,5),
-                     cov(1,0), cov(1,1), cov(1,5),
-                     cov(5,0), cov(5,1), cov(5,5);
+            MatrixXd cov2d(3, 3);
+            cov2d << cov(0, 0), cov(0, 1), cov(0, 5), cov(1, 0), cov(1, 1), cov(1, 5), cov(5, 0), cov(5, 1), cov(5, 5);
 
             meas = meas2d;
-            cov = cov2d;
+            cov  = cov2d;
         }
-        
-        std::cout << "\tid " << msg->detections.at(i).id << ": quality: " << msg->detections.at(i).quality << ", meas: " << meas.transpose() << std::endl;
+
+        // std::cout << "\tid " << msg->detections.at(i).id << ": quality: " << msg->detections.at(i).quality << ",
+        // meas: " << meas.transpose() << std::endl;
+
+        // ID & TYPE
+        WOLF_WARN_COND(msg->detections.at(i).id < -1,
+                       "Received a LandmarkDetection with ID < -1, changing to -1 (undefined).");
+        WOLF_WARN_COND(msg->detections.at(i).type < -1,
+                       "Received a LandmarkDetection with TYPE < -1, changing to -1 (undefined).");
+        int id   = msg->detections.at(i).id < 0 ? -1 : msg->detections.at(i).id + id_offset_;
+        int type = msg->detections.at(i).type < 0 ? -1 : msg->detections.at(i).type + type_offset_;
 
         // fill capture
         makePosDef(cov);
-        cap->addDetection(msg->detections.at(i).id, meas, cov, msg->detections.at(i).quality);
+        cap->addDetection(id, type, meas, cov, msg->detections.at(i).quality);
     }
 
     // process

src/subscriber_odom2d.cpp

@@ -35,6 +35,7 @@
  **************************/
 #include <ros/ros.h>
 #include <nav_msgs/Odometry.h>
+#include <tf2/utils.h>
 
 /**************************
  *      STD includes      *
@@ -42,6 +43,7 @@
 #include <iostream>
 #include <iomanip>
 #include <queue>
+#include <cmath>
 
 namespace wolf
 {
@@ -51,8 +53,11 @@ SubscriberOdom2d::SubscriberOdom2d(const std::string& _unique_name,
   : Subscriber(_unique_name, _server, _sensor_ptr)
   , last_odom_stamp_(ros::Time(0))
   , sensor_odom_(std::static_pointer_cast<SensorOdom2d>(_sensor_ptr))
+  , last_pose_2d_(0.0, 0.0, 0.0)
+  , get_odom_from_pose_(false)
 {
     assert(std::dynamic_pointer_cast<SensorOdom2d>(_sensor_ptr) != nullptr && "SubscriberOdom2d: sensor provided is not of type SensorOdom2d!");
+    get_odom_from_pose_ = _server.getParam<bool>(prefix_ + "/odom_from_pose");
 }
 
 void SubscriberOdom2d::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -68,8 +73,27 @@ void SubscriberOdom2d::callback(const nav_msgs::Odometry::ConstPtr& msg)
 
     if (last_odom_stamp_ != ros::Time(0))
     {
-        double           dt          = (msg->header.stamp - last_odom_stamp_).toSec();
-        Eigen::Vector2d data(msg->twist.twist.linear.x * dt, msg->twist.twist.angular.z * dt);
+        Eigen::Vector2d data;
+        if (get_odom_from_pose_)
+        {
+            double dx, dy, sign;
+            dx = msg->pose.pose.position.x - last_pose_2d_(0);
+            dy = msg->pose.pose.position.y - last_pose_2d_(1);
+            data(0) = std::sqrt(std::pow(dx,2)+std::pow(dy,2));
+            data(1) = tf2::getYaw(msg->pose.pose.orientation) - last_pose_2d_(2);
+            while (data(1) > M_PI)
+                data(1) -= 2*M_PI;
+            while (data(1) < -M_PI)
+                data(1) += 2*M_PI;
+            sign = dx*std::cos(last_pose_2d_(2)) + dy*std::sin(last_pose_2d_(2))/data(0);
+            data(0) *= (sign > 0.0? 1.0: -1.0);
+        }
+        else
+        {
+            double           dt          = (msg->header.stamp - last_odom_stamp_).toSec();
+            data(0) = msg->twist.twist.linear.x * dt;
+            data(1) = msg->twist.twist.angular.z * dt;
+        }
         CaptureOdom2dPtr new_capture = std::make_shared<CaptureOdom2d>(
             TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
             sensor_ptr_,
@@ -77,6 +101,9 @@ void SubscriberOdom2d::callback(const nav_msgs::Odometry::ConstPtr& msg)
             sensor_odom_->computeCovFromMotion(data));
         sensor_ptr_->process(new_capture);
     }
+    last_pose_2d_(0) = msg->pose.pose.position.x;
+    last_pose_2d_(1) = msg->pose.pose.position.y;
+    last_pose_2d_(2) = tf2::getYaw(msg->pose.pose.orientation);
     last_odom_stamp_ = msg->header.stamp;
 
     ROS_DEBUG("WolfNodePolyline::odomCallback: end");