Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3505] pkg/trunk
|
From: <MP...@us...> - 2008-08-22 22:43:17
|
Revision: 3505
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3505&view=rev
Author: MPPics
Date: 2008-08-22 22:43:27 +0000 (Fri, 22 Aug 2008)
Log Message:
-----------
Adding octrees to visualizer
Modified Paths:
--------------
pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp
pkg/trunk/visualization/ogre_visualizer/manifest.xml
Added Paths:
-----------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
Modified: pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp
===================================================================
--- pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp 2008-08-22 22:40:25 UTC (rev 3504)
+++ pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp 2008-08-22 22:43:27 UTC (rev 3505)
@@ -30,9 +30,9 @@
CloudToOctree(float cellSize);
~CloudToOctree();
void resetOctree();
- };
-
-
+ };
+
+
CloudToOctree::CloudToOctree(float cellSize) : ros::node("cloud_to_octree_node")
{
/* initialize Octree to depth 0. This means that the
@@ -46,7 +46,7 @@
//subsribe and advertise to relevant ROS topics
subscribe("full_cloud", mNewCloud, &CloudToOctree::fullCloudCallback,1);
- advertise<OctreeMsg>("full_octree",1);
+ advertise<OctreeMsg>("full_octree",1);
fprintf(stderr,"ROS cloud-to-octree node with cell size %f created and subscribed!\n",cellSize);
}
@@ -56,6 +56,7 @@
}
void CloudToOctree::fullCloudCallback() {
+ fprintf(stderr,"Cloud received with %d points!\n",mNewCloud.get_pts_size());
if ( mNewCloud.get_pts_size() == 0 ) {
return;
}
@@ -68,7 +69,7 @@
OctreeMsg outMsg;
mOctree->getAsMsg(outMsg);
//and publish
- publish("full_octree",outMsg);
+ publish("full_octree",outMsg);
}
/*! Clears the contents of the Octree. Note that the size and
depth are not changed, they remain whatever they were
@@ -80,7 +81,7 @@
2*cell_size along each dimension.
*/
void CloudToOctree::resetOctree() {
- mOctree->clear();
+ mOctree->clear();
}
} //namespace scan_utils
Modified: pkg/trunk/visualization/ogre_visualizer/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-08-22 22:40:25 UTC (rev 3504)
+++ pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-08-22 22:43:27 UTC (rev 3505)
@@ -15,6 +15,7 @@
<depend package="rosTF"/>
<depend package="laser_scan_utils"/>
<depend package="wg_robot_description_parser"/>
+ <depend package="scan_utils" />
<depend package="wxpropgrid"/>
<depend package="wx_topic_display"/>
<export>
Added: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp 2008-08-22 22:43:27 UTC (rev 3505)
@@ -0,0 +1,174 @@
+/*
+ * Copyright (c) 2008, 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.
+ */
+
+/*
+ * octree_visualizer.cpp
+ *
+ * Created on: Aug 20, 2008
+ * Author: Matthew Piccoli and Matei Ciocarlie
+ */
+
+#include "octree_visualizer.h"
+#include "octree.h"
+#include "dataTypes.h"
+#include "ros/node.h"
+#include <rosTF/rosTF.h>
+
+#include "../common.h"
+
+namespace ogre_vis
+{
+
+ OctreeVisualizer::OctreeVisualizer(Ogre::SceneManager* sceneManager, ros::node* node, rosTFClient* tfClient, const std::string& name, bool enabled )
+ : VisualizerBase( sceneManager, node, tfClient, name, enabled )
+ , m_R( 1.0 )
+ , m_G( 0.0 )
+ , m_B( 0.0 )
+ , m_NewMessage( false )
+ {
+ static uint32_t count = 0;
+ std::stringstream ss;
+ ss << "Cone" << count++;
+
+ m_ManualObject = m_SceneManager->createManualObject( ss.str() );
+
+ m_SceneNode = m_SceneManager->getRootSceneNode()->createChildSceneNode();
+ m_SceneNode->attachObject( m_ManualObject );
+
+ if ( IsEnabled() )
+ {
+ OnEnable();
+ }
+
+ }
+
+ OctreeVisualizer::~OctreeVisualizer() {
+ // TODO Auto-generated destructor stub
+ }
+
+ void OctreeVisualizer::OnEnable()
+ {
+ m_ManualObject->setVisible( true );
+ Subscribe();
+ }
+
+ void OctreeVisualizer::OnDisable()
+ {
+ m_ManualObject->setVisible( false );
+ Unsubscribe();
+ }
+
+ void OctreeVisualizer::Subscribe()
+ {
+ if ( !IsEnabled() )
+ {
+ return;
+ }
+
+ if ( !m_OctreeTopic.empty() )
+ {
+ m_ROSNode->subscribe( m_OctreeTopic, m_OctreeMessage, &OctreeVisualizer::IncomingOctreeCallback, this, 10 );
+ }
+ }
+
+ void OctreeVisualizer::Unsubscribe()
+ {
+ if ( !m_OctreeTopic.empty() )
+ {
+ m_ROSNode->unsubscribe( m_OctreeTopic );
+ }
+ }
+
+ void OctreeVisualizer::Update( float dt )
+ {
+ m_OctreeMessage.lock();
+
+ if ( m_NewMessage )
+ {
+ scan_utils::Octree<char> octree(0,0,0,0,0,0,1,0);
+ octree.setFromMsg(m_OctreeMessage);
+
+ std::list<scan_utils::Triangle> triangles;
+ octree.getAllTriangles(triangles);
+
+ std::list<scan_utils::Triangle>::iterator it;
+ Ogre::Vector3 v1, v2, v3;
+
+ m_ManualObject->clear();
+ m_ManualObject->begin( m_MaterialName, Ogre::RenderOperation::OT_TRIANGLE_LIST );
+
+ int i = 0;
+ for ( it = triangles.begin(); it!=triangles.end(); it++, i += 3)
+ {
+ v1 = Ogre::Vector3( it->p1.x, it->p1.y, it->p1.z );
+ RobotToOgre( v1 );
+ m_ManualObject->position( v1 );
+ m_ManualObject->colour( m_R, m_G, m_B );
+ v2 = Ogre::Vector3( it->p2.x, it->p2.y, it->p2.z );
+ RobotToOgre( v2 );
+ m_ManualObject->position( v2 );
+ m_ManualObject->colour( m_R, m_G, m_B );
+ v3 = Ogre::Vector3( it->p3.x, it->p3.y, it->p3.z );
+ RobotToOgre( v3 );
+ m_ManualObject->position( v3 );
+ m_ManualObject->colour( m_R, m_G, m_B );
+
+ //m_ManualObject->triangle( i, i + 2, i + 1 );
+ }
+ m_ManualObject->end();
+
+ m_NewMessage = false;
+ }
+
+ m_OctreeMessage.unlock();
+ }
+
+ void OctreeVisualizer::IncomingOctreeCallback()
+ {
+ m_OctreeMessage.lock();
+ m_NewMessage = true;
+ m_OctreeMessage.unlock();
+ }
+
+ void OctreeVisualizer::SetOctreeTopic( const std::string& topic )
+ {
+ Unsubscribe();
+
+ m_OctreeTopic = topic;
+
+ Subscribe();
+ }
+
+ void OctreeVisualizer::SetColor( float r, float g, float b )
+ {
+ m_R = r;
+ m_G = g;
+ m_B = b;
+ }
+}
Added: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h 2008-08-22 22:43:27 UTC (rev 3505)
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2008, 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.
+ */
+
+/*
+ * octree_visualizer.h
+ *
+ * Created on: Aug 20, 2008
+ * Author: Matthew Piccoli and Matei Ciocarlie
+ */
+
+#ifndef OCTREE_VISUALIZER_H_
+#define OCTREE_VISUALIZER_H_
+
+#include "../visualizer_base.h"
+#include <scan_utils/OctreeMsg.h>
+
+#include <Ogre.h>
+namespace ogre_vis
+{
+
+ class OctreeVisualizer : public VisualizerBase
+ {
+ public:
+ OctreeVisualizer( Ogre::SceneManager* sceneManager, ros::node* node, rosTFClient* tfClient, const std::string& name, bool enabled );
+ virtual ~OctreeVisualizer();
+
+ void SetOctreeTopic( const std::string& topic );
+ void SetColor( float r, float g, float b );
+
+ virtual void Update( float dt );
+
+ protected:
+ // overrides from VisualizerBase
+ virtual void OnEnable();
+ virtual void OnDisable();
+ void Subscribe();
+ void Unsubscribe();
+
+ void IncomingOctreeCallback();
+
+ float m_R;
+ float m_G;
+ float m_B;
+
+ Ogre::SceneNode* m_SceneNode;
+ Ogre::ManualObject* m_ManualObject;
+ Ogre::MaterialPtr m_Material;
+ std::string m_MaterialName;
+ std::string m_OctreeTopic;
+
+ scan_utils::OctreeMsg m_OctreeMessage;
+
+ bool m_NewMessage;
+ };
+
+}
+#endif /* OCTREE_VISUALIZER_H_ */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
