diff --git a/include/publisher_falko.h b/include/publisher_falko.h
index e4a965e387773e06fad6bedadb6c9cfdd396a925..b660abe0d27303a86cf965d63af7ba9a0ea5d9d1 100644
--- a/include/publisher_falko.h
+++ b/include/publisher_falko.h
@@ -51,12 +51,12 @@ class PublisherFalko : public Publisher
std_msgs::ColorRGBA marker_color_target_;
std_msgs::ColorRGBA marker_color_reference_;
- bool extrinsics_;
- SensorBasePtr sensor_;
- std::string frame_id_, map_frame_id_;
+ bool extrinsics_;
+ ProcessorBaseConstPtr processor_;
+ std::string map_frame_id_;
- FrameBasePtr last_frame;
- FrameBasePtr KF_old;
+ FrameBaseConstPtr last_frame;
+ FrameBaseConstPtr KF_old;
ros::Publisher pub_keypoints_;
ros::Publisher pub_marker_scene_target_;
@@ -64,10 +64,10 @@ class PublisherFalko : public Publisher
ros::Publisher pub_marker_associations_;
// Reference scene info
- std::map<CaptureBasePtr, visualization_msgs::Marker> map_markers_reference_scenes_;
+ std::map<CaptureBaseConstPtr, visualization_msgs::Marker> map_markers_reference_scenes_;
public:
- PublisherFalko(const std::string &_unique_name, const ParamsServer &_server, const ProblemPtr _problem);
+ PublisherFalko(const std::string &_unique_name, const ParamsServer &_server, ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherFalko);
virtual ~PublisherFalko(){};
@@ -76,26 +76,31 @@ class PublisherFalko : public Publisher
void publishDerived() override;
- void publishScene(std::vector<falkolib::FALKO> &_keypoints, laserscanutils::LaserScan &_scan,
- Eigen::Vector3d &_p_rob, Eigen::Quaterniond &_q_rob, ros::Publisher &_pub_lines,
- std_msgs::ColorRGBA _marker_color, int & _id, CaptureBasePtr cap);
+ void publishScene(const std::vector<falkolib::FALKO> &_keypoints,
+ const laserscanutils::LaserScan &_scan,
+ const Eigen::Vector3d &_p_rob,
+ const Eigen::Quaterniond &_q_rob,
+ const ros::Publisher &_pub_lines,
+ std_msgs::ColorRGBA _marker_color,
+ int _id,
+ CaptureBaseConstPtr cap);
void publishEmpty();
- void publishAssociations(std::vector<std::pair<int, int>> &associations_, CaptureBasePtr &cap_target,
- CaptureBasePtr &cap_reference);
+ void publishAssociations(const std::vector<std::pair<int, int>> &associations_,
+ CaptureBaseConstPtr cap_target,
+ CaptureBaseConstPtr cap_reference);
- void getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::LaserScan &_scan,
+ void getKeypointsAndScan(FrameBaseConstPtr _frame,
+ laserscanutils::LaserScan &_scan,
std::vector<falkolib::FALKO> &_keypoints);
void getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, VectorComposite current_state);
protected:
- //Eigen::Quaterniond q_frame_;
- //Eigen::Vector3d t_frame_;
int id_old_ = 1;
- std::map<FrameBasePtr, VectorComposite> map_frame_states;
+ std::map<FrameBaseConstPtr, VectorComposite> map_frame_states;
};
} // namespace wolf
diff --git a/include/publisher_laser_map.h b/include/publisher_laser_map.h
index 1af60603fbdc5e6d76f042ec00ead531066540af..c018cfb8fa86893f082e182d1ecf9a98603966de 100644
--- a/include/publisher_laser_map.h
+++ b/include/publisher_laser_map.h
@@ -48,7 +48,7 @@ namespace wolf
struct ScanData
{
- CaptureLaser2dPtr capture_;
+ CaptureLaser2dConstPtr capture_;
laserscanutils::LaserScanParams params_;
Eigen::MatrixXf local_points_;
pcl::PointCloud<pcl::PointXYZRGB> local_pc_;
@@ -78,14 +78,14 @@ class PublisherLaserMap: public Publisher
int pc_r_, pc_g_, pc_b_;
// std::maps
- std::map<FrameBasePtr,std::list<ScanData>> scans_;
- std::map<FrameBasePtr,Eigen::Vector3d> current_trajectory_, last_trajectory_occ_, last_trajectory_pc_;
+ std::map<FrameBaseConstPtr,std::list<ScanData>> scans_;
+ std::map<FrameBaseConstPtr,Eigen::Vector3d> current_trajectory_, last_trajectory_occ_, last_trajectory_pc_;
public:
PublisherLaserMap(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem);
+ const ParamsServer& _server,
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherLaserMap);
virtual ~PublisherLaserMap(){};
@@ -94,9 +94,9 @@ class PublisherLaserMap: public Publisher
protected:
void updateTrajectory();
- bool trajectoryChanged(const std::map<FrameBasePtr,Eigen::Vector3d>& _last_trajectory,
- const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const;
- void storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture);
+ bool trajectoryChanged(const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _last_trajectory,
+ const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _current_trajectory) const;
+ void storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPtr capture);
// occmap
void resetOccMap();
diff --git a/include/publisher_odom_icp.h b/include/publisher_odom_icp.h
index 1c60816d10d6d4d37b3bd5683c6da02e526b4550..1c129cbbb8eded621214cd5f28437d5a3b631b37 100644
--- a/include/publisher_odom_icp.h
+++ b/include/publisher_odom_icp.h
@@ -42,14 +42,14 @@ class PublisherOdomIcp: public Publisher
{
protected:
- ProcessorOdomIcpPtr processor_odom_icp_;
+ ProcessorOdomIcpConstPtr processor_odom_icp_;
geometry_msgs::PoseStamped msg_;
public:
PublisherOdomIcp(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem);
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherOdomIcp);
virtual ~PublisherOdomIcp(){};
diff --git a/src/publisher_falko.cpp b/src/publisher_falko.cpp
index fc33e05a8554f09fae26129a5e52295b6d0ff297..fbe7214132f0076b4b94eafbd184b15cb6fa7e02 100644
--- a/src/publisher_falko.cpp
+++ b/src/publisher_falko.cpp
@@ -25,8 +25,9 @@
namespace wolf {
-PublisherFalko::PublisherFalko(const std::string &_unique_name, const ParamsServer &_server,
- const ProblemPtr _problem)
+PublisherFalko::PublisherFalko(const std::string &_unique_name,
+ const ParamsServer &_server,
+ ProblemConstPtr _problem)
: Publisher(_unique_name, _server, _problem)
{
Eigen::Vector4d marker_color_v;
@@ -45,11 +46,10 @@ PublisherFalko::PublisherFalko(const std::string &_unique_name, const ParamsServ
max_points_ = getParamWithDefault<int>(_server, prefix_ + "/max_points", 1e3);
extrinsics_ = _server.getParam<bool>(prefix_ + "/extrinsics");
- sensor_ = _problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
- // dynamic_ptr_cast(sensor_)
- if (!sensor_)
- throw std::runtime_error("sensor not found");
- frame_id_ = _server.getParam<std::string>(prefix_ + "/frame_id");
+ processor_ = _problem->findProcessor(_server.getParam<std::string>(prefix_ + "/processor_name"));
+
+ if (!processor_)
+ throw std::runtime_error("processor not found");
}
void PublisherFalko::initialize(ros::NodeHandle &nh, const std::string &topic)
@@ -79,7 +79,7 @@ void PublisherFalko::publishDerived()
if (!KF)
return;
- auto cap = KF->getCaptureOf(sensor_, "CaptureLaser2d");
+ auto cap = KF->getCaptureOf(processor_->getSensor(), "CaptureLaser2d");
if (not cap)
return;
@@ -89,9 +89,7 @@ void PublisherFalko::publishDerived()
// state not ready
if (current_state.count('P') == 0 or current_state.count('O') == 0 or not loc_ts.ok())
- {
return;
- }
// 2D robot position and yaw
Eigen::Vector3d p = Eigen::Vector3d::Zero();
@@ -110,23 +108,6 @@ void PublisherFalko::publishDerived()
if (p(0) == 0 and p(1) == 0)
return;
- // LOOK FOR REFERENCE SCENE
- auto processor_list = sensor_->getProcessorList();
-
- ProcessorBasePtr processor_falko;
- for (auto processor : processor_list)
- {
- if (processor->getType() == "ProcessorLoopClosureFalko")
- {
- processor_falko = processor;
- break;
- }
- }
- if (!processor_falko)
- return;
-
- // FactorBasePtrList factor_list;
- // KF->getFactorList(factor_list);
if (KF == KF_old and KF_old != nullptr)
return;
@@ -137,10 +118,9 @@ void PublisherFalko::publishDerived()
id = 1;
- for (const FactorBasePtr &factor : KF->getConstrainedByList())
- if (factor->getProcessor() == processor_falko)
+ for (auto factor : KF->getConstrainedByList())
+ if (factor->getProcessor() == processor_)
{
-
auto ftr = factor->getFeature();
auto frame_other = ftr->getFrame();
@@ -166,31 +146,25 @@ void PublisherFalko::publishDerived()
std::cout << "keypoints target: " << keypoints.size() << std::endl;
std::cout << "keypoints reference: " << keypoints_other.size() << std::endl;
- // std::cout << "scan : " << std::endl;
- // for (int i = 0; i < scan.ranges_raw_.size(); i++)
- // {
- // std::cout << scan.ranges_raw_[i] << ",";
- // }
- // std::cout << "scan other : " << std::endl;
- // for (int i = 0; i < scan_other.ranges_raw_.size(); i++)
- // {
- // std::cout << scan_other.ranges_raw_[i] << ",";
- // }
-
publishScene(keypoints_other, scan_other, p_other, q_other, pub_marker_scene_reference_,
marker_color_reference_, id, cap);
// Publish associations between scenes
- auto matchings_list =
- std::static_pointer_cast<ProcessorLoopClosureFalkoAhtBsc>(processor_falko)->match_list_;
+ auto processor_aht_bsc = std::dynamic_pointer_cast<const ProcessorLoopClosureFalkoAhtBsc>(processor_);
- for (auto match : matchings_list)
+ if (processor_aht_bsc)
{
- if (match->capture_target == cap)
+ auto matchings_list = processor_aht_bsc->getMatchList();
+
+ for (auto match : matchings_list)
{
- auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(match);
- auto associations = match_falko->match_falko_->associations;
- publishAssociations(associations, cap, match->capture_reference);
+ if (match->capture_target == cap)
+ {
+ auto match_falko = std::static_pointer_cast<const MatchLoopClosureFalko>(match);
+ publishAssociations(match_falko->match_falko_->associations,
+ cap,
+ match->capture_reference);
+ }
}
}
}
@@ -200,8 +174,9 @@ void PublisherFalko::publishDerived()
return;
}
-void PublisherFalko::publishAssociations(std::vector<std::pair<int, int>> &associations_, CaptureBasePtr &cap_target,
- CaptureBasePtr &cap_reference)
+void PublisherFalko::publishAssociations(const std::vector<std::pair<int, int>> &associations_,
+ CaptureBaseConstPtr cap_target,
+ CaptureBaseConstPtr cap_reference)
{
std_msgs::ColorRGBA marker_color;
@@ -255,11 +230,16 @@ void PublisherFalko::publishAssociations(std::vector<std::pair<int, int>> &assoc
pub_marker_associations_.publish(asso_marker);
}
-void PublisherFalko::publishScene(std::vector<falkolib::FALKO> &_keypoints, laserscanutils::LaserScan &_scan,
- Eigen::Vector3d &_p_rob, Eigen::Quaterniond &_q_rob, ros::Publisher &_pub_lines,
- std_msgs::ColorRGBA _marker_color, int &_id, CaptureBasePtr cap)
+void PublisherFalko::publishScene(const std::vector<falkolib::FALKO> &_keypoints,
+ const laserscanutils::LaserScan &_scan,
+ const Eigen::Vector3d &_p_rob,
+ const Eigen::Quaterniond &_q_rob,
+ const ros::Publisher &_pub_lines,
+ std_msgs::ColorRGBA _marker_color,
+ int _id,
+ CaptureBaseConstPtr cap)
{
- auto sensor_laser = std::static_pointer_cast<SensorLaser2d>(sensor_);
+ auto sensor_laser = std::static_pointer_cast<const SensorLaser2d>(processor_->getSensor());
auto scan_params = sensor_laser->getScanParams();
std::vector<double> angle_discretization;
@@ -312,7 +292,7 @@ void PublisherFalko::publishScene(std::vector<falkolib::FALKO> &_keypoints, lase
if (_id == 0)
{
map_markers_reference_scenes_.insert(
- std::pair<CaptureBasePtr, visualization_msgs::Marker>(cap, marker_msg_falko_scene));
+ std::pair<CaptureBaseConstPtr, visualization_msgs::Marker>(cap, marker_msg_falko_scene));
}
// Close the area
@@ -351,11 +331,12 @@ void PublisherFalko::publishEmpty()
}
}
-void PublisherFalko::getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::LaserScan &_scan,
+void PublisherFalko::getKeypointsAndScan(FrameBaseConstPtr _frame,
+ laserscanutils::LaserScan &_scan,
std::vector<falkolib::FALKO> &_keypoints)
{
- auto cap = _frame->getCaptureOf(sensor_, "CaptureLaser2d");
- auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
+ auto cap = _frame->getCaptureOf(processor_->getSensor(), "CaptureLaser2d");
+ auto cap_laser = std::dynamic_pointer_cast<const CaptureLaser2d>(cap);
_scan = cap_laser->getScan();
@@ -363,7 +344,7 @@ void PublisherFalko::getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::L
{
if (feat->getType() == "FeatureSceneFalko")
{
- auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ auto feat_falko = std::static_pointer_cast<const FeatureSceneFalkoBsc>(feat);
_keypoints = feat_falko->getScene()->keypoints_list_;
}
}
@@ -372,9 +353,9 @@ void PublisherFalko::getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::L
void PublisherFalko::getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, VectorComposite _state)
{
- p.head(2) = _state['P']; //+ Eigen::Rotation2Dd(_state['O'](0)) * sensor_->getP()->getState().head(2);
+ p.head(2) = _state['P']; //+ Eigen::Rotation2Dd(_state['O'](0)) * processor_->getSensor()->getP()->getState().head(2);
q = Eigen::Quaterniond(
- Eigen::AngleAxisd(_state['O'](0) + sensor_->getO()->getState()(0), Eigen::Vector3d::UnitZ()));
+ Eigen::AngleAxisd(_state['O'](0) + processor_->getSensor()->getO()->getState()(0), Eigen::Vector3d::UnitZ()));
}
WOLF_REGISTER_PUBLISHER(PublisherFalko)
diff --git a/src/publisher_laser_map.cpp b/src/publisher_laser_map.cpp
index 39f140d78b059da289cc0efb831677b9079acd69..b699f1f42f1eee61a87d464a4a4de2140faaa424 100644
--- a/src/publisher_laser_map.cpp
+++ b/src/publisher_laser_map.cpp
@@ -35,8 +35,8 @@ namespace wolf
{
PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem) :
+ const ParamsServer& _server,
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem),
n_cells_(Eigen::Vector2i::Constant(100)),
resized_(false),
@@ -135,7 +135,8 @@ void PublisherLaserMap::updateTrajectory()
scans_.clear();
// copy new trajectory poses and scans
- for (auto frame_pair : problem_->getTrajectory()->getFrameMap())
+ auto frame_map = problem_->getTrajectory()->getFrameMap();
+ for (auto frame_pair : frame_map)
{
if (frame_pair.second->isRemoving())
continue;
@@ -148,7 +149,7 @@ void PublisherLaserMap::updateTrajectory()
auto cap_list = frame_pair.second->getCaptureList();
for (auto cap : cap_list)
{
- auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
+ auto cap_laser = std::dynamic_pointer_cast<const CaptureLaser2d>(cap);
if (cap_laser and not cap_laser->isRemoving())
storeScan(frame_pair.second, cap_laser);
}
@@ -162,11 +163,11 @@ void PublisherLaserMap::updateTrajectory()
}
}
-void PublisherLaserMap::storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture)
+void PublisherLaserMap::storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPtr capture)
{
ScanData scan_data;
scan_data.capture_ = capture;
- scan_data.params_ = std::static_pointer_cast<SensorLaser2d>(capture->getSensor())->getScanParams();
+ scan_data.params_ = std::static_pointer_cast<const SensorLaser2d>(capture->getSensor())->getScanParams();
// local points
Eigen::Vector2d laser_extrinsics_p = capture->getSensor()->getP()->getState();
@@ -210,8 +211,8 @@ void PublisherLaserMap::storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture)
scans_.at(frame).push_back(scan_data);
}
-bool PublisherLaserMap::trajectoryChanged(const std::map<FrameBasePtr,Eigen::Vector3d>& _last_trajectory,
- const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const
+bool PublisherLaserMap::trajectoryChanged(const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _last_trajectory,
+ const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _current_trajectory) const
{
// check if trajectory changed enough
for (auto frame_pos : _last_trajectory)
diff --git a/src/publisher_odom_icp.cpp b/src/publisher_odom_icp.cpp
index c9ed4ffb0c6b8af35be2c3ee82b1454227f7b456..eef4c179c0ad711788dff2a7a106ab14bb500716 100644
--- a/src/publisher_odom_icp.cpp
+++ b/src/publisher_odom_icp.cpp
@@ -32,7 +32,7 @@ namespace wolf
PublisherOdomIcp::PublisherOdomIcp(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem) :
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem),
processor_odom_icp_(nullptr)
{
@@ -45,7 +45,7 @@ PublisherOdomIcp::PublisherOdomIcp(const std::string& _unique_name,
{
if (proc->getName() == processor_name)
{
- processor_odom_icp_ = std::dynamic_pointer_cast<ProcessorOdomIcp>(proc);
+ processor_odom_icp_ = std::dynamic_pointer_cast<const ProcessorOdomIcp>(proc);
if (not processor_odom_icp_)
throw std::runtime_error("PublisherOdomIcp requires a processor of type 'ProcessorOdomIcp'.");
break;