From aeef4ee0098bd09874ed8e7ca9e4df13f910e947 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 4 Mar 2024 10:52:11 +0100
Subject: [PATCH] Devel
---
.gitignore | 1 +
include/subscriber_landmarks.h | 3 +-
include/subscriber_odom2d.h | 2 +
msg/LandmarkDetection.msg | 3 +-
src/subscriber_landmarks.cpp | 80 +++++++++++++++++++---------------
src/subscriber_odom2d.cpp | 31 ++++++++++++-
6 files changed, 80 insertions(+), 40 deletions(-)
diff --git a/.gitignore b/.gitignore
index 3cf468e..9934320 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,3 +6,4 @@
build-debug/
build-release/
build/
+.clang-format
diff --git a/include/subscriber_landmarks.h b/include/subscriber_landmarks.h
index 6358fac..0d26081 100644
--- a/include/subscriber_landmarks.h
+++ b/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_;
diff --git a/include/subscriber_odom2d.h b/include/subscriber_odom2d.h
index 3e051e1..b1f78ee 100644
--- a/include/subscriber_odom2d.h
+++ b/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:
diff --git a/msg/LandmarkDetection.msg b/msg/LandmarkDetection.msg
index 640da05..1a0d311 100644
--- a/msg/LandmarkDetection.msg
+++ b/msg/LandmarkDetection.msg
@@ -1,3 +1,4 @@
geometry_msgs/PoseWithCovariance pose
int32 id
-float32 quality
\ No newline at end of file
+int32 type
+float32 quality
diff --git a/src/subscriber_landmarks.cpp b/src/subscriber_landmarks.cpp
index 80fe5fe..f0db841 100644
--- a/src/subscriber_landmarks.cpp
+++ b/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
diff --git a/src/subscriber_odom2d.cpp b/src/subscriber_odom2d.cpp
index 386776b..dbefd3f 100644
--- a/src/subscriber_odom2d.cpp
+++ b/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");
--
GitLab