diff --git a/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp b/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp index 14d8109..a9a4b89 100644 --- a/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp +++ b/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp @@ -1,12 +1,17 @@ #pragma once #ifndef Q_MOC_RUN +// boost #include +// rviz #include + +// kindr ros #include #endif + namespace Ogre { class SceneNode; } // Ogre @@ -17,6 +22,7 @@ class FloatProperty; class IntProperty; } // rviz + // All the source in this plugin is in its own namespace. This is not // required but is good practice. namespace kindr_rviz_plugins { diff --git a/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp b/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp index 3728ca4..c09c7b6 100644 --- a/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp +++ b/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp @@ -1,8 +1,12 @@ #pragma once -#include +// rviz #include +// kindr ros +#include + + namespace Ogre { class Vector3; class Quaternion; @@ -13,6 +17,7 @@ class Arrow; class MovableText; } // rviz + namespace kindr_rviz_plugins { // Declare the visual class for this display. @@ -52,15 +57,18 @@ class VectorAtPositionVisual // Set the color and alpha of the visual, which are user-editable // parameters and therefore don't come from the VectorAtPosition message. - void setColor(float r, float g, float b, float a); + void setColor(const Ogre::ColourValue& color); -private: +protected: // The object implementing the actual arrow shape boost::shared_ptr arrow_; // The object implementing the arrow description text boost::shared_ptr text_; + // Store the name + std::string name_; + // Store the scaling factor and the length of the vector, so that it can be scale afterwards float length_; float lengthScalingFactor_; @@ -71,11 +79,11 @@ class VectorAtPositionVisual // Store the arrow color Ogre::ColourValue color_; - bool colorCanBeOverwritten_; // A SceneNode whose pose is set to match the coordinate frame of // the VectorAtPosition message header. - Ogre::SceneNode* arrow_node_; + Ogre::SceneNode* scene_node_arrow_; + Ogre::SceneNode* scene_node_text_; // The SceneManager, kept here only so the destructor can ask it to // destroy the ``frame_node_``. @@ -83,6 +91,12 @@ class VectorAtPositionVisual // Update the scaling of the arrow void updateScaling(); + + // Update the color of the arrow + void updateColor(); + + // Update the text of the arrow + void updateText(); }; } // kindr_rviz_plugins diff --git a/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp b/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp index 781ad41..625670b 100644 --- a/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp +++ b/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp @@ -1,46 +1,22 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - +// ogre #include #include +// tf #include +// rviz #include #include #include #include #include +// kindr rviz plugins #include "kindr_rviz_plugins/VectorAtPositionDisplay.hpp" #include "kindr_rviz_plugins/VectorAtPositionVisual.hpp" + namespace kindr_rviz_plugins { // The constructor must have no arguments, so we can't give the @@ -57,25 +33,25 @@ VectorAtPositionDisplay::VectorAtPositionDisplay() this, SLOT(updateScale())); width_scale_property_ = new rviz::FloatProperty("Width scale", 1.0, - "Scale of the width of the vector.", - this, SLOT(updateScale())); + "Scale of the width of the vector.", + this, SLOT(updateScale())); width_scale_property_->setMin(0); show_text_property_ = new rviz::BoolProperty("Show text", true, - "Enable or disable text rendering.", - this, SLOT(updateShowText())); + "Enable or disable text rendering.", + this, SLOT(updateShowText())); color_property_ = new rviz::ColorProperty("Color", QColor(0, 0, 0), - "Color to draw the vector (if not defined by vector type).", - this, SLOT(updateColorAndAlpha())); + "Color to draw the vector (if not defined by vector type).", + this, SLOT(updateColorAndAlpha())); alpha_property_ = new rviz::FloatProperty("Alpha", 1.0, - "0 is fully transparent, 1.0 is fully opaque.", - this, SLOT(updateColorAndAlpha())); + "0 is fully transparent, 1.0 is fully opaque.", + this, SLOT(updateColorAndAlpha())); history_length_property_ = new rviz::IntProperty("History Length", 1, - "Number of prior measurements to display.", - this, SLOT(updateHistoryLength())); + "Number of prior measurements to display.", + this, SLOT(updateHistoryLength())); history_length_property_->setMin(1); history_length_property_->setMax(100000); } @@ -134,11 +110,11 @@ void VectorAtPositionDisplay::updateShowText() void VectorAtPositionDisplay::updateColorAndAlpha() { color_ = color_property_->getOgreColor(); - alpha_ = alpha_property_->getFloat(); + color_.a = alpha_property_->getFloat(); for(size_t i = 0; i < visuals_.size(); i++) { - visuals_[i]->setColor(color_.r, color_.g, color_.b, alpha_); + visuals_[i]->setColor(color_); } } @@ -156,7 +132,7 @@ void VectorAtPositionDisplay::processMessage(const kindr_msgs::VectorAtPosition: Ogre::Vector3 arrowPosition; Ogre::Quaternion arrowOrientation; - // If the position has a different frame than the vector + // Check if the position has an empty or the same frame as the vector if (msg->position_frame_id.empty() || msg->position_frame_id == msg->header.frame_id) { // Get arrow position and orientation @@ -202,14 +178,9 @@ void VectorAtPositionDisplay::processMessage(const kindr_msgs::VectorAtPosition: visual->setMessage(msg); visual->setArrowPosition(arrowPosition); // position is taken from position in msg visual->setArrowOrientation(arrowOrientation); // orientation is taken from vector in msg - - lengthScale_ = length_scale_property_->getFloat(); - widthScale_ = width_scale_property_->getFloat(); visual->setScalingFactors(lengthScale_, widthScale_); - showText_ = show_text_property_->getBool(); visual->setShowText(showText_); - alpha_ = alpha_property_->getFloat(); - visual->setColor(color_.r, color_.g, color_.b, alpha_); + visual->setColor(color_); // And send it to the end of the circular buffer visuals_.push_back(visual); @@ -217,7 +188,8 @@ void VectorAtPositionDisplay::processMessage(const kindr_msgs::VectorAtPosition: } // kindr_rviz_plugins + // Tell pluginlib about this class. It is important to do this in // global scope, outside our package's namespace. #include -PLUGINLIB_EXPORT_CLASS(kindr_rviz_plugins::VectorAtPositionDisplay,rviz::Display) +PLUGINLIB_EXPORT_CLASS(kindr_rviz_plugins::VectorAtPositionDisplay, rviz::Display) diff --git a/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp b/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp index feb81bd..50d9caf 100644 --- a/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp +++ b/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp @@ -44,8 +44,7 @@ VectorAtPositionVisual::VectorAtPositionVisual(Ogre::SceneManager* scene_manager lengthScalingFactor_(1.0), widthScalingFactor_(1.0), showText_(true), - color_(Ogre::ColourValue(0,0,0,1)), - colorCanBeOverwritten_(true) + color_(Ogre::ColourValue(0,0,0,1)) { scene_manager_ = scene_manager; @@ -56,21 +55,27 @@ VectorAtPositionVisual::VectorAtPositionVisual(Ogre::SceneManager* scene_manager // // Here we create a node to store the pose of the VectorAtPosition's header frame // relative to the RViz fixed frame. - arrow_node_ = parent_node->createChildSceneNode(); + scene_node_arrow_ = parent_node->createChildSceneNode(); + scene_node_text_ = scene_node_arrow_->createChildSceneNode(); + scene_node_text_->setPosition(Ogre::Vector3(0, 0, -0.05)); // We create the arrow object within the frame node so that we can // set its position and direction relative to its header frame. - arrow_.reset(new rviz::Arrow(scene_manager_, arrow_node_, 0.8f, 0.07f, 0.2f, 0.15f)); + arrow_.reset(new rviz::Arrow(scene_manager_, scene_node_arrow_, 0.8f, 0.07f, 0.2f, 0.15f)); } VectorAtPositionVisual::~VectorAtPositionVisual() { // Destroy the frame node since we don't need it anymore. - scene_manager_->destroySceneNode(arrow_node_); + scene_manager_->destroySceneNode(scene_node_arrow_); + scene_manager_->destroySceneNode(scene_node_text_); } void VectorAtPositionVisual::setMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg) { + // Set the name of the arrow. + name_ = msg->name; + // Convert the geometry_msgs::Vector3 to an Ogre::Vector3. const Ogre::Vector3 vectorOgre(msg->vector.x, msg->vector.y, msg->vector.z); @@ -80,94 +85,67 @@ void VectorAtPositionVisual::setMessage(const kindr_msgs::VectorAtPosition::Cons // Set the orientation of the arrow to match the direction of the vector. arrow_->setDirection(vectorOgre); - // Find the magnitude of the acceleration vector. + // Find the magnitude of the vector. length_ = vectorOgre.length(); // update the scaling updateScaling(); - // add description text if available - if (showText_) - { - text_.reset(new rviz::MovableText(msg->name, "Arial", 0.1)); - text_->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_BELOW); - arrow_node_->attachObject(text_.get()); - } - else - { - text_.reset(new rviz::MovableText("", "Arial", 0.1)); - text_->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_BELOW); - arrow_node_->attachObject(text_.get()); - } - -// // set color -// if (msg->type == msg->TYPE_POSITION) -// { -// color_ = Ogre::ColourValue::Blue; -// colorCanBeOverwritten_ = true; -// setColor(color_.r, color_.g, color_.b, color_.a); -// colorCanBeOverwritten_ = false; -// } -// else if (msg->type == msg->TYPE_VELOCITY || msg->type == msg->TYPE_ANGULAR_VELOCITY) -// { -// color_ = Ogre::ColourValue::Green; -// colorCanBeOverwritten_ = true; -// setColor(color_.r, color_.g, color_.b, color_.a); -// colorCanBeOverwritten_ = false; -// } -// else if (msg->type == msg->TYPE_FORCE || msg->type == msg->TYPE_TORQUE) -// { -// // color_ = Ogre::ColourValue::Red; -// colorCanBeOverwritten_ = true; -// setColor(color_.r, color_.g, color_.b, color_.a); -// colorCanBeOverwritten_ = false; -// } -// else -// { -// color_ = Ogre::ColourValue::Black; - colorCanBeOverwritten_ = true; - setColor(color_.r, color_.g, color_.b, color_.a); -// } + // update the color + updateColor(); + + // update the text + updateText(); } -// Position and orientation are passed through to the SceneNode. void VectorAtPositionVisual::setArrowPosition(const Ogre::Vector3& position) { - arrow_node_->setPosition(position); + scene_node_arrow_->setPosition(position); } void VectorAtPositionVisual::setArrowOrientation(const Ogre::Quaternion& orientation) { - arrow_node_->setOrientation(orientation); + scene_node_arrow_->setOrientation(orientation); } -// Scale is passed through to the Arrow object. void VectorAtPositionVisual::setScalingFactors(float lengthScalingFactor, float widthScalingFactor) { lengthScalingFactor_ = lengthScalingFactor; widthScalingFactor_ = widthScalingFactor; updateScaling(); } -void VectorAtPositionVisual::setShowText(bool showText) + +void VectorAtPositionVisual::setColor(const Ogre::ColourValue& color) { - showText_ = showText; + color_ = color; + updateColor(); } -// Color is passed through to the Arrow object. -void VectorAtPositionVisual::setColor(float r, float g, float b, float a) +void VectorAtPositionVisual::setShowText(bool showText) { -// if (colorCanBeOverwritten_) -// { - arrow_->setColor(r, g, b, a); -// } + showText_ = showText; + updateText(); } -// Update the scaling of the arrow. void VectorAtPositionVisual::updateScaling() { // Scale the arrow's thickness in each dimension along with its length and scaling factors. arrow_->setScale(Ogre::Vector3(lengthScalingFactor_ * length_, widthScalingFactor_, widthScalingFactor_)); } +void VectorAtPositionVisual::updateColor() +{ + arrow_->setColor(color_); +} + +void VectorAtPositionVisual::updateText() +{ + // add description text if available + const std::string textString = showText_? name_ : ""; + text_.reset(new rviz::MovableText(textString, "Arial", 0.1)); + text_->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_BELOW); + scene_node_text_->attachObject(text_.get()); +} + } // kindr_rviz_plugins