From b48f003d7cd57095afe4b648ad424174c5e6f135 Mon Sep 17 00:00:00 2001
From: Aoi Nakane <nakane@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 29 Nov 2023 20:27:25 +0900
Subject: [PATCH] Add scale parameter for imu visualization

---
 rviz_plugin_tutorials/src/imu_display.cpp | 16 ++++++++++++++++
 rviz_plugin_tutorials/src/imu_display.h   |  2 ++
 rviz_plugin_tutorials/src/imu_visual.cpp  | 11 ++++++++---
 rviz_plugin_tutorials/src/imu_visual.h    |  7 +++++++
 4 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/rviz_plugin_tutorials/src/imu_display.cpp b/rviz_plugin_tutorials/src/imu_display.cpp
index 8e617a93..77e6da80 100644
--- a/rviz_plugin_tutorials/src/imu_display.cpp
+++ b/rviz_plugin_tutorials/src/imu_display.cpp
@@ -58,6 +58,10 @@ ImuDisplay::ImuDisplay()
                                              "0 is fully transparent, 1.0 is fully opaque.",
                                              this, SLOT( updateColorAndAlpha() ));
 
+  scale_property_ = new rviz::FloatProperty( "Scale", 1.0,
+                                             "Scale of the acceleration arrows.",
+                                             this, SLOT( updateScale() ));
+
   history_length_property_ = new rviz::IntProperty( "History Length", 1,
                                                     "Number of prior measurements to display.",
                                                     this, SLOT( updateHistoryLength() ));
@@ -104,6 +108,17 @@ void ImuDisplay::updateColorAndAlpha()
   }
 }
 
+// Set the current scale value for each visual.
+void ImuDisplay::updateScale()
+{
+  float scale = scale_property_->getFloat();
+
+  for( size_t i = 0; i < visuals_.size(); i++ )
+  {
+    visuals_[ i ]->setScale( scale );
+  }
+}
+
 // Set the number of past visuals to show.
 void ImuDisplay::updateHistoryLength()
 {
@@ -147,6 +162,7 @@ void ImuDisplay::processMessage( const sensor_msgs::Imu::ConstPtr& msg )
   float alpha = alpha_property_->getFloat();
   Ogre::ColourValue color = color_property_->getOgreColor();
   visual->setColor( color.r, color.g, color.b, alpha );
+  visual->setScale(scale_property_->getFloat());
 
   // And send it to the end of the circular buffer
   visuals_.push_back(visual);
diff --git a/rviz_plugin_tutorials/src/imu_display.h b/rviz_plugin_tutorials/src/imu_display.h
index ee6480fa..2133fd47 100644
--- a/rviz_plugin_tutorials/src/imu_display.h
+++ b/rviz_plugin_tutorials/src/imu_display.h
@@ -97,6 +97,7 @@ Q_OBJECT
 private Q_SLOTS:
   void updateColorAndAlpha();
   void updateHistoryLength();
+  void updateScale();
 
   // Function to handle an incoming ROS message.
 private:
@@ -109,6 +110,7 @@ private Q_SLOTS:
   // User-editable property variables.
   rviz::ColorProperty* color_property_;
   rviz::FloatProperty* alpha_property_;
+  rviz::FloatProperty* scale_property_;
   rviz::IntProperty* history_length_property_;
 };
 // END_TUTORIAL
diff --git a/rviz_plugin_tutorials/src/imu_visual.cpp b/rviz_plugin_tutorials/src/imu_visual.cpp
index a56d9b1e..79d603aa 100644
--- a/rviz_plugin_tutorials/src/imu_visual.cpp
+++ b/rviz_plugin_tutorials/src/imu_visual.cpp
@@ -71,11 +71,10 @@ void ImuVisual::setMessage( const sensor_msgs::Imu::ConstPtr& msg )
   Ogre::Vector3 acc( a.x, a.y, a.z );
 
   // Find the magnitude of the acceleration vector.
-  float length = acc.length();
+  float length = acc.length() * scale_;
 
   // Scale the arrow's thickness in each dimension along with its length.
-  Ogre::Vector3 scale( length, length, length );
-  acceleration_arrow_->setScale( scale );
+  acceleration_arrow_->setScale(Ogre::Vector3(length, length, length));
 
   // Set the orientation of the arrow to match the direction of the
   // acceleration vector.
@@ -98,6 +97,12 @@ void ImuVisual::setColor( float r, float g, float b, float a )
 {
   acceleration_arrow_->setColor( r, g, b, a );
 }
+
+// Scale is passed through to the Arrow object.
+void ImuVisual::setScale(float s)
+{
+  scale_ = s;
+}
 // END_TUTORIAL
 
 } // end namespace rviz_plugin_tutorials
diff --git a/rviz_plugin_tutorials/src/imu_visual.h b/rviz_plugin_tutorials/src/imu_visual.h
index 1334e180..aecdaa5a 100644
--- a/rviz_plugin_tutorials/src/imu_visual.h
+++ b/rviz_plugin_tutorials/src/imu_visual.h
@@ -78,10 +78,17 @@ class ImuVisual
   // parameters and therefore don't come from the Imu message.
   void setColor( float r, float g, float b, float a );
 
+  // Set the scale of the visual, which are user-editable
+  // parameters and therefore don't come from the Imu message.
+  void setScale(float s);
+
 private:
   // The object implementing the actual arrow shape
   boost::shared_ptr<rviz::Arrow> acceleration_arrow_;
 
+  // arrow scale
+  float scale_;
+
   // A SceneNode whose pose is set to match the coordinate frame of
   // the Imu message header.
   Ogre::SceneNode* frame_node_;