Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11904] pkg/trunk
|
From: <jfa...@us...> - 2009-02-27 22:34:50
|
Revision: 11904
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11904&view=rev
Author: jfaustwg
Date: 2009-02-27 22:34:45 +0000 (Fri, 27 Feb 2009)
Log Message:
-----------
Add LINE_LIST option for visualization markers.
Modified Paths:
--------------
pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg
pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.h
pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.cpp
pkg/trunk/visualization/rviz/src/test/marker_test.cpp
Modified: pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg 2009-02-27 22:34:20 UTC (rev 11903)
+++ pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg 2009-02-27 22:34:45 UTC (rev 11904)
@@ -7,8 +7,9 @@
byte CUBE=1
byte SPHERE=2
byte ROBOT=3
-byte LINE_STRIP=4
-byte CYLINDER=5
+byte CYLINDER=4
+byte LINE_STRIP=5
+byte LINE_LIST=6
byte ADD=0
byte MODIFY=0
@@ -31,5 +32,5 @@
int32 r # red color value (0-255) (if available)
int32 g # green color value
int32 b # blue color value
-#These are only used if the object type is LINE_STRIP
+#These are only used if the object type is LINE_STRIP or LINE_LIST
Point[] points
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.cpp 2009-02-27 22:34:20 UTC (rev 11903)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.cpp 2009-02-27 22:34:45 UTC (rev 11904)
@@ -38,12 +38,15 @@
#include <sstream>
+#include <ros/assert.h>
+
namespace ogre_tools
{
BillboardLine::BillboardLine( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node )
: Object( scene_manager )
, width_( 0.1f )
+, current_line_(0)
{
if ( !parent_node )
{
@@ -56,8 +59,8 @@
static int count = 0;
ss << "BillboardLine chain" << count++;
chain_ = scene_manager_->createBillboardChain(ss.str());
- chain_->setNumberOfChains(1);
- chain_->setMaxChainElements(1000);
+ setNumLines(1);
+ setMaxPointsPerLine(100);
scene_node_->attachObject( chain_ );
@@ -77,24 +80,43 @@
void BillboardLine::clear()
{
- points_.clear();
- chain_->clearChain(0);
+ chain_->clearAllChains();
+ current_line_ = 0;
+
+ for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it)
+ {
+ *it = 0;
+ }
}
+void BillboardLine::setMaxPointsPerLine(uint32_t max)
+{
+ chain_->setMaxChainElements(max);
+}
+
+void BillboardLine::setNumLines(uint32_t num)
+{
+ chain_->setNumberOfChains(num);
+ num_elements_.resize(num);
+}
+
+void BillboardLine::newLine()
+{
+ ++current_line_;
+
+ ROS_ASSERT(current_line_ < chain_->getNumberOfChains());
+}
+
void BillboardLine::addPoint( const Ogre::Vector3& point )
{
- points_.push_back( point );
+ ++num_elements_[current_line_];
+ ROS_ASSERT(num_elements_[current_line_] <= (chain_->getMaxChainElements() * (current_line_+1)));
- if ( points_.size() > chain_->getMaxChainElements() )
- {
- chain_->setMaxChainElements( chain_->getMaxChainElements() * 2 );
- }
-
Ogre::BillboardChain::Element e;
e.position = point;
e.width = width_;
e.colour = color_;
- chain_->addChainElement(0, e);
+ chain_->addChainElement(current_line_, e);
}
void BillboardLine::setPoints( const V_Vector3& points )
@@ -112,10 +134,20 @@
void BillboardLine::setLineWidth( float width )
{
width_ = width;
- V_Vector3 points;
- points.swap( points_ );
- setPoints( points );
+ uint32_t num_chains = chain_->getNumberOfChains();
+ for (uint32_t chain = 0; chain < num_chains; ++chain)
+ {
+ uint32_t element_count = num_elements_[chain];
+
+ for ( uint32_t i = 0; i < element_count; ++i )
+ {
+ Ogre::BillboardChain::Element e = chain_->getChainElement(chain, i);
+
+ e.width = width_;
+ chain_->updateChainElement(chain, i, e);
+ }
+ }
}
void BillboardLine::setPosition( const Ogre::Vector3& position )
@@ -148,13 +180,18 @@
color_ = Ogre::ColourValue( r, g, b, a );
- uint32_t element_count = points_.size();
- for ( uint32_t i = 0; i < element_count; ++i )
+ uint32_t num_chains = chain_->getNumberOfChains();
+ for (uint32_t chain = 0; chain < num_chains; ++chain)
{
- Ogre::BillboardChain::Element e = chain_->getChainElement(0, i);
+ uint32_t element_count = num_elements_[chain];
- e.colour = color_;
- chain_->updateChainElement(0, i, e);
+ for ( uint32_t i = 0; i < element_count; ++i )
+ {
+ Ogre::BillboardChain::Element e = chain_->getChainElement(chain, i);
+
+ e.colour = color_;
+ chain_->updateChainElement(chain, i, e);
+ }
}
}
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.h 2009-02-27 22:34:20 UTC (rev 11903)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.h 2009-02-27 22:34:45 UTC (rev 11904)
@@ -67,6 +67,7 @@
virtual ~BillboardLine();
void clear();
+ void newLine();
void addPoint(const Ogre::Vector3& point);
typedef std::vector<Ogre::Vector3> V_Vector3;
@@ -74,6 +75,9 @@
void setLineWidth( float width );
+ void setMaxPointsPerLine(uint32_t max);
+ void setNumLines(uint32_t num);
+
// overrides from Object
virtual void setOrientation( const Ogre::Quaternion& orientation );
virtual void setPosition( const Ogre::Vector3& position );
@@ -98,9 +102,14 @@
Ogre::BillboardChain* chain_;
Ogre::MaterialPtr material_;
- V_Vector3 points_;
Ogre::ColourValue color_;
float width_;
+
+ int current_line_;
+
+ // Ogre 1.4 doesn't have getNumChainElements()
+ typedef std::vector<uint32_t> V_uint32;
+ V_uint32 num_elements_;
};
} // namespace ogre_tools
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.cpp 2009-02-27 22:34:20 UTC (rev 11903)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.cpp 2009-02-27 22:34:45 UTC (rev 11904)
@@ -61,7 +61,7 @@
kinematic_model_ = new planning_models::KinematicModel();
kinematic_model_->setVerbose( false );
- notifier_ = new tf::MessageNotifier<robot_msgs::VisualizationMarker>(tf_, ros_node_, boost::bind(&MarkerDisplay::incomingMarker, this, _1), "", "", 100);
+ notifier_ = new tf::MessageNotifier<robot_msgs::VisualizationMarker>(tf_, ros_node_, boost::bind(&MarkerDisplay::incomingMarker, this, _1), "", "", 1000);
}
MarkerDisplay::~MarkerDisplay()
@@ -236,6 +236,12 @@
object = line;
}
break;
+ case robot_msgs::VisualizationMarker::LINE_LIST:
+ {
+ ogre_tools::BillboardLine* line = new ogre_tools::BillboardLine( scene_manager_, scene_node_ );
+ object = line;
+ }
+ break;
default:
ROS_ERROR( "Unknown marker type: %d\n", message->type );
}
@@ -311,6 +317,7 @@
line->clear();
line->setLineWidth( message->xScale );
+ line->setMaxPointsPerLine(message->points.size());
std::vector<robot_msgs::Point>::iterator it = message->points.begin();
std::vector<robot_msgs::Point>::iterator end = message->points.end();
@@ -324,6 +331,45 @@
line->addPoint( v );
}
}
+ else if ( message->type == robot_msgs::VisualizationMarker::LINE_LIST )
+ {
+ if (message->points.size() % 2 == 0)
+ {
+ ogre_tools::BillboardLine* line = dynamic_cast<ogre_tools::BillboardLine*>(object);
+ ROS_ASSERT( line );
+
+ line->clear();
+ line->setLineWidth( message->xScale );
+ line->setMaxPointsPerLine(2);
+ line->setNumLines(message->points.size() / 2);
+
+ std::vector<robot_msgs::Point>::iterator it = message->points.begin();
+ std::vector<robot_msgs::Point>::iterator end = message->points.end();
+ for ( ; it != end; ++it )
+ {
+ if (it != message->points.begin())
+ {
+ line->newLine();
+ }
+
+ robot_msgs::Point& p = *it;
+ ++it;
+ robot_msgs::Point& p2 = *it;
+
+ Ogre::Vector3 v( p.x, p.y, p.z );
+ robotToOgre( v );
+ line->addPoint( v );
+
+ v = Ogre::Vector3( p2.x, p2.y, p2.z );
+ robotToOgre( v );
+ line->addPoint( v );
+ }
+ }
+ else
+ {
+ ROS_ERROR("Marker [%d] with type LINE_LIST has an odd number of points", message->id);
+ }
+ }
}
void MarkerDisplay::update( float dt )
Modified: pkg/trunk/visualization/rviz/src/test/marker_test.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/test/marker_test.cpp 2009-02-27 22:34:20 UTC (rev 11903)
+++ pkg/trunk/visualization/rviz/src/test/marker_test.cpp 2009-02-27 22:34:45 UTC (rev 11904)
@@ -17,6 +17,7 @@
usleep( 1000000 );
+#if 0
for ( int i = -50; i < 50; ++i )
{
robot_msgs::VisualizationMarker marker;
@@ -41,10 +42,44 @@
node->publish( "visualizationMarker", marker );
}
+#endif
+
+ int count = 10000;
+ robot_msgs::VisualizationMarker marker;
+ marker.header.frame_id = "map";
+ marker.header.stamp = ros::Time();
+ marker.id = 0;
+ marker.type = robot_msgs::VisualizationMarker::LINE_LIST;
+ marker.action = robot_msgs::VisualizationMarker::ADD;
+ marker.x = 0;
+ marker.y = 0;
+ marker.z = 0;
+ marker.yaw = 0.0;
+ marker.pitch = 0.0;
+ marker.roll = 0.0;
+ marker.xScale = 0.1;
+ marker.alpha = 255;
+ marker.r = 255;
+ marker.g = 0;
+ marker.b = 0;
+ for ( int i = 0; i < count; ++i )
+ {
+ robot_msgs::Point p1, p2;
+ p1.x = -1;
+ p1.y = (i - count/2);
+ p1.z = 1;
+ p2.x = -1;
+ p2.y = (i - count/2);
+ p2.z = 2;
+ marker.points.push_back(p1);
+ marker.points.push_back(p2);
+ }
+ node->publish( "visualizationMarker", marker );
+
robot_msgs::VisualizationMarker line_marker;
line_marker.header.frame_id = "map";
line_marker.header.stamp = ros::Time();
- line_marker.id = 1000;
+ line_marker.id = count + 1;
line_marker.type = robot_msgs::VisualizationMarker::LINE_STRIP;
line_marker.action = 0;
line_marker.x = 0;
@@ -74,5 +109,5 @@
node->shutdown();
delete node;
-
+
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
