Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[11904] pkg/trunk (Page 28)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

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.




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[11904] pkg/trunk (Page 28)

personalrobots-commit