From 830a76ee513659eebff2f73ff7a419592bfeb475 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 10 Oct 2022 18:01:36 +0200
Subject: [PATCH] added subscriber for external landmarks
---
CMakeLists.txt | 3 +-
include/subscriber_diffdrive.h | 2 -
include/subscriber_landmarks.h | 57 ++++++++++++++++++
src/subscriber_diffdrive.cpp | 7 +--
src/subscriber_landmarks.cpp | 105 +++++++++++++++++++++++++++++++++
5 files changed, 165 insertions(+), 9 deletions(-)
create mode 100644 include/subscriber_landmarks.h
create mode 100644 src/subscriber_landmarks.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cc9d517..cc20c2f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,7 +57,8 @@ include_directories(
add_executable(${PROJECT_NAME} src/node.cpp)
add_library(subscriber_${PROJECT_NAME}
- src/subscriber_diffdrive.cpp
+ src/subscriber_diffdrive.cpp
+ src/subscriber_landmarks.cpp
src/subscriber_odom2d.cpp
src/subscriber_pose.cpp)
diff --git a/include/subscriber_diffdrive.h b/include/subscriber_diffdrive.h
index d733eba..48f64e2 100644
--- a/include/subscriber_diffdrive.h
+++ b/include/subscriber_diffdrive.h
@@ -42,9 +42,7 @@ namespace wolf
class SubscriberDiffdrive : public Subscriber
{
protected:
- ros::Time last_odom_stamp_;
Eigen::Vector2d last_angles_;
- int last_odom_seq_;
int last_kf = -1;
double ticks_cov_factor_;
diff --git a/include/subscriber_landmarks.h b/include/subscriber_landmarks.h
new file mode 100644
index 0000000..6b8d16b
--- /dev/null
+++ b/include/subscriber_landmarks.h
@@ -0,0 +1,57 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+#include "subscriber.h"
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include <core/common/wolf.h>
+#include <core/utils/params_server.h>
+
+/**************************
+ * ROS includes *
+ **************************/
+#include <ros/ros.h>
+#include "wolf_ros_node/LandmarkDetectionArray.h"
+
+
+namespace wolf
+{
+class SubscriberLandmarks : public Subscriber
+{
+ protected:
+ SizeEigen dim;
+
+ public:
+
+ SubscriberLandmarks(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr);
+ WOLF_SUBSCRIBER_CREATE(SubscriberLandmarks);
+
+ virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
+
+ void callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr& msg);
+};
+} // namespace wolf
\ No newline at end of file
diff --git a/src/subscriber_diffdrive.cpp b/src/subscriber_diffdrive.cpp
index 1ecb9d9..815e5f8 100644
--- a/src/subscriber_diffdrive.cpp
+++ b/src/subscriber_diffdrive.cpp
@@ -36,8 +36,6 @@ SubscriberDiffdrive::SubscriberDiffdrive(const std::string& _unique_name,
const ParamsServer& _server,
const SensorBasePtr _sensor_ptr)
: Subscriber(_unique_name, _server, _sensor_ptr)
-, last_odom_stamp_(ros::Time(0))
-, last_odom_seq_(-1)
{
last_angles_ = Eigen::Vector2d();
ticks_cov_factor_ = std::static_pointer_cast<SensorDiffDrive>(_sensor_ptr)->getParams()->ticks_cov_factor;
@@ -62,7 +60,7 @@ void SubscriberDiffdrive::callback(const sensor_msgs::JointState::ConstPtr& msg)
Eigen::MatrixXd cov = ticks_cov_factor_ * (angles_inc.cwiseAbs() + Eigen::Vector2d::Ones()).asDiagonal(); // TODO check this
- if (last_odom_seq_ != -1)
+ if (last_seq_ != -1)
{
CaptureDiffDrivePtr cptr = std::make_shared<CaptureDiffDrive>(
TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_, angles_inc, cov, nullptr);
@@ -82,12 +80,9 @@ void SubscriberDiffdrive::callback(const sensor_msgs::JointState::ConstPtr& msg)
last_kf = current_kf;
}
-
}
last_angles_ = msg_angles;
- last_odom_stamp_ = msg->header.stamp;
- last_odom_seq_ = msg->header.seq;
}
WOLF_REGISTER_SUBSCRIBER(SubscriberDiffdrive)
diff --git a/src/subscriber_landmarks.cpp b/src/subscriber_landmarks.cpp
new file mode 100644
index 0000000..7c2ac35
--- /dev/null
+++ b/src/subscriber_landmarks.cpp
@@ -0,0 +1,105 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "subscriber_landmarks.h"
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include <core/capture/capture_landmarks_external.h>
+
+/**************************
+ * ROS includes *
+ **************************/
+#include <tf/transform_datatypes.h>
+
+namespace wolf
+{
+SubscriberLandmarks::SubscriberLandmarks(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr)
+ : Subscriber(_unique_name, _server, _sensor_ptr)
+{
+ assert(_sensor_ptr);
+ dim = _sensor_ptr->getProblem()->getDim();
+}
+
+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)
+{
+ ROS_DEBUG("SubscriberLandmarks::callback");
+
+ updateLastHeader(msg->header);
+
+ 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++)
+ {
+ // measurement
+ VectorXd meas(dim == 2 ? 3 : 7);
+ if (dim == 2)
+ {
+ meas(0) = msg->detections.at(i).pose.pose.position.x;
+ meas(1) = msg->detections.at(i).pose.pose.position.y;
+ meas(2) = tf::getYaw(msg->detections.at(i).pose.pose.orientation);
+ }
+ else
+ {
+ meas(0) = msg->detections.at(i).pose.pose.position.x;
+ meas(1) = msg->detections.at(i).pose.pose.position.y;
+ meas(2) = msg->detections.at(i).pose.pose.position.z;
+ meas(3) = msg->detections.at(i).pose.pose.orientation.x;
+ meas(4) = msg->detections.at(i).pose.pose.orientation.y;
+ meas(5) = msg->detections.at(i).pose.pose.orientation.z;
+ meas(6) = msg->detections.at(i).pose.pose.orientation.w;
+ }
+ // covariance
+ // 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(dim == 2 ? 3 : 6, dim == 2 ? 3 : 6);
+ if (dim == 2)
+ {
+ cov << msg->detections.at(i).pose.covariance.at(0), msg->detections.at(i).pose.covariance.at(1), msg->detections.at(i).pose.covariance.at(5),
+ msg->detections.at(i).pose.covariance.at(6), msg->detections.at(i).pose.covariance.at(7), msg->detections.at(i).pose.covariance.at(11),
+ msg->detections.at(i).pose.covariance.at(30), msg->detections.at(i).pose.covariance.at(31), msg->detections.at(i).pose.covariance.at(35);
+ }
+ else
+ cov = Eigen::Map<const Eigen::Matrix6d>(msg->detections.at(i).pose.covariance.data());
+
+ // fill capture
+ cap->addDetection(msg->detections.at(i).id, meas, cov, msg->detections.at(i).quality);
+ }
+
+ // process
+ sensor_ptr_->process(cap);
+
+ ROS_DEBUG("SubscriberLandmarks::callback: end");
+}
+
+WOLF_REGISTER_SUBSCRIBER(SubscriberLandmarks)
+} // namespace wolf
--
GitLab