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

Merge branch 'devel' of...

Merge branch 'devel' of ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git into devel
parents 36164672 63ae3139
No related branches found
No related tags found
2 merge requests!11new release,!10new release
...@@ -12,47 +12,47 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name, ...@@ -12,47 +12,47 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name,
{ {
// LOAD PARAMETERS (all optionals) --------------------------------------------------- // LOAD PARAMETERS (all optionals) ---------------------------------------------------
// General // General
map_frame_id_ = getParamWithDefault<std::string>(_server, prefix_ + "map_frame_id", "map"); map_frame_id_ = getParamWithDefault<std::string>(_server, prefix_ + "/map_frame_id", "map");
viz_overlapped_factors_ = getParamWithDefault<bool> (_server, prefix_ + "viz_overlapped_factors", false); viz_overlapped_factors_ = getParamWithDefault<bool> (_server, prefix_ + "/viz_overlapped_factors", false);
text_scale_ = getParamWithDefault<double> (_server, prefix_ + "text_scale", 0.5); text_scale_ = getParamWithDefault<double> (_server, prefix_ + "/text_scale", 0.5);
viz_scale_ = getParamWithDefault<double> (_server, prefix_ + "viz_scale", 1); viz_scale_ = getParamWithDefault<double> (_server, prefix_ + "/viz_scale", 1);
// landmarks // landmarks
landmark_text_z_offset_ = getParamWithDefault<double> (_server, prefix_ + "landmark_text_z_offset", 1); landmark_text_z_offset_ = getParamWithDefault<double> (_server, prefix_ + "/landmark_text_z_offset", 1);
landmark_width_ = getParamWithDefault<double> (_server, prefix_ + "landmark_width", 0.1); landmark_width_ = getParamWithDefault<double> (_server, prefix_ + "/landmark_width", 0.1);
landmark_length_ = getParamWithDefault<double> (_server, prefix_ + "landmark_length", 1); landmark_length_ = getParamWithDefault<double> (_server, prefix_ + "/landmark_length", 1);
// frames // frames
frame_width_ = getParamWithDefault<double> (_server, prefix_ + "frame_width", 0.1); frame_width_ = getParamWithDefault<double> (_server, prefix_ + "/frame_width", 0.1);
frame_length_ = getParamWithDefault<double> (_server, prefix_ + "frame_length", 1); frame_length_ = getParamWithDefault<double> (_server, prefix_ + "/frame_length", 1);
frame_color_.r = getParamWithDefault<double> (_server, prefix_ + "frame_color_r", 1); frame_color_.r = getParamWithDefault<double> (_server, prefix_ + "/frame_color_r", 1);
frame_color_.g = getParamWithDefault<double> (_server, prefix_ + "frame_color_g", 0.8); frame_color_.g = getParamWithDefault<double> (_server, prefix_ + "/frame_color_g", 0.8);
frame_color_.b = getParamWithDefault<double> (_server, prefix_ + "frame_color_b", 0.0); frame_color_.b = getParamWithDefault<double> (_server, prefix_ + "/frame_color_b", 0.0);
frame_color_.a = getParamWithDefault<double> (_server, prefix_ + "frame_color_a", 1); frame_color_.a = getParamWithDefault<double> (_server, prefix_ + "/frame_color_a", 1);
// factors // factors
factors_width_ = getParamWithDefault<double> (_server, prefix_ + "factors_width", 0.02); factors_width_ = getParamWithDefault<double> (_server, prefix_ + "/factors_width", 0.02);
factors_absolute_height_= getParamWithDefault<double> (_server, prefix_ + "factors_absolute_height", 2); factors_absolute_height_= getParamWithDefault<double> (_server, prefix_ + "/factors_absolute_height", 2);
factor_abs_color_.r = getParamWithDefault<double> (_server, prefix_ + "factor_abs_color_r", 0.9); factor_abs_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_r", 0.9);
factor_abs_color_.g = getParamWithDefault<double> (_server, prefix_ + "factor_abs_color_g", 0.2); factor_abs_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_g", 0.2);
factor_abs_color_.b = getParamWithDefault<double> (_server, prefix_ + "factor_abs_color_b", 0.6); factor_abs_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_b", 0.6);
factor_abs_color_.a = getParamWithDefault<double> (_server, prefix_ + "factor_abs_color_a", 1); factor_abs_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_abs_color_a", 1);
factor_motion_color_.r = getParamWithDefault<double> (_server, prefix_ + "factor_motion_color_r", 1); factor_motion_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_r", 1);
factor_motion_color_.g = getParamWithDefault<double> (_server, prefix_ + "factor_motion_color_g", 1); factor_motion_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_g", 1);
factor_motion_color_.b = getParamWithDefault<double> (_server, prefix_ + "factor_motion_color_b", 0); factor_motion_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_b", 0);
factor_motion_color_.a = getParamWithDefault<double> (_server, prefix_ + "factor_motion_color_a", 1); factor_motion_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_motion_color_a", 1);
factor_loop_color_.r = getParamWithDefault<double> (_server, prefix_ + "factor_loop_color_r", 1); factor_loop_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_r", 1);
factor_loop_color_.g = getParamWithDefault<double> (_server, prefix_ + "factor_loop_color_g", 0); factor_loop_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_g", 0);
factor_loop_color_.b = getParamWithDefault<double> (_server, prefix_ + "factor_loop_color_b", 0); factor_loop_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_b", 0);
factor_loop_color_.a = getParamWithDefault<double> (_server, prefix_ + "factor_loop_color_a", 1); factor_loop_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_loop_color_a", 1);
factor_lmk_color_.r = getParamWithDefault<double> (_server, prefix_ + "factor_lmk_color_r", 0); factor_lmk_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_r", 0);
factor_lmk_color_.g = getParamWithDefault<double> (_server, prefix_ + "factor_lmk_color_g", 0); factor_lmk_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_g", 0);
factor_lmk_color_.b = getParamWithDefault<double> (_server, prefix_ + "factor_lmk_color_b", 1); factor_lmk_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_b", 1);
factor_lmk_color_.a = getParamWithDefault<double> (_server, prefix_ + "factor_lmk_color_a", 1); factor_lmk_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_lmk_color_a", 1);
factor_geom_color_.r = getParamWithDefault<double> (_server, prefix_ + "factor_geom_color_r", 0); factor_geom_color_.r = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_r", 0);
factor_geom_color_.g = getParamWithDefault<double> (_server, prefix_ + "factor_geom_color_g", 1); factor_geom_color_.g = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_g", 1);
factor_geom_color_.b = getParamWithDefault<double> (_server, prefix_ + "factor_geom_color_b", 1); factor_geom_color_.b = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_b", 1);
factor_geom_color_.a = getParamWithDefault<double> (_server, prefix_ + "factor_geom_color_a", 1); factor_geom_color_.a = getParamWithDefault<double> (_server, prefix_ + "/factor_geom_color_a", 1);
// INIT MARKERS --------------------------------------------------- // INIT MARKERS ---------------------------------------------------
// factor markers message // factor markers message
...@@ -369,7 +369,7 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac, ...@@ -369,7 +369,7 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
// FRAME // FRAME
if (fac->getFrameOther() != nullptr) { if (fac->getFrameOther() != nullptr) {
// special case: Motion from ProcessorImu // special case: Motion from ProcessorMotion
auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(fac->getProcessor()); auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(fac->getProcessor());
if (proc_motion and fac->getCaptureOther()) if (proc_motion and fac->getCaptureOther())
{ {
...@@ -438,7 +438,7 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac, ...@@ -438,7 +438,7 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
} }
// CAPTURE (with Frame) // CAPTURE (with Frame)
else if (fac->getCaptureOther() != nullptr && else if (fac->getCaptureOther() != nullptr &&
fac->getCaptureOther()->getFrame() != nullptr) { fac->getCaptureOther()->getFrame() != nullptr) {
point2.x = fac->getCaptureOther()->getFrame()->getP()->getState()(0); point2.x = fac->getCaptureOther()->getFrame()->getP()->getState()(0);
point2.y = fac->getCaptureOther()->getFrame()->getP()->getState()(1); point2.y = fac->getCaptureOther()->getFrame()->getP()->getState()(1);
if (fac->getProblem()->getDim() == 3) if (fac->getProblem()->getDim() == 3)
...@@ -450,14 +450,10 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac, ...@@ -450,14 +450,10 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
else if (fac->getFeatureOther() != nullptr && else if (fac->getFeatureOther() != nullptr &&
fac->getFeatureOther()->getCapture() != nullptr && fac->getFeatureOther()->getCapture() != nullptr &&
fac->getFeatureOther()->getCapture()->getFrame() != nullptr) { fac->getFeatureOther()->getCapture()->getFrame() != nullptr) {
point2.x = point2.x = fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(0);
fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(0); point2.y = fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(1);
point2.y =
fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(1);
if (fac->getProblem()->getDim() == 3) if (fac->getProblem()->getDim() == 3)
point2.z = point2.z = fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(2);
fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(
2);
else else
point2.z = 0; point2.z = 0;
} }
...@@ -499,14 +495,14 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac, ...@@ -499,14 +495,14 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
fac_marker.colors.push_back(color); fac_marker.colors.push_back(color);
fac_marker.colors.push_back(color);// 2 times because of 2 points fac_marker.colors.push_back(color);// 2 times because of 2 points
fac_marker.ns = std::string("factors_"+fac->getTopology()); fac_marker.ns = std::string("factors_"+(fac->getProcessor() ? fac->getProcessor()->getName() : "unnamed_processor"));
// TEXT MARKER -------------------------------------------------------- // TEXT MARKER --------------------------------------------------------
fac_text_marker.text = std::to_string(fac->id()); fac_text_marker.text = std::to_string(fac->id());
fac_text_marker.pose.position.x = (point1.x + point2.x)/(double) 2; fac_text_marker.pose.position.x = (point1.x + point2.x)/(double) 2;
fac_text_marker.pose.position.y = (point1.y + point2.y)/(double) 2; fac_text_marker.pose.position.y = (point1.y + point2.y)/(double) 2;
fac_text_marker.pose.position.z = fac_marker.pose.position.z; fac_text_marker.pose.position.z = fac_marker.pose.position.z;
fac_text_marker.ns = "factors_text"; fac_text_marker.ns = std::string("factors_text_"+(fac->getProcessor() ? fac->getProcessor()->getName() : "unnamed_processor"));
} }
void PublisherGraph::fillFrameMarker(FrameBaseConstPtr frm, void PublisherGraph::fillFrameMarker(FrameBaseConstPtr frm,
......
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