From a7e6404b097ca609d12ee2da5dd92614762107f2 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 23 Dec 2021 19:39:25 +0100
Subject: [PATCH] user defined frame velocity scale
---
include/publisher_graph.h | 2 +-
src/publisher_graph.cpp | 7 ++++---
2 files changed, 5 insertions(+), 4 deletions(-)
diff --git a/include/publisher_graph.h b/include/publisher_graph.h
index 72380de..cc75f37 100644
--- a/include/publisher_graph.h
+++ b/include/publisher_graph.h
@@ -88,7 +88,7 @@ class PublisherGraph: public Publisher
// Options
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_;
+ 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_;
// auxiliar variables
diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp
index 913a863..026b204 100644
--- a/src/publisher_graph.cpp
+++ b/src/publisher_graph.cpp
@@ -49,6 +49,7 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name,
// frames
frame_width_ = getParamWithDefault<double> (_server, prefix_ + "/frame_width", 0.1);
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());
@@ -675,9 +676,9 @@ void PublisherGraph::fillFrameMarker(FrameBaseConstPtr frm,
if (frm->getO()->getSize() > 1)
{
Eigen::Vector3d v_local = Eigen::Quaterniond(Eigen::Vector4d(frm->getO()->getState())).conjugate() * frm->getV()->getState();
- frm_marker.points.back().x = v_local(0);
- frm_marker.points.back().y = v_local(1);
- frm_marker.points.back().z = v_local(2);
+ frm_marker.points.back().x = v_local(0) * frame_vel_scale_;
+ frm_marker.points.back().y = v_local(1) * frame_vel_scale_;
+ frm_marker.points.back().z = v_local(2) * frame_vel_scale_;
}
// 2d
else
--
GitLab