Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[5922] pkg/trunk
|
From: <jfa...@us...> - 2008-10-29 18:22:22
|
Revision: 5922
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5922&view=rev
Author: jfaustwg
Date: 2008-10-29 18:22:01 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
r10894@lan-dhcp-121: jfaust | 2008-10-28 11:38:36 -0700
First pass at fixed-frame transforms of fixed data, and relative transforms of only the camera + robot-centric things
Modified Paths:
--------------
pkg/trunk/visualization/ogre_tools/CMakeLists.txt
pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h
pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:10894
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_tools/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/ogre_tools/CMakeLists.txt 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/CMakeLists.txt 2008-10-29 18:22:01 UTC (rev 5922)
@@ -2,8 +2,8 @@
include(rosbuild)
include(FindPkgConfig)
-set(ROS_BUILD_TYPE RelWithDebInfo)
-#set(ROS_BUILD_TYPE Debug)
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE Debug)
rospack(ogre_tools)
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -43,6 +43,7 @@
CameraBase::CameraBase( Ogre::SceneManager* scene_manager )
: scene_manager_( scene_manager )
+, relative_node_( NULL )
{
std::stringstream ss;
static uint32_t count = 0;
@@ -55,6 +56,15 @@
scene_manager_->destroyCamera( camera_ );
}
+void CameraBase::setRelativeNode( Ogre::SceneNode* node )
+{
+ relative_node_ = node;
+
+ relativeNodeChanged();
+
+ update();
+}
+
void CameraBase::setPosition( const Ogre::Vector3& position )
{
setPosition( position.x, position.y, position.z );
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -65,6 +65,12 @@
* @return The Ogre camera associated with this camera object
*/
Ogre::Camera* getOgreCamera() { return camera_; }
+
+ void setRelativeNode( Ogre::SceneNode* node );
+ virtual void relativeNodeChanged() {}
+
+ virtual void update() = 0;
+
/**
* \brief Set the position of the camera
*/
@@ -188,6 +194,8 @@
protected:
Ogre::Camera* camera_; ///< Ogre camera associated with this camera object
Ogre::SceneManager* scene_manager_; ///< Scene manager this camera is part of
+
+ Ogre::SceneNode* relative_node_;
};
} // namespace ogre_tools
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -53,6 +53,14 @@
{
}
+void FPSCamera::relativeNodeChanged()
+{
+ if ( relative_node_ )
+ {
+ relative_node_->attachObject( camera_ );
+ }
+}
+
void FPSCamera::update()
{
Ogre::Matrix3 pitch, yaw;
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -58,6 +58,7 @@
virtual void setOrientation( float x, float y, float z, float w );
virtual void setPosition( float x, float y, float z );
virtual void setFrom( CameraBase* camera );
+ virtual void relativeNodeChanged();
virtual Ogre::Vector3 getPosition();
virtual Ogre::Quaternion getOrientation();
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -41,7 +41,7 @@
namespace ogre_tools
{
-Grid::Grid( Ogre::SceneManager* scene_manager, uint32_t gridSize, float cell_length, float r, float g, float b )
+Grid::Grid( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, uint32_t gridSize, float cell_length, float r, float g, float b )
: scene_manager_( scene_manager )
{
static uint32_t gridCount = 0;
@@ -50,7 +50,12 @@
manual_object_ = scene_manager_->createManualObject( ss.str() );
- scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+ if ( !parent_node )
+ {
+ parent_node = scene_manager_->getRootSceneNode();
+ }
+
+ scene_node_ = parent_node->createChildSceneNode();
scene_node_->attachObject( manual_object_ );
set( gridSize, cell_length, r, g, b );
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -66,7 +66,7 @@
* @param g Green color component, in the range [0, 1]
* @param b Blue color component, in the range [0, 1]
*/
- Grid( Ogre::SceneManager* manager, uint32_t cell_count, float cell_length, float r, float g, float b );
+ Grid( Ogre::SceneManager* manager, Ogre::SceneNode* parent_node, uint32_t cell_count, float cell_length, float r, float g, float b );
~Grid();
/**
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -51,7 +51,7 @@
OrbitCamera::OrbitCamera( Ogre::SceneManager* scene_manager )
: CameraBase( scene_manager )
-, focal_point_( 0.0f, 0.0f, 0.0f )
+, focal_point_( Ogre::Vector3::ZERO )
, yaw_( YAW_START )
, pitch_( PITCH_START )
, distance_( 10.0f )
@@ -87,12 +87,27 @@
void OrbitCamera::update()
{
- float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + focal_point_.x;
- float y = distance_ * cos( pitch_ ) + focal_point_.y;
- float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + focal_point_.z;
+ Ogre::Vector3 global_focal_point = focal_point_;
- camera_->setPosition( x, y, z );
- camera_->lookAt( focal_point_ );
+ if ( relative_node_ )
+ {
+ global_focal_point = relative_node_->getOrientation() * focal_point_ + relative_node_->getPosition();
+ }
+
+ float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + global_focal_point.x;
+ float y = distance_ * cos( pitch_ ) + global_focal_point.y;
+ float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + global_focal_point.z;
+
+ Ogre::Vector3 pos( x, y, z );
+
+ if ( relative_node_ )
+ {
+ Ogre::Vector3 vec = pos - global_focal_point;
+ pos = relative_node_->getOrientation() * vec + global_focal_point;
+ }
+
+ camera_->setPosition( pos );
+ camera_->lookAt( global_focal_point );
}
void OrbitCamera::yaw( float angle )
@@ -202,8 +217,14 @@
void OrbitCamera::move( float x, float y, float z )
{
- focal_point_ += camera_->getOrientation() * Ogre::Vector3( x, y, z );
+ Ogre::Quaternion orientation = camera_->getOrientation();
+ if ( relative_node_ )
+ {
+ orientation = relative_node_->getOrientation() * orientation;
+ }
+ focal_point_ += orientation * Ogre::Vector3( x, y, z );
+
update();
}
@@ -219,9 +240,21 @@
void OrbitCamera::lookAt( const Ogre::Vector3& point )
{
- distance_ = point.distance( camera_->getPosition() );
- focal_point_ = point;
+ Ogre::Vector3 focal_point = point;
+ Ogre::Vector3 camera_position = camera_->getPosition();
+ if ( relative_node_ )
+ {
+ Ogre::Vector3 rel_pos = relative_node_->getPosition();
+ Ogre::Quaternion rel_orient = relative_node_->getOrientation();
+
+ focal_point = rel_orient.Inverse() * ( focal_point - rel_pos );
+ camera_position = rel_orient.Inverse() * ( camera_position - rel_pos );
+ }
+
+ distance_ = focal_point.distance( camera_position );
+ focal_point_ = focal_point;
+
update();
}
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -92,11 +92,13 @@
virtual void mouseRightDrag( int diff_x, int diff_y );
virtual void scrollWheel( int diff );
-private:
/**
* \brief Calculates the camera's position and orientation from the yaw, pitch, distance and focal point
*/
- void update();
+ virtual void update();
+
+private:
+
/**
* \brief Calculates pitch and yaw values given a new position and the current focal point
* @param position Position to calculate the pitch/yaw for
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -45,7 +45,7 @@
OrthoCamera( wxOgreRenderWindow* render_window, Ogre::SceneManager* scene_manager );
virtual ~OrthoCamera();
- void update();
+ virtual void update();
virtual void setFrom( CameraBase* camera );
virtual void yaw( float angle );
Modified: pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -74,7 +74,7 @@
m_WXRenderWindow->getViewport()->setCamera( camera_->getOgreCamera() );
- ogre_tools::Grid* grid = new ogre_tools::Grid( scene_manager_, 10, 1.0f, 1.0f, 0.0f, 0.0f );
+ ogre_tools::Grid* grid = new ogre_tools::Grid( scene_manager_, NULL, 10, 1.0f, 1.0f, 0.0f, 0.0f );
//grid->getSceneNode()->pitch( Ogre::Degree( 90 ) );
//ogre_tools::Axes* axes = new ogre_tools::Axes( scene_manager_ );
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -83,6 +83,9 @@
scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
ray_scene_query_ = scene_manager_->createRayQuery( Ogre::Ray() );
+ target_relative_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+ updateRelativeNode();
+
selection_bounds_particle_system_ = scene_manager_->createParticleSystem( "VisualizationManagerSelectionBoundsParticleSystem" );
selection_bounds_particle_system_->setMaterialName( "BaseWhiteNoLighting" );
Ogre::SceneNode* node = scene_manager_->getRootSceneNode()->createChildSceneNode();
@@ -101,10 +104,13 @@
property_manager_ = new PropertyManager( vis_panel_->getPropertyGrid() );
CategoryProperty* category = property_manager_->createCategory( "Global Options", "", NULL );
- coordinate_frame_property_ = property_manager_->createProperty<StringProperty>( "Coordinate Frame", "", boost::bind( &VisualizationManager::getCoordinateFrame, this ),
- boost::bind( &VisualizationManager::setCoordinateFrame, this, _1 ), category );
+ target_frame_property_ = property_manager_->createProperty<StringProperty>( "Target Frame", "", boost::bind( &VisualizationManager::getTargetFrame, this ),
+ boost::bind( &VisualizationManager::setTargetFrame, this, _1 ), category );
+ fixed_frame_property_ = property_manager_->createProperty<StringProperty>( "Fixed Frame", "", boost::bind( &VisualizationManager::getFixedFrame, this ),
+ boost::bind( &VisualizationManager::setFixedFrame, this, _1 ), category );
- setCoordinateFrame( "base" );
+ setTargetFrame( "base" );
+ setFixedFrame( "map" );
}
VisualizationManager::~VisualizationManager()
@@ -184,6 +190,10 @@
vis_panel_->queueRender();
render_update_time = 0.0f;
}
+
+ updateRelativeNode();
+
+ vis_panel_->getCurrentCamera()->update();
}
void VisualizationManager::addVisualizer( VisualizerBase* visualizer, bool allow_deletion, bool enabled )
@@ -198,6 +208,7 @@
visualizer->setUnlockRenderCallback( boost::bind( &VisualizationPanel::unlockRender, vis_panel_ ) );
visualizer->setTargetFrame( target_frame_ );
+ visualizer->setFixedFrame( fixed_frame_ );
vis_panel_->getPropertyGrid()->Freeze();
@@ -476,7 +487,7 @@
removeVisualizer( visualizer );
}
-void VisualizationManager::setCoordinateFrame( const std::string& frame )
+void VisualizationManager::setTargetFrame( const std::string& frame )
{
target_frame_ = frame;
@@ -489,9 +500,25 @@
visualizer->setTargetFrame(frame);
}
- coordinate_frame_property_->changed();
+ target_frame_property_->changed();
}
+void VisualizationManager::setFixedFrame( const std::string& frame )
+{
+ fixed_frame_ = frame;
+
+ V_VisualizerInfo::iterator it = visualizers_.begin();
+ V_VisualizerInfo::iterator end = visualizers_.end();
+ for ( ; it != end; ++it )
+ {
+ VisualizerBase* visualizer = it->visualizer_;
+
+ visualizer->setFixedFrame(frame);
+ }
+
+ fixed_frame_property_->changed();
+}
+
bool VisualizationManager::isDeletionAllowed( VisualizerBase* visualizer )
{
VisualizerInfo* info = getVisualizerInfo( visualizer );
@@ -520,4 +547,32 @@
}
}
+void VisualizationManager::updateRelativeNode()
+{
+ tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0.0f, 0.0f, 0.0f ), btVector3( 0.0f, 0.0f, 0.0f ) ),
+ ros::Time(0), target_frame_ );
+
+ try
+ {
+ tf_->transformPose( fixed_frame_, pose, pose );
+ }
+ catch(tf::TransformException& e)
+ {
+ ROS_ERROR( "Error transforming from frame '%s' to frame '%s': %s\n", target_frame_.c_str(), fixed_frame_.c_str() );
+ }
+
+ Ogre::Vector3 position = Ogre::Vector3( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
+ robotToOgre( position );
+
+ btQuaternion quat;
+ pose.getBasis().getRotation( quat );
+ Ogre::Quaternion orientation( Ogre::Quaternion::IDENTITY );
+ ogreToRobot( orientation );
+ orientation = Ogre::Quaternion( quat.w(), quat.x(), quat.y(), quat.z() ) * orientation;
+ robotToOgre( orientation );
+
+ target_relative_node_->setPosition( position );
+ target_relative_node_->setOrientation( orientation );
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -51,6 +51,7 @@
{
class Root;
class SceneManager;
+class SceneNode;
class Camera;
class RaySceneQuery;
class ParticleSystem;
@@ -160,10 +161,17 @@
* \brief Set the coordinate frame we should be displaying in
* @param frame The string name -- must match the frame name broadcast to libTF
*/
- void setCoordinateFrame( const std::string& frame );
- const std::string& getCoordinateFrame() { return target_frame_; }
+ void setTargetFrame( const std::string& frame );
+ const std::string& getTargetFrame() { return target_frame_; }
/**
+ * \brief Set the coordinate frame we should be transforming all fixed data to
+ * @param frame The string name -- must match the frame name broadcast to libTF
+ */
+ void setFixedFrame( const std::string& frame );
+ const std::string& getFixedFrame() { return fixed_frame_; }
+
+ /**
* \brief Performs a linear search to find a visualizer based on its name
* @param name Name of the visualizer to search for
*/
@@ -191,6 +199,8 @@
VisualizerSignal& getVisualizerStateSignal() { return visualizer_state_; }
+ Ogre::SceneNode* getTargetRelativeNode() { return target_relative_node_; }
+
protected:
/**
* \brief Add a visualizer to be managed by this panel
@@ -208,6 +218,8 @@
/// Called from the update timer
void onUpdate( wxTimerEvent& event );
+ void updateRelativeNode();
+
Ogre::Root* ogre_root_; ///< Ogre Root
Ogre::SceneManager* scene_manager_; ///< Ogre scene manager associated with this panel
Ogre::RaySceneQuery* ray_scene_query_; ///< Used for querying the scene based on the mouse location
@@ -234,13 +246,17 @@
M_Factory factories_; ///< Factories by visualizer type name
std::string target_frame_; ///< Target coordinate frame we're displaying everything in
+ std::string fixed_frame_; ///< Frame to transform fixed data to
PropertyManager* property_manager_;
- StringProperty* coordinate_frame_property_;
+ StringProperty* target_frame_property_;
+ StringProperty* fixed_frame_property_;
VisualizationPanel* vis_panel_;
VisualizerSignal visualizer_state_;
+
+ Ogre::SceneNode* target_relative_node_;
};
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -120,14 +120,17 @@
fps_camera_ = new ogre_tools::FPSCamera( manager_->getSceneManager() );
fps_camera_->getOgreCamera()->setNearClipDistance( 0.1f );
fps_camera_->setPosition( 0, 0, 15 );
+ fps_camera_->setRelativeNode( manager_->getTargetRelativeNode() );
orbit_camera_ = new ogre_tools::OrbitCamera( manager_->getSceneManager() );
orbit_camera_->getOgreCamera()->setNearClipDistance( 0.1f );
orbit_camera_->setPosition( 0, 0, 15 );
+ orbit_camera_->setRelativeNode( manager_->getTargetRelativeNode() );
top_down_ortho_ = new ogre_tools::OrthoCamera( render_panel_, manager_->getSceneManager() );
top_down_ortho_->setPosition( 0, 30, 0 );
top_down_ortho_->pitch( -Ogre::Math::HALF_PI );
+ top_down_ortho_->setRelativeNode( manager_->getTargetRelativeNode() );
current_camera_ = orbit_camera_;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -131,6 +131,16 @@
}
}
+void VisualizerBase::setFixedFrame( const std::string& frame )
+{
+ fixed_frame_ = frame;
+
+ if ( isEnabled() )
+ {
+ fixedFrameChanged();
+ }
+}
+
void VisualizerBase::setPropertyManager( PropertyManager* manager, CategoryProperty* parent )
{
property_manager_ = manager;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -127,7 +127,7 @@
*/
virtual void createProperties() {}
- /// Set the target frame of this visualizer. This is a frame id which should match something being broadcast through libTF.
+ /// Set the target frame of this visualizer. This is a frame id which should match something being broadcast through TF.
void setTargetFrame( const std::string& frame );
/**
@@ -136,6 +136,17 @@
virtual void targetFrameChanged() = 0;
/**
+ * \brief Set the fixed frame of this visualizer. This is a frame id which should generally be the top-level frame being broadcast through TF
+ * @param frame The fixed frame
+ */
+ void setFixedFrame( const std::string& frame );
+
+ /**
+ * \brief Called from within setFixedFrame, notifying child classes that the fixed frame has changed
+ */
+ virtual void fixedFrameChanged() = 0;
+
+ /**
* \brief Returns whether an object owned by this visualizer is pickable/mouse selectable
* @param object The Ogre::MovableObject to check
*/
@@ -171,14 +182,15 @@
std::string name_; ///< The name of this visualizer
bool enabled_; ///< Are we enabled?
- std::string target_frame_; ///< The frame we should transform everything into
+ std::string target_frame_; ///< The frame we should transform all periodically-updated data into
+ std::string fixed_frame_; ///< The frame we should transform all fixed data into
boost::function<void ()> render_callback_; ///< Render callback
boost::function<void ()> render_lock_; ///< Render lock callback
boost::function<void ()> render_unlock_; ///< Render unlock callback
ros::node* ros_node_; ///< ros node
- tf::TransformListener* tf_; ///< rosTF client
+ tf::TransformListener* tf_; ///< tf client
std::string property_prefix_; ///< Prefix to prepend to our properties
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -28,6 +28,7 @@
*/
#include "axes_visualizer.h"
+#include "visualization_manager.h"
#include "properties/property.h"
#include "properties/property_manager.h"
#include "common.h"
@@ -47,7 +48,7 @@
, length_( 1.0 )
, radius_( 0.1 )
{
- axes_ = new ogre_tools::Axes( scene_manager_, NULL, length_, radius_ );
+ axes_ = new ogre_tools::Axes( scene_manager_, manager->getTargetRelativeNode(), length_, radius_ );
axes_->getSceneNode()->setVisible( isEnabled() );
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -66,6 +66,7 @@
// Overrides from VisualizerBase
virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged() {}
virtual void createProperties();
static const char* getTypeStatic() { return "Axes"; }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -28,6 +28,8 @@
*/
#include "grid_visualizer.h"
+#include "common.h"
+#include "visualization_manager.h"
#include "properties/property.h"
#include "properties/property_manager.h"
@@ -50,7 +52,11 @@
, cellsize_property_( NULL )
, color_property_( NULL )
{
- grid_ = new ogre_tools::Grid( scene_manager_, cell_count_, cell_size_, color_.r_, color_.g_, color_.b_ );
+ grid_ = new ogre_tools::Grid( scene_manager_, manager->getTargetRelativeNode(), cell_count_, cell_size_, color_.r_, color_.g_, color_.b_ );
+
+ /*Ogre::Quaternion orient( Ogre::Quaternion::IDENTITY );
+ ogreToRobot( orient );
+ grid_->getSceneNode()->setOrientation( orient );*/
}
GridVisualizer::~GridVisualizer()
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -96,6 +96,7 @@
// Overrides from VisualizerBase
virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged() {}
virtual void createProperties();
static const char* getTypeStatic() { return "Grid"; }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -169,6 +169,17 @@
causeRender();
}
+void LaserScanVisualizer::clear()
+{
+ RenderAutoLock renderLock( this );
+
+ cloud_->setCloudVisible( false );
+ cloud_->clear();
+ points_.clear();
+ point_times_.clear();
+ cloud_messages_.clear();
+}
+
void LaserScanVisualizer::onEnable()
{
cloud_->setCloudVisible( true );
@@ -179,11 +190,7 @@
{
unsubscribe();
- cloud_->setCloudVisible( false );
- cloud_->clear();
- points_.clear();
- point_times_.clear();
- cloud_messages_.clear();
+ clear();
}
void LaserScanVisualizer::subscribe()
@@ -270,17 +277,17 @@
if ( message.header.frame_id.empty() )
{
- message.header.frame_id = target_frame_;
+ message.header.frame_id = fixed_frame_;
}
try
{
std_msgs::PointCloud* casted_message = reinterpret_cast<std_msgs::PointCloud*>(&message);
- tf_->transformPointCloud(target_frame_, *casted_message, *casted_message);
+ tf_->transformPointCloud(fixed_frame_, *casted_message, *casted_message);
}
catch(tf::TransformException& e)
{
- ROS_ERROR( "Error transforming laser scan '%s', frame '%s' to frame '%s'\n", name_.c_str(), message.header.frame_id.c_str(), target_frame_.c_str() );
+ ROS_ERROR( "Error transforming laser scan '%s', frame '%s' to frame '%s'\n", name_.c_str(), message.header.frame_id.c_str(), fixed_frame_.c_str() );
}
uint32_t point_count_ = message.get_pts_size();
@@ -363,7 +370,7 @@
if ( scan_message_.header.frame_id.empty() )
{
- scan_message_.header.frame_id = target_frame_;
+ scan_message_.header.frame_id = fixed_frame_;
}
std_msgs::PointCloud* casted_message = reinterpret_cast<std_msgs::PointCloud*>(&cloud_message_);
@@ -373,6 +380,7 @@
cloud_message_.unlock();
}
+#if 0
void LaserScanVisualizer::targetFrameChanged()
{
cloud_message_.lock();
@@ -392,7 +400,13 @@
cloud_message_.unlock();
}
+#endif
+void LaserScanVisualizer::fixedFrameChanged()
+{
+ clear();
+}
+
void LaserScanVisualizer::createProperties()
{
style_property_ = property_manager_->createProperty<EnumProperty>( "Style", property_prefix_, boost::bind( &LaserScanVisualizer::getStyle, this ),
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -122,7 +122,8 @@
virtual void update( float dt );
// Overrides from VisualizerBase
- virtual void targetFrameChanged();
+ virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged();
virtual void createProperties();
static const char* getTypeStatic() { return "Laser Scan"; }
@@ -132,6 +133,8 @@
virtual void onEnable();
virtual void onDisable();
+ void clear();
+
/**
* \brief Subscribes to the cloud and scan topics if they have been set
*/
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -259,14 +259,11 @@
try
{
- if ( target_frame_ != "map" )
- {
- tf_->transformPose( target_frame_, pose, pose );
- }
+ tf_->transformPose( fixed_frame_, pose, pose );
}
catch(tf::TransformException& e)
{
- ROS_ERROR( "Error transforming map '%s' to frame '%s'\n", name_.c_str(), target_frame_.c_str() );
+ ROS_ERROR( "Error transforming map '%s' to frame '%s'\n", name_.c_str(), fixed_frame_.c_str() );
}
Ogre::Vector3 position( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
@@ -284,14 +281,8 @@
{
static float timer = 0.0f;
- if ( loaded_ )
+ if ( !loaded_ )
{
- transformMap();
-
- timer = 0.0f;
- }
- else
- {
timer -= dt;
if ( timer < 0.0f )
@@ -315,4 +306,9 @@
FloatProperty::Setter(), parent_category_, this );
}
+void MapVisualizer::fixedFrameChanged()
+{
+ transformMap();
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -67,6 +67,7 @@
// Overrides from VisualizerBase
virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged();
virtual void createProperties();
virtual void update( float dt );
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -200,7 +200,7 @@
{
Robot* robot = new Robot( scene_manager_ );
robot->load( urdf_, false, true );
- robot->update( kinematic_model_, target_frame_ );
+ robot->update( kinematic_model_, fixed_frame_ );
object = robot;
}
@@ -257,20 +257,18 @@
std::string frame_id = message.header.frame_id;
if ( frame_id.empty() )
{
- frame_id = target_frame_;
+ frame_id = fixed_frame_;
}
- tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( message.yaw, message.pitch, message.roll ), btVector3( message.x, message.y, message.z ) ), ros::Time(0), frame_id );
+ tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( message.yaw, message.pitch, message.roll ), btVector3( message.x, message.y, message.z ) ),
+ ros::Time(message.header.stamp), frame_id );
try
{
- if ( frame_id != target_frame_ )
- {
- tf_->transformPose( target_frame_, pose, pose );
- }
+ tf_->transformPose( fixed_frame_, pose, pose );
}
catch(tf::TransformException& e)
{
- ROS_ERROR( "Error transforming marker '%d' from frame '%s' to frame '%s'\n", message.id, frame_id.c_str(), target_frame_.c_str() );
+ ROS_ERROR( "Error transforming marker '%d' from frame '%s' to frame '%s'\n", message.id, frame_id.c_str(), fixed_frame_.c_str() );
}
Ogre::Vector3 position( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
@@ -295,24 +293,26 @@
{
current_message_.lock();
- if ( !message_queue_.empty() )
+ V_MarkerMessage local_queue;
+ local_queue.swap( message_queue_ );
+
+ current_message_.unlock();
+
+ if ( !local_queue.empty() )
{
- V_MarkerMessage::iterator message_it = message_queue_.begin();
- V_MarkerMessage::iterator message_end = message_queue_.end();
+ V_MarkerMessage::iterator message_it = local_queue.begin();
+ V_MarkerMessage::iterator message_end = local_queue.end();
for ( ; message_it != message_end; ++message_it )
{
std_msgs::VisualizationMarker& marker = *message_it;
processMessage( marker );
}
-
- message_queue_.clear();
}
-
- current_message_.unlock();
}
-void MarkerVisualizer::targetFrameChanged()
+#if 0
+void MarkerVisualizer::targetFrameChanged();
{
M_IDToMarker::iterator marker_it = markers_.begin();
M_IDToMarker::iterator marker_end = markers_.end();
@@ -325,5 +325,11 @@
processMessage( info.message_ );
}
}
+#endif
+void MarkerVisualizer::fixedFrameChanged()
+{
+ clearMarkers();
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -104,7 +104,8 @@
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return true; }
- virtual void targetFrameChanged();
+ virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged();
static const char* getTypeStatic() { return "Markers"; }
virtual const char* getType() { return getTypeStatic(); }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -75,6 +75,7 @@
// Overrides from VisualizerBase
virtual void targetFrameChanged();
+ virtual void fixedFrameChanged() {}
virtual void createProperties();
virtual void update( float dt );
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -113,6 +113,7 @@
// Overrides from VisualizerBase
virtual void targetFrameChanged();
+ virtual void fixedFrameChanged() {}
virtual void createProperties();
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return true; }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -176,18 +176,18 @@
{
if ( message_.header.frame_id.empty() )
{
- message_.header.frame_id = target_frame_;
+ message_.header.frame_id = fixed_frame_;
}
tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0, 0, 0 ), btVector3( 0, 0, 0 ) ), message_.header.stamp, message_.header.frame_id );
try
{
- tf_->transformPose( target_frame_, pose, pose );
+ tf_->transformPose( fixed_frame_, pose, pose );
}
catch(tf::TransformException& e)
{
- ROS_ERROR( "Error transforming point cloud '%s' from frame '%s' to frame '%s'\n", name_.c_str(), message_.header.frame_id.c_str(), target_frame_.c_str() );
+ ROS_ERROR( "Error transforming point cloud '%s' from frame '%s' to frame '%s'\n", name_.c_str(), message_.header.frame_id.c_str(), fixed_frame_.c_str() );
}
Ogre::Vector3 position( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
@@ -285,6 +285,7 @@
transformCloud();
}
+#if 0
void PointCloudVisualizer::targetFrameChanged()
{
message_.lock();
@@ -293,7 +294,15 @@
message_.unlock();
}
+#endif
+void PointCloudVisualizer::fixedFrameChanged()
+{
+ RenderAutoLock renderLock( this );
+
+ cloud_->clear();
+}
+
void PointCloudVisualizer::createProperties()
{
style_property_ = property_manager_->createProperty<EnumProperty>( "Style", property_prefix_, boost::bind( &PointCloudVisualizer::getStyle, this ),
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -105,7 +105,8 @@
int getStyle() { return style_; }
// Overrides from VisualizerBase
- virtual void targetFrameChanged();
+ virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged();
virtual void createProperties();
static const char* getTypeStatic() { return "Point Cloud"; }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -209,7 +209,7 @@
std::string frame_id = message.header.frame_id;
if ( frame_id.empty() )
{
- frame_id = target_frame_;
+ frame_id = fixed_frame_;
}
tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( message.pos.th, 0.0f, 0.0f ), btVector3( message.pos.x, message.pos.y, 0.0f ) ),
@@ -217,14 +217,11 @@
try
{
- if ( frame_id != target_frame_ )
- {
- tf_->transformPose( target_frame_, pose, pose );
- }
+ tf_->transformPose( fixed_frame_, pose, pose );
}
catch(tf::TransformException& e)
{
- ROS_ERROR( "Error transforming 2d base pose '%s' from frame '%s' to frame '%s': %s\n", name_.c_str(), message.header.frame_id.c_str(), target_frame_.c_str(), e.what() );
+ ROS_ERROR( "Error transforming 2d base pose '%s' from frame '%s' to frame '%s'\n", name_.c_str(), message.header.frame_id.c_str(), fixed_frame_.c_str() );
}
btScalar yaw, pitch, roll;
@@ -238,6 +235,7 @@
arrow->setPosition( pos );
}
+#if 0
void RobotBase2DPoseVisualizer::targetFrameChanged()
{
ROS_ASSERT( messages_.size() == arrows_.size() );
@@ -249,7 +247,13 @@
transformArrow( *msg_it, *arrow_it );
}
}
+#endif
+void RobotBase2DPoseVisualizer::fixedFrameChanged()
+{
+ clear();
+}
+
void RobotBase2DPoseVisualizer::update( float dt )
{
V_RobotBase2DOdom local_queue;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -77,7 +77,8 @@
float getAngleTolerance() { return angle_tolerance_; }
// Overrides from VisualizerBase
- virtual void targetFrameChanged();
+ virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged();
virtual void createProperties();
virtual void update( float dt );
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return true; }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -211,7 +211,7 @@
if ( has_new_transforms_ || update )
{
- robot_->update( tf_, target_frame_ );
+ robot_->update( tf_, fixed_frame_ );
causeRender();
has_new_transforms_ = false;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -110,6 +110,7 @@
// Overrides from VisualizerBase
virtual void targetFrameChanged();
+ virtual void fixedFrameChanged() {}
virtual void createProperties();
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return true; }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -115,6 +115,7 @@
// Overrides from VisualizerBase
virtual void update( float dt );
virtual void targetFrameChanged();
+ virtual void fixedFrameChanged() {}
virtual void createProperties();
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return true; }
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
