From 1c858bada57fa784057bfb4b75cddc12d997c708 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 24 Dec 2021 12:53:15 +0100
Subject: [PATCH] color for frame velocity
---
include/publisher_graph.h | 2 +-
src/publisher_graph.cpp | 40 ++++++++++-----------------------------
2 files changed, 11 insertions(+), 31 deletions(-)
diff --git a/include/publisher_graph.h b/include/publisher_graph.h
index cc75f37..7722dd7 100644
--- a/include/publisher_graph.h
+++ b/include/publisher_graph.h
@@ -89,7 +89,7 @@ class PublisherGraph: public Publisher
std::string map_frame_id_;
bool viz_overlapped_factors_, viz_inactive_factors_;
double viz_scale_, text_scale_, factors_width_, factors_absolute_height_, landmark_text_z_offset_, landmark_width_, landmark_length_, frame_width_, frame_length_, frame_vel_scale_;
- std_msgs::ColorRGBA frame_color_, factor_abs_color_, factor_motion_color_, factor_loop_color_, factor_lmk_color_, factor_geom_color_, factor_other_color_;
+ std_msgs::ColorRGBA frame_vel_color_, factor_abs_color_, factor_motion_color_, factor_loop_color_, factor_lmk_color_, factor_geom_color_, factor_other_color_;
// auxiliar variables
unsigned int landmark_max_hits_;
diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp
index 026b204..1dd2b70 100644
--- a/src/publisher_graph.cpp
+++ b/src/publisher_graph.cpp
@@ -51,12 +51,12 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name,
frame_length_ = getParamWithDefault<double> (_server, prefix_ + "/frame_length", 1);
frame_vel_scale_ = getParamWithDefault<double> (_server, prefix_ + "/frame_vel_scale", 0.1);
color = getParamWithDefault<Eigen::Vector4d>(_server,
- prefix_ + "/frame_color",
- (Eigen::Vector4d() << 1, 0.8, 0, 1).finished());
- frame_color_.r = color(0);
- frame_color_.g = color(1);
- frame_color_.b = color(2);
- frame_color_.a = color(3);
+ prefix_ + "/frame_vel_color",
+ (Eigen::Vector4d() << 0.5, 0, 1, 1).finished());
+ frame_vel_color_.r = color(0);
+ frame_vel_color_.g = color(1);
+ frame_vel_color_.b = color(2);
+ frame_vel_color_.a = color(3);
// factors
factors_width_ = getParamWithDefault<double> (_server, prefix_ + "/factors_width", 0.02);
@@ -134,7 +134,6 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name,
frame_marker_.header.frame_id = map_frame_id_;
frame_marker_.ns = "frames";
frame_marker_.scale.x = viz_scale_*frame_width_;
- frame_marker_.color = frame_color_;
frame_text_marker_ = frame_marker_;
frame_text_marker_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
frame_text_marker_.ns = "frames_text";
@@ -198,13 +197,9 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name,
// zero vector
frame_marker_.points.push_back(frame_marker_.points.front());
frame_marker_.points.push_back(frame_marker_.points.front());
- // yellow
- frame_marker_.colors.push_back(frame_marker_.colors.front());
- frame_marker_.colors.back().r = 1;//yellow
- frame_marker_.colors.back().g = 1;
- frame_marker_.colors.back().b = 0;
- frame_marker_.colors.back().a = 1;
- frame_marker_.colors.push_back(frame_marker_.colors.back());
+ // vel color
+ frame_marker_.colors.push_back(frame_vel_color_);
+ frame_marker_.colors.push_back(frame_vel_color_);
}
// landmark markers
@@ -595,7 +590,7 @@ void PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
fac_marker.points.push_back(point2);
// colors ------------------------------------------------------
- auto color = frame_color_;
+ auto color = factor_abs_color_;
if (fac->getTopology() == TOP_ABS)
color = factor_abs_color_;
if (fac->getTopology() == TOP_MOTION)
@@ -629,21 +624,6 @@ void PublisherGraph::fillFrameMarker(FrameBaseConstPtr frm,
visualization_msgs::Marker &frm_marker,
visualization_msgs::Marker &frm_text_marker)
{
-// // SHAPE ------------------------------------------------------
-// // Position-> SPHERE
-// // Pose -> ARROW
-// if (frm->getO() != nullptr) {
-// landmark_marker_.type = visualization_msgs::Marker::ARROW;
-// frm_marker.scale.x = viz_scale_*frame_length_;
-// frm_marker.scale.y = viz_scale_*frame_width_;
-// frm_marker.scale.z = viz_scale_*frame_width_;
-// } else {
-// landmark_marker_.type = visualization_msgs::Marker::SPHERE;
-// frm_marker.scale.x = viz_scale_*frame_width_;
-// frm_marker.scale.y = viz_scale_*frame_width_;
-// frm_marker.scale.z = viz_scale_*frame_width_;
-// }
-
// POSITION & ORIENTATION
// ------------------------------------------------------ position
frm_marker.pose.position.x = frm->getP()->getState()(0);
--
GitLab