Skip to content

Commit

Permalink
removed commented code, cleanup, caption is now a bit lower
Browse files Browse the repository at this point in the history
  • Loading branch information
@remod
remod committed Sep 8, 2015
1 parent 6085bd2 commit b78c358
Show file tree
Hide file tree
Showing 4 changed files with 85 additions and 115 deletions.
6 changes: 6 additions & 0 deletions kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,17 @@
#pragma once

#ifndef Q_MOC_RUN
// boost
#include <boost/circular_buffer.hpp>

// rviz
#include <rviz/message_filter_display.h>

// kindr ros
#include <kindr_msgs/VectorAtPosition.h>
#endif


namespace Ogre {
class SceneNode;
} // Ogre
Expand All @@ -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 {
Expand Down
24 changes: 19 additions & 5 deletions kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,12 @@
#pragma once

#include <kindr_msgs/VectorAtPosition.h>
// rviz
#include <rviz/default_plugin/markers/text_view_facing_marker.h>

// kindr ros
#include <kindr_msgs/VectorAtPosition.h>


namespace Ogre {
class Vector3;
class Quaternion;
Expand All @@ -13,6 +17,7 @@ class Arrow;
class MovableText;
} // rviz


namespace kindr_rviz_plugins {

// Declare the visual class for this display.
Expand Down Expand Up @@ -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<rviz::Arrow> arrow_;

// The object implementing the arrow description text
boost::shared_ptr<rviz::MovableText> 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_;
Expand All @@ -71,18 +79,24 @@ 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_``.
Ogre::SceneManager* scene_manager_;

// 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
Expand Down
70 changes: 21 additions & 49 deletions kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp
View file
Original file line number Diff line number Diff line change
@@ -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 <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>

// tf
#include <tf/transform_listener.h>

// rviz
#include <rviz/visualization_manager.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/frame_manager.h>

// 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
Expand All @@ -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);
}
Expand Down Expand Up @@ -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_);
}
}

Expand All @@ -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
Expand Down Expand Up @@ -202,22 +178,18 @@ 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);
}

} // kindr_rviz_plugins


// Tell pluginlib about this class. It is important to do this in
// global scope, outside our package's namespace.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(kindr_rviz_plugins::VectorAtPositionDisplay,rviz::Display)
PLUGINLIB_EXPORT_CLASS(kindr_rviz_plugins::VectorAtPositionDisplay, rviz::Display)
100 changes: 39 additions & 61 deletions kindr_rviz_plugins/src/VectorAtPositionVisual.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);

Expand All @@ -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

0 comments on commit b78c358

Please sign in to comment.