Skip to content
GitLab
Commit 16f7f13b authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

adapting to new const-nonconst API

parent 77975d2a
Hide whitespace changes
Inline Side-by-side
include/publisher_falko.h
View file @ 16f7f13b
......@@ -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
......
include/publisher_laser_map.h
View file @ 16f7f13b
......@@ -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();
......
include/publisher_odom_icp.h
View file @ 16f7f13b
......@@ -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(){};
......
src/publisher_falko.cpp
View file @ 16f7f13b
......@@ -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)
......
src/publisher_laser_map.cpp
View file @ 16f7f13b
......@@ -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)
......
src/publisher_odom_icp.cpp
View file @ 16f7f13b
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment