Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add a feature to display pose markers(axes or arrow) #1059

Merged
merged 1 commit into from
Oct 17, 2016
Merged

Add a feature to display pose markers(axes or arrow) #1059

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
234 changes: 234 additions & 0 deletions src/rviz/default_plugin/path_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,11 +82,54 @@ PathDisplay::PathDisplay()
offset_property_ = new VectorProperty( "Offset", Ogre::Vector3::ZERO,
"Allows you to offset the path from the origin of the reference frame. In meters.",
this, SLOT( updateOffset() ));

pose_style_property_ = new EnumProperty( "Pose Style", "None", "Shape to display the pose as.",
this, SLOT( updatePoseStyle() ));
pose_style_property_->addOption( "None", NONE );
pose_style_property_->addOption( "Axes", AXES );
pose_style_property_->addOption( "Arrows", ARROWS );

pose_axes_length_property_ = new rviz::FloatProperty( "Length", 0.3,
"Length of the axes.",
this, SLOT(updatePoseAxisGeometry()) );
pose_axes_radius_property_ = new rviz::FloatProperty( "Radius", 0.03,
"Radius of the axes.",
this, SLOT(updatePoseAxisGeometry()) );

pose_arrow_color_property_ = new ColorProperty( "Color",
QColor( 255, 85, 255 ),
"Color to draw the poses.",
this, SLOT(updatePoseArrowColor()));
pose_arrow_shaft_length_property_ = new rviz::FloatProperty( "Shaft Length", 0.1,
"Length of the arrow shaft.",
this,
SLOT(updatePoseArrowGeometry()));
pose_arrow_head_length_property_ = new rviz::FloatProperty( "Head Length", 0.2,
"Length of the arrow head.",
this,
SLOT(updatePoseArrowGeometry()));
pose_arrow_shaft_diameter_property_ = new rviz::FloatProperty( "Shaft Diameter", 0.1,
"Diameter of the arrow shaft.",
this,
SLOT(updatePoseArrowGeometry()));
pose_arrow_head_diameter_property_ = new rviz::FloatProperty( "Head Diameter", 0.3,
"Diameter of the arrow head.",
this,
SLOT(updatePoseArrowGeometry()));
pose_axes_length_property_->hide();
pose_axes_radius_property_->hide();
pose_arrow_color_property_->hide();
pose_arrow_shaft_length_property_->hide();
pose_arrow_head_length_property_->hide();
pose_arrow_shaft_diameter_property_->hide();
pose_arrow_head_diameter_property_->hide();
}

PathDisplay::~PathDisplay()
{
destroyObjects();
destroyPoseAxesChain();
destroyPoseArrowChain();
}

void PathDisplay::onInitialize()
Expand All @@ -102,6 +145,58 @@ void PathDisplay::reset()
}


void PathDisplay::allocateAxesVector(std::vector<rviz::Axes*>& axes_vect, int num)
{
if (num > axes_vect.size()) {
for (size_t i = axes_vect.size(); i < num; i++) {
rviz::Axes* axes = new rviz::Axes( scene_manager_, scene_node_,
pose_axes_length_property_->getFloat(),
pose_axes_radius_property_->getFloat());
axes_vect.push_back(axes);
}
}
else if (num < axes_vect.size()) {
for (int i = axes_vect.size() - 1; num <= i; i--) {
delete axes_vect[i];
}
axes_vect.resize(num);
}
}

void PathDisplay::allocateArrowVector(std::vector<rviz::Arrow*>& arrow_vect, int num)
{
if (num > arrow_vect.size()) {
for (size_t i = arrow_vect.size(); i < num; i++) {
rviz::Arrow* arrow = new rviz::Arrow( scene_manager_, scene_node_ );
arrow_vect.push_back(arrow);
}
}
else if (num < arrow_vect.size()) {
for (int i = arrow_vect.size() - 1; num <= i; i--) {
delete arrow_vect[i];
}
arrow_vect.resize(num);
}
}

void PathDisplay::destroyPoseAxesChain()
{
for( size_t i = 0; i < axes_chain_.size(); i++ )
{
allocateAxesVector(axes_chain_[i], 0);
}
axes_chain_.resize(0);
}

void PathDisplay::destroyPoseArrowChain()
{
for( size_t i = 0; i < arrow_chain_.size(); i++ )
{
allocateArrowVector(arrow_chain_[i], 0);
}
arrow_chain_.resize(0);
}

void PathDisplay::updateStyle()
{
LineStyle style = (LineStyle) style_property_->getOptionInt();
Expand Down Expand Up @@ -142,6 +237,86 @@ void PathDisplay::updateOffset()
context_->queueRender();
}

void PathDisplay::updatePoseStyle()
{
PoseStyle pose_style = (PoseStyle) pose_style_property_->getOptionInt();
switch (pose_style)
{
case AXES:
pose_axes_length_property_->show();
pose_axes_radius_property_->show();
pose_arrow_color_property_->hide();
pose_arrow_shaft_length_property_->hide();
pose_arrow_head_length_property_->hide();
pose_arrow_shaft_diameter_property_->hide();
pose_arrow_head_diameter_property_->hide();
break;
case ARROWS:
pose_axes_length_property_->hide();
pose_axes_radius_property_->hide();
pose_arrow_color_property_->show();
pose_arrow_shaft_length_property_->show();
pose_arrow_head_length_property_->show();
pose_arrow_shaft_diameter_property_->show();
pose_arrow_head_diameter_property_->show();
break;
default:
pose_axes_length_property_->hide();
pose_axes_radius_property_->hide();
pose_arrow_color_property_->hide();
pose_arrow_shaft_length_property_->hide();
pose_arrow_head_length_property_->hide();
pose_arrow_shaft_diameter_property_->hide();
pose_arrow_head_diameter_property_->hide();
}
updateBufferLength();
}

void PathDisplay::updatePoseAxisGeometry()
{
for(size_t i = 0; i < axes_chain_.size() ; i++)
{
std::vector<rviz::Axes*>& axes_vect = axes_chain_[i];
for(size_t j = 0; j < axes_vect.size() ; j++)
{
axes_vect[j]->set( pose_axes_length_property_->getFloat(),
pose_axes_radius_property_->getFloat() );
}
}
context_->queueRender();
}

void PathDisplay::updatePoseArrowColor()
{
QColor color = pose_arrow_color_property_->getColor();

for( size_t i = 0; i < arrow_chain_.size(); i++ )
{
std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
for( size_t j = 0; j < arrow_vect.size(); j++ )
{
arrow_vect[j]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
}
}
context_->queueRender();
}

void PathDisplay::updatePoseArrowGeometry()
{
for( size_t i = 0; i < arrow_chain_.size(); i++ )
{
std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
for( size_t j = 0; j < arrow_vect.size(); j++ )
{
arrow_vect[j]->set(pose_arrow_shaft_length_property_->getFloat(),
pose_arrow_shaft_diameter_property_->getFloat(),
pose_arrow_head_length_property_->getFloat(),
pose_arrow_head_diameter_property_->getFloat());
}
}
context_->queueRender();
}

void PathDisplay::destroyObjects()
{
// Destroy all simple lines, if any
Expand Down Expand Up @@ -173,6 +348,10 @@ void PathDisplay::updateBufferLength()
// Delete old path objects
destroyObjects();

// Destroy all axes and arrows
destroyPoseAxesChain();
destroyPoseArrowChain();

// Read options
int buffer_length = buffer_length_property_->getInt();
LineStyle style = (LineStyle) style_property_->getOptionInt();
Expand Down Expand Up @@ -201,6 +380,8 @@ void PathDisplay::updateBufferLength()
}
break;
}
axes_chain_.resize( buffer_length );
arrow_chain_.resize( buffer_length );


}
Expand Down Expand Up @@ -293,6 +474,59 @@ void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
break;
}

// process pose markers
PoseStyle pose_style = (PoseStyle) pose_style_property_->getOptionInt();
std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[ bufferIndex ];
std::vector<rviz::Axes*>& axes_vect = axes_chain_[ bufferIndex ];

switch(pose_style)
{
case AXES:
allocateAxesVector(axes_vect, num_points);
for( uint32_t i=0; i < num_points; ++i)
{
const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
axes_vect[i]->setPosition(xpos);
Ogre::Quaternion orientation(msg->poses[ i ].pose.orientation.w,
msg->poses[ i ].pose.orientation.x,
msg->poses[ i ].pose.orientation.y,
msg->poses[ i ].pose.orientation.z);
axes_vect[i]->setOrientation(orientation);
}
break;

case ARROWS:
allocateArrowVector(arrow_vect, num_points);
for( uint32_t i=0; i < num_points; ++i)
{
const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );

QColor color = pose_arrow_color_property_->getColor();
arrow_vect[i]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );

arrow_vect[i]->set(pose_arrow_shaft_length_property_->getFloat(),
pose_arrow_shaft_diameter_property_->getFloat(),
pose_arrow_head_length_property_->getFloat(),
pose_arrow_head_diameter_property_->getFloat());
arrow_vect[i]->setPosition(xpos);
Ogre::Quaternion orientation(msg->poses[ i ].pose.orientation.w,
msg->poses[ i ].pose.orientation.x,
msg->poses[ i ].pose.orientation.y,
msg->poses[ i ].pose.orientation.z);

Ogre::Vector3 dir(1, 0, 0);
dir = orientation * dir;
arrow_vect[i]->setDirection(dir);
}
break;

default:
break;
}
context_->queueRender();

}

} // namespace rviz
Expand Down
29 changes: 29 additions & 0 deletions src/rviz/default_plugin/path_display.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@
#include <nav_msgs/Path.h>

#include "rviz/message_filter_display.h"
#include <rviz/ogre_helpers/arrow.h>
#include <rviz/ogre_helpers/axes.h>

namespace Ogre
{
Expand Down Expand Up @@ -77,12 +79,22 @@ private Q_SLOTS:
void updateStyle();
void updateLineWidth();
void updateOffset();
void updatePoseStyle();
void updatePoseAxisGeometry();
void updatePoseArrowColor();
void updatePoseArrowGeometry();

private:
void destroyObjects();
void allocateArrowVector(std::vector<rviz::Arrow*>& arrow_vect, int num);
void allocateAxesVector(std::vector<rviz::Axes*>& axes_vect, int num);
void destroyPoseAxesChain();
void destroyPoseArrowChain();

std::vector<Ogre::ManualObject*> manual_objects_;
std::vector<rviz::BillboardLine*> billboard_lines_;
std::vector<std::vector<rviz::Axes*> >axes_chain_;
std::vector<std::vector<rviz::Arrow*> >arrow_chain_;

EnumProperty* style_property_;
ColorProperty* color_property_;
Expand All @@ -96,6 +108,23 @@ private Q_SLOTS:
BILLBOARDS
};

// pose marker property
EnumProperty* pose_style_property_;
FloatProperty* pose_axes_length_property_;
FloatProperty* pose_axes_radius_property_;
ColorProperty* pose_arrow_color_property_;
FloatProperty* pose_arrow_shaft_length_property_;
FloatProperty* pose_arrow_head_length_property_;
FloatProperty* pose_arrow_shaft_diameter_property_;
FloatProperty* pose_arrow_head_diameter_property_;

enum PoseStyle
{
NONE,
AXES,
ARROWS,
};

};

} // namespace rviz
Expand Down