diff --git a/include/wolf_ros_scan_visualizer.h b/include/wolf_ros_scan_visualizer.h
index 504b1047ee66cfc8ec433b9a49455d837fc217af..59e502748076538ef6d29aaad81e8d81a6dcdfa4 100644
--- a/include/wolf_ros_scan_visualizer.h
+++ b/include/wolf_ros_scan_visualizer.h
@@ -25,12 +25,13 @@ using namespace wolf;
class WolfRosScanVisualizer: public WolfRosVisualizer
{
+ int max_frames_;
public:
WolfRosScanVisualizer();
virtual ~WolfRosScanVisualizer(){};
void initialize(ros::NodeHandle &nh) override;
- pcl::PointCloud<pcl::PointXYZ> buildPointCloud(const ProblemPtr);
- pcl::PointCloud<pcl::PointXYZ> buildPointCloudWithMeasurements(const ProblemPtr);
+ pcl::PointCloud<pcl::PointXYZRGB> buildPointCloud(const ProblemPtr);
+ pcl::PointCloud<pcl::PointXYZRGB> buildPointCloudWithMeasurements(const ProblemPtr);
void publishPointCloud(const ProblemPtr);
ros::Publisher pub_pcl_from_states;
ros::Publisher pub_pcl_from_measurements;
diff --git a/src/wolf_ros_scan_visualizer.cpp b/src/wolf_ros_scan_visualizer.cpp
index 44638c66c537b21a8e5910f2225c5836ed3d92ff..e60a35f5a2ceaee0f2caeba976ae62325fb3e735 100644
--- a/src/wolf_ros_scan_visualizer.cpp
+++ b/src/wolf_ros_scan_visualizer.cpp
@@ -3,6 +3,7 @@
#include "Eigen/src/Geometry/Transform.h"
#include "core/common/wolf.h"
#include "core/feature/feature_base.h"
+#include "pcl/impl/point_types.hpp"
#include <iterator>
#include <laser/capture/capture_laser_2D.h>
#include <laser/sensor/sensor_laser_2D.h>
@@ -28,6 +29,8 @@ WolfRosScanVisualizer::WolfRosScanVisualizer() : WolfRosVisualizer()
void WolfRosScanVisualizer::initialize(ros::NodeHandle& nh)
{
WolfRosVisualizer::initialize(nh);
+
+ nh.param<int>("num_frames_visualize", max_frames_, 2);
WOLF_INFO("scan visualizer initialize");
pub_pcl_from_states = nh.advertise<sensor_msgs::PointCloud2>("point_cloud", 1);
pub_pcl_from_measurements = nh.advertise<sensor_msgs::PointCloud2>("point_cloud_measurements", 1);
@@ -42,7 +45,7 @@ void WolfRosScanVisualizer::fillFactorMarker(FactorBaseConstPtr fac,
visualization_msgs::Marker& fac_text_marker)
{
WolfRosVisualizer::fillFactorMarker(fac, fac_marker, fac_text_marker);
- if (fac->getFeature()->getType() == "ICP ALIGN" and fac->getType() == "ODOM 2D")
+ if (fac->getFeature()->getType() == "FeatureIcpAlign" and fac->getType() == "FactorOdom2D")
{
FactorOdom2DConstPtr fac_odom = std::static_pointer_cast<const FactorOdom2D>(fac);
@@ -50,7 +53,7 @@ void WolfRosScanVisualizer::fillFactorMarker(FactorBaseConstPtr fac,
unsigned int cap_id_other = -1;
for (const auto& cap_other : fac->getFrameOther()->getCaptureList())
{
- if (cap_other->getType() == "LASER 2D")
+ if (cap_other->getType() == "CaptureLaser2D")
cap_id_other = cap_other->id();
}
@@ -61,9 +64,9 @@ void WolfRosScanVisualizer::fillFactorMarker(FactorBaseConstPtr fac,
fac_text_marker.pose.position.z = fac_text_marker.pose.position.z + 0.25;
}
}
-void transformPointCloudFromLaserScan(pcl::PointCloud<pcl::PointXYZ>& cloud,
- CaptureLaser2DPtr cptr_laser,
- Eigen::Affine3f transform)
+void transformPointCloudFromLaserScan(pcl::PointCloud<pcl::PointXYZRGB>& cloud,
+ CaptureLaser2DPtr cptr_laser,
+ Eigen::Affine3f transform)
{
SensorLaser2DPtr sensor_laser = std::static_pointer_cast<SensorLaser2D>(cptr_laser->getSensor());
sensor_msgs::LaserScan laser_msg;
@@ -88,17 +91,26 @@ void transformPointCloudFromLaserScan(pcl::PointCloud<pcl::PointXYZ>& cloud,
pcl::transformPointCloud(cloud, cloud, transform);
}
-pcl::PointCloud<pcl::PointXYZ> WolfRosScanVisualizer::buildPointCloud(const ProblemPtr problem)
+pcl::PointCloud<pcl::PointXYZRGB> WolfRosScanVisualizer::buildPointCloud(const ProblemPtr problem)
{
std::ofstream file;
- pcl::PointCloud<pcl::PointXYZ> agg_cloud;
- for (auto key_frame : problem->getTrajectory()->getFrameList())
+ pcl::PointCloud<pcl::PointXYZRGB> agg_cloud;
+ auto trajectory_aux = problem->getTrajectory()->getFrameList();
+ trajectory_aux.reverse();
+ auto end = std::next(trajectory_aux.begin(), min(trajectory_aux.size(), max_frames_ + 1));
+ if(max_frames_ == 0)
+ end = trajectory_aux.end();
+ std::list<FrameBasePtr> trajectory(trajectory_aux.begin(), end);
+ unsigned int counter = 0;
+ bool first = true;
+ WOLF_INFO("Start scan Visualize ======================================================================");
+ for (auto key_frame : trajectory)
{
- if (key_frame->isKey())
+ if (key_frame->isKey() or first)
{
for (auto cptr : key_frame->getCaptureList())
{
- if (cptr->getType() == "LASER 2D")
+ if (cptr->getType() == "CaptureLaser2D")
{
// Preparation
CaptureLaser2DPtr cptr_laser = std::static_pointer_cast<CaptureLaser2D>(cptr);
@@ -119,27 +131,67 @@ pcl::PointCloud<pcl::PointXYZ> WolfRosScanVisualizer::buildPointCloud(const Prob
Eigen::Affine3f T_world_sensor = T_world_robot * T_robot_sensor;
// Transform laser scan point cloud
- pcl::PointCloud<pcl::PointXYZ> aux_cloud;
+ pcl::PointCloud<pcl::PointXYZRGB> aux_cloud;
transformPointCloudFromLaserScan(aux_cloud, cptr_laser, T_world_sensor);
+ for(auto& p : aux_cloud)
+ {
+ // pack r/g/b into rgb
+ uint8_t r, g, b; // Example: Red color
+ double z = 0;
+ switch (counter) {
+ case 0:
+ r = 255;
+ g = 165;
+ b = 0;
+ z = 0.4;
+ break;
+ case 1:
+ r = 255;
+ g = b = 0;
+ z = 0.3;
+ break;
+ case 2:
+ r = 0;
+ g = 255;
+ b = 0;
+ z = 0.2;
+ break;
+ case 3:
+ r = 0;
+ g = 0;
+ b = 255;
+ z = 0.1;
+ break;
+ default:
+ r = 128;
+ g = 128;
+ b = 192;
+ }
+ uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
+ p.rgb = *reinterpret_cast<float*>(&rgb);
+ p.z = z;
+ }
agg_cloud = agg_cloud + aux_cloud;
}
+ ++counter;
+ first = false;
}
}
}
+ WOLF_INFO("End scan Visualize ======================================================================");
return agg_cloud;
}
-pcl::PointCloud<pcl::PointXYZ> WolfRosScanVisualizer::buildPointCloudWithMeasurements(const ProblemPtr problem)
+pcl::PointCloud<pcl::PointXYZRGB> WolfRosScanVisualizer::buildPointCloudWithMeasurements(const ProblemPtr problem)
{
FrameBasePtr last, current = nullptr;
FrameBasePtrList key_frames;
- pcl::PointCloud<pcl::PointXYZ> agg_cloud;
+ pcl::PointCloud<pcl::PointXYZRGB> agg_cloud;
Eigen::Affine3f T_world_n = Eigen::Affine3f::Identity();
auto trajectory_frames = problem->getTrajectory()->getFrameList();
std::copy_if(trajectory_frames.rbegin(),
trajectory_frames.rend(),
std::back_inserter(key_frames),
[](FrameBasePtr _frame) { return _frame->isKey(); });
- WOLF_TRACE("KEY FRAMES EMPTY? ", key_frames.empty());
for (const auto& key_frame : key_frames)
{
if (last == nullptr)
@@ -151,7 +203,7 @@ pcl::PointCloud<pcl::PointXYZ> WolfRosScanVisualizer::buildPointCloudWithMeasure
// Get all laser captures from the key frame
auto cptr_list = key_frame->getCaptureList();
std::copy_if(cptr_list.begin(), cptr_list.end(), std::back_inserter(laser_cptrs), [](CaptureBasePtr _cptr) {
- return (_cptr->getType() == "LASER 2D");
+ return (_cptr->getType() == "CaptureLaser2D");
});
FeatureBasePtrList icp_aligns;
// From the first LASER 2D capture we get the ICP ALIGN features. We are assuming that the list of LASER 2D
@@ -162,7 +214,7 @@ pcl::PointCloud<pcl::PointXYZ> WolfRosScanVisualizer::buildPointCloudWithMeasure
// std::for_each(ftr_list.begin(), ftr_list.end(), [](FeatureBasePtr _ftr){WOLF_TRACE("TYPE ",
// _ftr->getType());});
std::copy_if(ftr_list.begin(), ftr_list.end(), std::back_inserter(icp_aligns), [](FeatureBasePtr _ftr) {
- return (_ftr->getType() == "ICP ALIGN");
+ return (_ftr->getType() == "FeatureIcpAlign");
});
FeatureICPAlignPtr last_icp_align;
if (not icp_aligns.empty())
@@ -178,7 +230,7 @@ pcl::PointCloud<pcl::PointXYZ> WolfRosScanVisualizer::buildPointCloudWithMeasure
CaptureLaser2DPtr cptr_laser = std::static_pointer_cast<CaptureLaser2D>(
icp_aligns.front()->getCapture());
- pcl::PointCloud<pcl::PointXYZ> aux_cloud;
+ pcl::PointCloud<pcl::PointXYZRGB> aux_cloud;
transformPointCloudFromLaserScan(aux_cloud, cptr_laser, T_world_n);
agg_cloud = agg_cloud + aux_cloud;
}
diff --git a/src/wolf_ros_subscriber_laser2D.cpp b/src/wolf_ros_subscriber_laser2D.cpp
index cbb0d6cf5485d5f036fc7f532b3087a9fc93a51f..4ee3f2974c201c2bf3db3ab3e7f3f5c8890dc4fb 100644
--- a/src/wolf_ros_subscriber_laser2D.cpp
+++ b/src/wolf_ros_subscriber_laser2D.cpp
@@ -1,4 +1,5 @@
#include "wolf_ros_subscriber_laser2D.h"
+#include <limits>
namespace wolf
{
@@ -18,7 +19,29 @@ void SubscriberWrapperLaser2D::callback(const sensor_msgs::LaserScan::ConstPtr&
{
CaptureLaser2DPtr new_capture = std::make_shared<CaptureLaser2D>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_, msg->ranges);
- std::cout << "Cap Seq " << new_capture->id() << " " << msg->header.seq << "\n";
+ // auto n = std::min(msg->ranges.size(), new_capture->getScan().ranges_raw_.size());
+ // auto total = 0;
+ // for(int i = 0; i < n; ++i)
+ // {
+ // if (msg->ranges[i] != std::numeric_limits<double>::infinity() and new_capture->getScan().ranges_raw_[i] != std::numeric_limits<double>::infinity())
+ // total += std::abs(msg->ranges[i] - new_capture->getScan().ranges_raw_[i]);
+ // else if (msg->ranges[i] != std::numeric_limits<double>::infinity() or new_capture->getScan().ranges_raw_[i] != std::numeric_limits<double>::infinity())
+ // {
+ // total += 10e3;
+ // }
+ // }
+ // std::cout << "Total error " << total << "\n";
+
+ // auto sensor_laser_ptr = std::dynamic_pointer_cast<SensorLaser2D>(sensor_ptr_);
+ // auto params = sensor_laser_ptr->getScanParams();
+
+ // WOLF_INFO("MSG ", msg->angle_increment, " PARAMS ", params.angle_step_);
+ // WOLF_INFO("MSG ", msg->angle_max, " PARAMS " , params.angle_max_);
+ // WOLF_INFO("MSG ", msg->angle_min, " PARAMS " , params.angle_min_);
+ // WOLF_INFO("MSG ", msg->range_max, " PARAMS " , params.range_max_);
+ // WOLF_INFO("MSG ", msg->range_min, " PARAMS " , params.range_min_);
+
+ // std::cout << "Cap Seq " << new_capture->id() << " " << msg->header.seq << "\n";
//Currently this line is just to bypass the ROS "auto config". from the ROS msg. Maybe we want to explore
//getting the params from the msg instead of the yaml file in the future.
laser_intrinsics_set_ = true;
@@ -48,19 +71,19 @@ void SubscriberWrapperLaser2D::callback(const sensor_msgs::LaserScan::ConstPtr&
}
new_capture->process();
- if (msg->header.seq == 14031)
- {
- auto problem = sensor_ptr_->getProblem();
- for(const auto& frame : problem->getTrajectory()->getFrameList())
- {
- if(frame->isKey()){
- for(const auto& cap : frame->getCaptureList())
- {
- if(cap->getType() == "LASER 2D") std::cout << "Frm Cap " << frame->id() << " " << cap->id() << "\n";
- }
- }
- }
- }
+ // if (msg->header.seq == 14031)
+ // {
+ // auto problem = sensor_ptr_->getProblem();
+ // for(const auto& frame : problem->getTrajectory()->getFrameList())
+ // {
+ // if(frame->isKey()){
+ // for(const auto& cap : frame->getCaptureList())
+ // {
+ // if(cap->getType() == "LASER 2D") std::cout << "Frm Cap " << frame->id() << " " << cap->id() << "\n";
+ // }
+ // }
+ // }
+ // }
}
std::shared_ptr<SubscriberWrapper> SubscriberWrapperLaser2D::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr)