Skip to content
Snippets Groups Projects
Commit aeef4ee0 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Devel

parent c6f6d585
No related branches found
No related tags found
1 merge request!19Devel
......@@ -6,3 +6,4 @@
build-debug/
build-release/
build/
.clang-format
......@@ -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_;
......
......@@ -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:
......
geometry_msgs/PoseWithCovariance pose
int32 id
float32 quality
\ No newline at end of file
int32 type
float32 quality
......@@ -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
......
......@@ -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");
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment