Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21672] pkg/trunk
|
From: <jfa...@us...> - 2009-08-12 18:55:27
|
Revision: 21672
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21672&view=rev
Author: jfaustwg
Date: 2009-08-12 18:55:19 +0000 (Wed, 12 Aug 2009)
Log Message:
-----------
Remove use of Polyline from nav_view, rviz
rviz PolyLine2D display split into GridCells, Path, Polygon
Modified Paths:
--------------
pkg/trunk/highlevel/move_base_stage/move_base/nav_view.xml
pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/stacks/visualization/rviz/CMakeLists.txt
pkg/trunk/stacks/visualization/rviz/src/rviz/factory.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/visualization_manager.i
Added Paths:
-----------
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.h
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.h
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/polygon_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/polygon_display.h
Removed Paths:
-------------
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
Modified: pkg/trunk/highlevel/move_base_stage/move_base/nav_view.xml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/nav_view.xml 2009-08-12 18:42:45 UTC (rev 21671)
+++ pkg/trunk/highlevel/move_base_stage/move_base/nav_view.xml 2009-08-12 18:55:19 UTC (rev 21672)
@@ -1,10 +1,10 @@
<launch>
<node pkg="nav_view" type="nav_view" respawn="false">
<remap from="goal" to="/move_base/activate" />
- <remap from="raw_obstacles" to="/move_base/local_costmap/raw_obstacles" />
- <remap from="inflated_obstacles" to="/move_base/local_costmap/inflated_obstacles" />
- <remap from="gui_path" to="/move_base/NavfnROS/plan" />
- <remap from="local_path" to="/move_base/TrajectoryPlannerROS/local_plan" />
- <remap from="robot_footprint" to="/move_base/TrajectoryPlannerROS/robot_footprint"/>
+ <remap from="obstacles" to="/move_base/local_costmap/obstacles_new" />
+ <remap from="inflated_obstacles" to="/move_base/local_costmap/inflated_obstacles_new" />
+ <remap from="global_plan" to="/move_base/NavfnROS/plan_new" />
+ <remap from="local_plan" to="/move_base/TrajectoryPlannerROS/local_plan_new" />
+ <remap from="robot_footprint" to="/move_base/TrajectoryPlannerROS/robot_footprint_new"/>
</node>
</launch>
Modified: pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.cpp 2009-08-12 18:42:45 UTC (rev 21671)
+++ pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.cpp 2009-08-12 18:55:19 UTC (rev 21672)
@@ -67,8 +67,8 @@
, map_object_( NULL )
, cloud_object_( NULL )
, radius_object_( NULL )
-, path_line_object_( NULL )
-, local_path_object_( NULL )
+, global_plan_object_( NULL )
+, local_plan_object_( NULL )
, footprint_object_( NULL )
, inflated_obstacles_object_( NULL )
, raw_obstacles_object_( NULL )
@@ -98,25 +98,22 @@
particle_cloud_sub_ = nh_.subscribe("particlecloud", 1, &NavViewPanel::incomingPoseArray, this);
- gui_path_sub_.subscribe(nh_, "gui_path", 2);
- local_path_sub_.subscribe(nh_, "local_path", 2);
+ global_plan_sub_.subscribe(nh_, "global_plan", 2);
+ local_plan_sub_.subscribe(nh_, "local_plan", 2);
robot_footprint_sub_.subscribe(nh_, "robot_footprint", 2);
inflated_obs_sub_.subscribe(nh_, "inflated_obstacles", 2);
- raw_obs_sub_.subscribe(nh_, "raw_obstacles", 2);
- gui_laser_sub_.subscribe(nh_, "gui_laser", 2);
- gui_path_filter_ = PolylineFilterPtr(new PolylineFilter(gui_path_sub_, *tf_client_, global_frame_id_, 2));
- local_path_filter_ = PolylineFilterPtr(new PolylineFilter(local_path_sub_, *tf_client_, global_frame_id_, 2));
- robot_footprint_filter_ = PolylineFilterPtr(new PolylineFilter(robot_footprint_sub_, *tf_client_, global_frame_id_, 2));
- inflated_obs_filter_ = PolylineFilterPtr(new PolylineFilter(inflated_obs_sub_, *tf_client_, global_frame_id_, 2));
- raw_obs_filter_ = PolylineFilterPtr(new PolylineFilter(raw_obs_sub_, *tf_client_, global_frame_id_, 2));
- gui_laser_filter_ = PolylineFilterPtr(new PolylineFilter(gui_laser_sub_, *tf_client_, global_frame_id_, 2));
+ raw_obs_sub_.subscribe(nh_, "obstacles", 2);
+ global_plan_filter_.reset(new PathFilter(global_plan_sub_, *tf_client_, global_frame_id_, 2));
+ local_plan_filter_.reset(new PathFilter(local_plan_sub_, *tf_client_, global_frame_id_, 2));
+ robot_footprint_filter_.reset(new PolygonFilter(robot_footprint_sub_, *tf_client_, global_frame_id_, 2));
+ inflated_obs_filter_.reset(new GridCellsFilter(inflated_obs_sub_, *tf_client_, global_frame_id_, 2));
+ raw_obs_filter_.reset(new GridCellsFilter(raw_obs_sub_, *tf_client_, global_frame_id_, 2));
- gui_path_filter_->registerCallback(boost::bind(&NavViewPanel::incomingGuiPath, this, _1));
- local_path_filter_->registerCallback(boost::bind(&NavViewPanel::incomingLocalPath, this, _1));
+ global_plan_filter_->registerCallback(boost::bind(&NavViewPanel::incomingGuiPath, this, _1));
+ local_plan_filter_->registerCallback(boost::bind(&NavViewPanel::incomingLocalPath, this, _1));
robot_footprint_filter_->registerCallback(boost::bind(&NavViewPanel::incomingRobotFootprint, this, _1));
inflated_obs_filter_->registerCallback(boost::bind(&NavViewPanel::incomingInflatedObstacles, this, _1));
raw_obs_filter_->registerCallback(boost::bind(&NavViewPanel::incomingRawObstacles, this, _1));
- gui_laser_filter_->registerCallback(boost::bind(&NavViewPanel::incomingGuiLaser, this, _1));
render_panel_->Connect( wxEVT_LEFT_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
render_panel_->Connect( wxEVT_MIDDLE_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
@@ -160,18 +157,16 @@
render_panel_->Disconnect( wxEVT_MOUSEWHEEL, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
particle_cloud_sub_.shutdown();
- gui_path_sub_.unsubscribe();
- local_path_sub_.unsubscribe();
+ global_plan_sub_.unsubscribe();
+ local_plan_sub_.unsubscribe();
robot_footprint_sub_.unsubscribe();
inflated_obs_sub_.unsubscribe();
raw_obs_sub_.unsubscribe();
- gui_laser_sub_.unsubscribe();
- gui_path_filter_.reset();
- local_path_filter_.reset();
+ global_plan_filter_.reset();
+ local_plan_filter_.reset();
robot_footprint_filter_.reset();
inflated_obs_filter_.reset();
raw_obs_filter_.reset();
- gui_laser_filter_.reset();
delete update_timer_;
delete current_tool_;
@@ -425,7 +420,7 @@
{
static int count = 0;
std::stringstream ss;
- ss << "NavViewPathLine" << count++;
+ ss << "NavViewPolyLine" << count++;
object = scene_manager_->createManualObject( ss.str() );
Ogre::SceneNode* node = root_node_->createChildSceneNode();
node->attachObject( object );
@@ -474,6 +469,136 @@
queueRender();
}
+void NavViewPanel::createObjectFromPath(Ogre::ManualObject*& object, const nav_msgs::Path& path, const Ogre::ColourValue& color, float depth)
+{
+ if ( !object )
+ {
+ static int count = 0;
+ std::stringstream ss;
+ ss << "NavViewPath" << count++;
+ object = scene_manager_->createManualObject( ss.str() );
+ Ogre::SceneNode* node = root_node_->createChildSceneNode();
+ node->attachObject( object );
+ }
+
+ object->clear();
+
+ size_t num_points = path.poses.size();
+
+ if ( num_points > 0 )
+ {
+ object->estimateVertexCount( num_points);
+ object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
+ for( size_t i=0; i < num_points; ++i)
+ {
+ tf::Stamped<tf::Point> point;
+ tf::pointMsgToTF(path.poses[i].pose.position, point);
+ point.frame_id_ = path.header.frame_id;
+ point.stamp_ = path.header.stamp;
+
+ tf_client_->transformPoint(global_frame_id_, point, point);
+
+ object->position(point.x(), point.y(), point.z());
+ object->colour( color );
+ }
+
+ object->end();
+
+ object->getParentSceneNode()->setPosition( Ogre::Vector3( 0.0f, 0.0f, depth ) );
+ }
+
+ queueRender();
+}
+
+void point32MsgToTF(const geometry_msgs::Point32& from, tf::Point& to)
+{
+ to.setX(from.x);
+ to.setY(from.y);
+ to.setZ(from.z);
+}
+
+void NavViewPanel::createObjectFromPolygon(Ogre::ManualObject*& object, const geometry_msgs::PolygonStamped& polygon, const Ogre::ColourValue& color, float depth)
+{
+ if ( !object )
+ {
+ static int count = 0;
+ std::stringstream ss;
+ ss << "NavViewPolygon" << count++;
+ object = scene_manager_->createManualObject( ss.str() );
+ Ogre::SceneNode* node = root_node_->createChildSceneNode();
+ node->attachObject( object );
+ }
+
+ object->clear();
+
+ size_t num_points = polygon.polygon.points.size();
+
+ if ( num_points > 0 )
+ {
+ object->estimateVertexCount( num_points);
+ object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
+ for( size_t i=0; i < num_points + 1; ++i)
+ {
+ tf::Stamped<tf::Point> point;
+ point32MsgToTF(polygon.polygon.points[i % num_points], point);
+ point.frame_id_ = polygon.header.frame_id;
+ point.stamp_ = polygon.header.stamp;
+
+ tf_client_->transformPoint(global_frame_id_, point, point);
+
+ object->position(point.x(), point.y(), point.z());
+ object->colour( color );
+ }
+
+ object->end();
+
+ object->getParentSceneNode()->setPosition( Ogre::Vector3( 0.0f, 0.0f, depth ) );
+ }
+
+ queueRender();
+}
+
+void NavViewPanel::createObjectFromGridCells(Ogre::ManualObject*& object, const nav_msgs::GridCells& cells, const Ogre::ColourValue& color, float depth)
+{
+ if ( !object )
+ {
+ static int count = 0;
+ std::stringstream ss;
+ ss << "NavViewGridCells" << count++;
+ object = scene_manager_->createManualObject( ss.str() );
+ Ogre::SceneNode* node = root_node_->createChildSceneNode();
+ node->attachObject( object );
+ }
+
+ object->clear();
+
+ size_t num_points = cells.cells.size();
+
+ if ( num_points > 0 )
+ {
+ object->estimateVertexCount( num_points);
+ object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_POINT_LIST );
+ for( size_t i=0; i < num_points; ++i)
+ {
+ tf::Stamped<tf::Point> point;
+ tf::pointMsgToTF(cells.cells[i], point);
+ point.frame_id_ = cells.header.frame_id;
+ point.stamp_ = cells.header.stamp;
+
+ tf_client_->transformPoint(global_frame_id_, point, point);
+
+ object->position(point.x(), point.y(), point.z());
+ object->colour( color );
+ }
+
+ object->end();
+
+ object->getParentSceneNode()->setPosition( Ogre::Vector3( 0.0f, 0.0f, depth ) );
+ }
+
+ queueRender();
+}
+
void NavViewPanel::incomingPoseArray(const geometry_msgs::PoseArray::ConstPtr& msg)
{
if ( !cloud_object_ )
@@ -524,36 +649,31 @@
queueRender();
}
-void NavViewPanel::incomingGuiPath(const visualization_msgs::Polyline::ConstPtr& msg)
+void NavViewPanel::incomingGuiPath(const nav_msgs::Path::ConstPtr& msg)
{
- createObjectFromPolyLine( path_line_object_, *msg, Ogre::RenderOperation::OT_LINE_LIST, PATH_LINE_DEPTH, false );
+ createObjectFromPath( global_plan_object_, *msg, Ogre::ColourValue(0.0, 1.0, 0.0), PATH_LINE_DEPTH );
}
-void NavViewPanel::incomingLocalPath(const visualization_msgs::Polyline::ConstPtr& msg)
+void NavViewPanel::incomingLocalPath(const nav_msgs::Path::ConstPtr& msg)
{
- createObjectFromPolyLine( local_path_object_, *msg, Ogre::RenderOperation::OT_LINE_LIST, LOCAL_PATH_DEPTH, false );
+ createObjectFromPath( local_plan_object_, *msg, Ogre::ColourValue(0.0, 0.0, 1.0), LOCAL_PATH_DEPTH );
}
-void NavViewPanel::incomingRobotFootprint(const visualization_msgs::Polyline::ConstPtr& msg)
+void NavViewPanel::incomingRobotFootprint(const geometry_msgs::PolygonStamped::ConstPtr& msg)
{
- createObjectFromPolyLine( footprint_object_, *msg, Ogre::RenderOperation::OT_LINE_LIST, FOOTPRINT_DEPTH, true );
+ createObjectFromPolygon( footprint_object_, *msg, Ogre::ColourValue(1.0, 0.0, 0.0), FOOTPRINT_DEPTH );
}
-void NavViewPanel::incomingInflatedObstacles(const visualization_msgs::Polyline::ConstPtr& msg)
+void NavViewPanel::incomingInflatedObstacles(const nav_msgs::GridCells::ConstPtr& msg)
{
- createObjectFromPolyLine( inflated_obstacles_object_, *msg, Ogre::RenderOperation::OT_POINT_LIST, INFLATED_OBSTACLES_DEPTH, false );
+ createObjectFromGridCells( inflated_obstacles_object_, *msg, Ogre::ColourValue(0.0, 0.0, 1.0), INFLATED_OBSTACLES_DEPTH );
}
-void NavViewPanel::incomingRawObstacles(const visualization_msgs::Polyline::ConstPtr& msg)
+void NavViewPanel::incomingRawObstacles(const nav_msgs::GridCells::ConstPtr& msg)
{
- createObjectFromPolyLine( raw_obstacles_object_, *msg, Ogre::RenderOperation::OT_POINT_LIST, RAW_OBSTACLES_DEPTH, false );
+ createObjectFromGridCells( raw_obstacles_object_, *msg, Ogre::ColourValue(1.0, 0.0, 0.0), RAW_OBSTACLES_DEPTH );
}
-void NavViewPanel::incomingGuiLaser(const visualization_msgs::Polyline::ConstPtr& msg)
-{
- createObjectFromPolyLine( laser_scan_object_, *msg, Ogre::RenderOperation::OT_POINT_LIST, LASER_SCAN_DEPTH, false );
-}
-
void NavViewPanel::onRenderWindowMouseEvents( wxMouseEvent& event )
{
int last_x = mouse_x_;
Modified: pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.h 2009-08-12 18:42:45 UTC (rev 21671)
+++ pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.h 2009-08-12 18:55:19 UTC (rev 21672)
@@ -34,8 +34,11 @@
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/PoseStamped.h"
+#include "geometry_msgs/PolygonStamped.h"
+#include "nav_msgs/Path.h"
+#include "nav_msgs/GridCells.h"
+#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "visualization_msgs/Polyline.h"
-#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <OGRE/OgreTexture.h>
#include <OGRE/OgreMaterial.h>
@@ -89,17 +92,16 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "particlecloud"geometry_msgs/PoseArray : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
-- @b "gui_path"/Polyline : a path from a planner. Rendered as a dashed line.
-- @b "gui_laser"/Polyline : re-projected laser scan from a planner. Rendered as a set of points.
-- @b "local_path"/Polyline : local path from a planner. Rendered as a dashed line.
-- @b "robot_footprint"/Polyline : Box "footprint" around the robot. Rendered as a dashed line
-- @b "raw_obstacles"/Polyline : Raw obstacle data. Rendered as points
-- @b "inflated_obstacles"/Polyline : Inflated obstacle data. Rendered as points
+- @b "particlecloud" - geometry_msgs/PoseArray : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
+- @b "global_plan" - nav_msgs/Path : a path from a planner. Rendered as a line.
+- @b "local_plan" - nav_msgs/Path: local path from a planner. Rendered as a line.
+- @b "robot_footprint" - geometry_msgs/Polygon : Box "footprint" around the robot. Rendered as a dashed line
+- @b "obstacles" - nav_msgs/GridCells : Raw obstacle data. Rendered as points
+- @b "inflated_obstacles" - nav_msgs/GridCells : Inflated obstacle data. Rendered as points
Publishes to (name / type):
-- @b "goal"/PoseStamped : goal for planner. Sent when using the Goal tool
-- @b "initialpose"/Pose2DFloat32 : pose to initialize localization system. Sent when using the Pose tool
+- @b "goal" - geometry_msgs/PoseStamped : goal for planner. Sent when using the Goal tool
+- @b "initialpose" - geometry_msgs/PoseWithCovarianceStamped : pose to initialize localization system. Sent when using the Pose tool
<hr>
@@ -165,11 +167,11 @@
void loadMap();
void clearMap();
void incomingPoseArray(const geometry_msgs::PoseArray::ConstPtr& msg);
- void incomingGuiPath(const visualization_msgs::Polyline::ConstPtr& msg);
- void incomingLocalPath(const visualization_msgs::Polyline::ConstPtr& msg);
- void incomingRobotFootprint(const visualization_msgs::Polyline::ConstPtr& msg);
- void incomingInflatedObstacles(const visualization_msgs::Polyline::ConstPtr& msg);
- void incomingRawObstacles(const visualization_msgs::Polyline::ConstPtr& msg);
+ void incomingGuiPath(const nav_msgs::Path::ConstPtr& msg);
+ void incomingLocalPath(const nav_msgs::Path::ConstPtr& msg);
+ void incomingRobotFootprint(const geometry_msgs::PolygonStamped::ConstPtr& msg);
+ void incomingInflatedObstacles(const nav_msgs::GridCells::ConstPtr& msg);
+ void incomingRawObstacles(const nav_msgs::GridCells::ConstPtr& msg);
void incomingGuiLaser(const visualization_msgs::Polyline::ConstPtr& msg);
void onRenderWindowMouseEvents( wxMouseEvent& event );
@@ -177,7 +179,10 @@
void createRadiusObject();
void updateRadiusPosition();
- void createObjectFromPolyLine( Ogre::ManualObject*& object, const visualization_msgs::Polyline& path, Ogre::RenderOperation::OperationType op, float depth, bool loop );
+ void createObjectFromPolyLine(Ogre::ManualObject*& object, const visualization_msgs::Polyline& path, Ogre::RenderOperation::OperationType op, float depth, bool loop);
+ void createObjectFromPath(Ogre::ManualObject*& object, const nav_msgs::Path& path, const Ogre::ColourValue& color, float depth);
+ void createObjectFromPolygon(Ogre::ManualObject*& object, const geometry_msgs::PolygonStamped& polygon, const Ogre::ColourValue& color, float depth);
+ void createObjectFromGridCells(Ogre::ManualObject*& object, const nav_msgs::GridCells& cells, const Ogre::ColourValue& color, float depth);
void createTransientObject();
@@ -198,26 +203,30 @@
typedef tf::MessageFilter<visualization_msgs::Polyline> PolylineFilter;
typedef boost::shared_ptr<PolylineFilter> PolylineFilterPtr;
- message_filters::Subscriber<visualization_msgs::Polyline> gui_path_sub_;
- PolylineFilterPtr gui_path_filter_;
- message_filters::Subscriber<visualization_msgs::Polyline> local_path_sub_;
- PolylineFilterPtr local_path_filter_;
- message_filters::Subscriber<visualization_msgs::Polyline> robot_footprint_sub_;
- PolylineFilterPtr robot_footprint_filter_;
- message_filters::Subscriber<visualization_msgs::Polyline> inflated_obs_sub_;
- PolylineFilterPtr inflated_obs_filter_;
- message_filters::Subscriber<visualization_msgs::Polyline> raw_obs_sub_;
- PolylineFilterPtr raw_obs_filter_;
- message_filters::Subscriber<visualization_msgs::Polyline> gui_laser_sub_;
- PolylineFilterPtr gui_laser_filter_;
+ typedef tf::MessageFilter<nav_msgs::Path> PathFilter;
+ typedef boost::shared_ptr<PathFilter> PathFilterPtr;
+ typedef tf::MessageFilter<nav_msgs::GridCells> GridCellsFilter;
+ typedef boost::shared_ptr<GridCellsFilter> GridCellsFilterPtr;
+ typedef tf::MessageFilter<geometry_msgs::PolygonStamped> PolygonFilter;
+ typedef boost::shared_ptr<PolygonFilter> PolygonFilterPtr;
+ message_filters::Subscriber<nav_msgs::Path> global_plan_sub_;
+ PathFilterPtr global_plan_filter_;
+ message_filters::Subscriber<nav_msgs::Path> local_plan_sub_;
+ PathFilterPtr local_plan_filter_;
+ message_filters::Subscriber<geometry_msgs::PolygonStamped> robot_footprint_sub_;
+ PolygonFilterPtr robot_footprint_filter_;
+ message_filters::Subscriber<nav_msgs::GridCells> inflated_obs_sub_;
+ GridCellsFilterPtr inflated_obs_filter_;
+ message_filters::Subscriber<nav_msgs::GridCells> raw_obs_sub_;
+ GridCellsFilterPtr raw_obs_filter_;
Ogre::ManualObject* map_object_;
Ogre::MaterialPtr map_material_;
Ogre::ManualObject* cloud_object_;
Ogre::ManualObject* radius_object_;
- Ogre::ManualObject* path_line_object_;
- Ogre::ManualObject* local_path_object_;
+ Ogre::ManualObject* global_plan_object_;
+ Ogre::ManualObject* local_plan_object_;
Ogre::ManualObject* footprint_object_;
Ogre::ManualObject* inflated_obstacles_object_;
Ogre::ManualObject* raw_obstacles_object_;
Modified: pkg/trunk/stacks/visualization/rviz/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/visualization/rviz/CMakeLists.txt 2009-08-12 18:42:45 UTC (rev 21671)
+++ pkg/trunk/stacks/visualization/rviz/CMakeLists.txt 2009-08-12 18:55:19 UTC (rev 21672)
@@ -71,7 +71,6 @@
src/rviz/displays/robot_model_display.h
src/rviz/displays/robot_base2d_pose_display.h
src/rviz/displays/particle_cloud_2d_display.h
- src/rviz/displays/poly_line_2d_display.h
src/rviz/displays/map_display.h
src/rviz/displays/tf_display.h
src/rviz/displays/camera_display.h
@@ -120,7 +119,9 @@
src/rviz/displays/planning_display.cpp
src/rviz/displays/robot_base2d_pose_display.cpp
src/rviz/displays/particle_cloud_2d_display.cpp
- src/rviz/displays/poly_line_2d_display.cpp
+ src/rviz/displays/path_display.cpp
+ src/rviz/displays/polygon_display.cpp
+ src/rviz/displays/grid_cells_display.cpp
src/rviz/displays/polygonal_map_display.cpp
src/rviz/displays/collision_map_display.cpp
src/rviz/displays/map_display.cpp
Copied: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.cpp (from rev 21616, pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp)
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.cpp (rev 0)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.cpp 2009-08-12 18:55:19 UTC (rev 21672)
@@ -0,0 +1,264 @@
+/*
+ * 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.
+ */
+
+#include "grid_cells_display.h"
+#include "visualization_manager.h"
+#include "properties/property.h"
+#include "properties/property_manager.h"
+#include "common.h"
+
+#include "ogre_tools/arrow.h"
+
+#include <tf/transform_listener.h>
+
+#include <boost/bind.hpp>
+
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreManualObject.h>
+#include <OGRE/OgreBillboardSet.h>
+
+#include <ogre_tools/point_cloud.h>
+
+namespace rviz
+{
+
+GridCellsDisplay::GridCellsDisplay( const std::string& name, VisualizationManager* manager )
+: Display( name, manager )
+, color_( 0.1f, 1.0f, 0.0f )
+, tf_filter_(*manager->getTFClient(), "", 10, update_nh_)
+{
+ scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+
+ static int count = 0;
+ std::stringstream ss;
+ ss << "PolyLine" << count++;
+
+ cloud_ = new ogre_tools::PointCloud();
+ cloud_->setRenderMode( ogre_tools::PointCloud::RM_BILLBOARDS_COMMON_FACING );
+ cloud_->setCommonDirection( Ogre::Vector3::UNIT_Y );
+ cloud_->setCommonUpVector( Ogre::Vector3::NEGATIVE_UNIT_Z );
+ scene_node_->attachObject(cloud_);
+ setAlpha( 1.0f );
+
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&GridCellsDisplay::incomingMessage, this, _1));
+}
+
+GridCellsDisplay::~GridCellsDisplay()
+{
+ unsubscribe();
+ clear();
+
+ scene_manager_->destroySceneNode(scene_node_->getName());
+ delete cloud_;
+}
+
+void GridCellsDisplay::clear()
+{
+ cloud_->clear();
+}
+
+void GridCellsDisplay::setTopic( const std::string& topic )
+{
+ unsubscribe();
+
+ topic_ = topic;
+
+ subscribe();
+
+ propertyChanged(topic_property_);
+
+ causeRender();
+}
+
+void GridCellsDisplay::setColor( const Color& color )
+{
+ color_ = color;
+
+ propertyChanged(color_property_);
+
+ processMessage(current_message_);
+ causeRender();
+}
+
+void GridCellsDisplay::setAlpha( float alpha )
+{
+ alpha_ = alpha;
+
+ cloud_->setAlpha( alpha );
+
+ propertyChanged(alpha_property_);
+
+ processMessage(current_message_);
+ causeRender();
+}
+
+void GridCellsDisplay::subscribe()
+{
+ if ( !isEnabled() )
+ {
+ return;
+ }
+
+ sub_.subscribe(update_nh_, topic_, 10);
+}
+
+void GridCellsDisplay::unsubscribe()
+{
+ sub_.unsubscribe();
+}
+
+void GridCellsDisplay::onEnable()
+{
+ scene_node_->setVisible( true );
+ subscribe();
+}
+
+void GridCellsDisplay::onDisable()
+{
+ unsubscribe();
+ clear();
+ scene_node_->setVisible( false );
+}
+
+void GridCellsDisplay::fixedFrameChanged()
+{
+ clear();
+
+ tf_filter_.setTargetFrame( fixed_frame_ );
+}
+
+void GridCellsDisplay::update(float wall_dt, float ros_dt)
+{
+}
+
+void GridCellsDisplay::processMessage(const nav_msgs::GridCells::ConstPtr& msg)
+{
+ if (!msg)
+ {
+ return;
+ }
+
+ clear();
+
+ std::string frame_id = msg->header.frame_id;
+ if (frame_id.empty())
+ {
+ frame_id = "/map";
+ }
+
+ tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0.0f, 0.0f, 0.0f ), btVector3( 0.0f, 0.0f, 0.0f ) ),
+ ros::Time(), frame_id);
+
+ if (vis_manager_->getTFClient()->canTransform(fixed_frame_, frame_id, ros::Time()))
+ {
+ try
+ {
+ vis_manager_->getTFClient()->transformPose( fixed_frame_, pose, pose );
+ }
+ catch(tf::TransformException& e)
+ {
+ ROS_ERROR( "Error transforming from frame 'map' to frame '%s'", 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 );
+
+ scene_node_->setPosition( position );
+ scene_node_->setOrientation( orientation );
+
+ Ogre::ColourValue color( color_.r_, color_.g_, color_.b_, alpha_ );
+
+ cloud_->setDimensions(msg->cell_width, msg->cell_height, 0.0);
+
+ uint32_t num_points = msg->cells.size();
+
+ typedef std::vector< ogre_tools::PointCloud::Point > V_Point;
+ V_Point points;
+ points.resize( num_points );
+ for(uint32_t i = 0; i < num_points; i++)
+ {
+ ogre_tools::PointCloud::Point& current_point = points[ i ];
+
+ Ogre::Vector3 pos(msg->cells[i].x, msg->cells[i].y, msg->cells[i].z);
+ robotToOgre(pos);
+
+ current_point.x = pos.x;
+ current_point.y = pos.y;
+ current_point.z = pos.z;
+ current_point.setColor(color.r, color.g, color.b);
+ }
+
+ cloud_->clear();
+
+ if ( !points.empty() )
+ {
+ cloud_->addPoints( &points.front(), points.size() );
+ }
+}
+
+void GridCellsDisplay::incomingMessage(const nav_msgs::GridCells::ConstPtr& msg)
+{
+ processMessage(msg);
+}
+
+void GridCellsDisplay::reset()
+{
+ clear();
+}
+
+void GridCellsDisplay::createProperties()
+{
+ color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &GridCellsDisplay::getColor, this ),
+ boost::bind( &GridCellsDisplay::setColor, this, _1 ), category_, this );
+ alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &GridCellsDisplay::getAlpha, this ),
+ boost::bind( &GridCellsDisplay::setAlpha, this, _1 ), category_, this );
+
+ topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &GridCellsDisplay::getTopic, this ),
+ boost::bind( &GridCellsDisplay::setTopic, this, _1 ), category_, this );
+ ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
+ topic_prop->setMessageType(nav_msgs::GridCells::__s_getDataType());
+}
+
+const char* GridCellsDisplay::getDescription()
+{
+ return "Displays data from a nav_msgs::GridCells message as either points or lines.";
+}
+
+} // namespace rviz
+
Property changes on: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
/pkg/branches/josh/rviz_handles/rviz/src/rviz/displays/poly_line_2d_display.cpp:18490-18807
/pkg/branches/josh/rviz_selection/rviz/src/rviz/displays/poly_line_2d_display.cpp:12152-13640
/pkg/branches/josh/visualization_scratch/rviz/src/rviz/displays/poly_line_2d_display.cpp:14041-14428,16191-17859
Copied: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.h (from rev 21616, pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.h)
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.h (rev 0)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.h 2009-08-12 18:55:19 UTC (rev 21672)
@@ -0,0 +1,120 @@
+/*
+ * 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.
+ */
+
+
+#ifndef RVIZ_GRID_CELLS_DISPLAY_H
+#define RVIZ_GRID_CELLS_DISPLAY_H
+
+#include "display.h"
+#include "helpers/color.h"
+#include "properties/forwards.h"
+
+#include <nav_msgs/GridCells.h>
+#include <nav_msgs/MapMetaData.h>
+
+#include <message_filters/subscriber.h>
+#include <tf/message_filter.h>
+
+#include <boost/shared_ptr.hpp>
+
+namespace ogre_tools
+{
+class PointCloud;
+}
+
+namespace Ogre
+{
+class SceneNode;
+class ManualObject;
+}
+
+namespace rviz
+{
+
+/**
+ * \class GridCellsDisplay
+ * \brief Displays a nav_msgs::GridCells message
+ */
+class GridCellsDisplay : public Display
+{
+public:
+ GridCellsDisplay( const std::string& name, VisualizationManager* manager );
+ virtual ~GridCellsDisplay();
+
+ void setTopic( const std::string& topic );
+ const std::string& getTopic() { return topic_; }
+
+ void setColor( const Color& color );
+ const Color& getColor() { return color_; }
+
+ void setAlpha( float alpha );
+ float getAlpha() { return alpha_; }
+
+ // Overrides from Display
+ virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged();
+ virtual void createProperties();
+ virtual void update(float wall_dt, float ros_dt);
+ virtual void reset();
+
+ static const char* getTypeStatic() { return "GridCells"; }
+ virtual const char* getType() const { return getTypeStatic(); }
+ static const char* getDescription();
+
+protected:
+ void subscribe();
+ void unsubscribe();
+ void clear();
+ void incomingMessage(const nav_msgs::GridCells::ConstPtr& msg);
+ void processMessage(const nav_msgs::GridCells::ConstPtr& msg);
+
+ // overrides from Display
+ virtual void onEnable();
+ virtual void onDisable();
+
+ std::string topic_;
+ Color color_;
+ float alpha_;
+
+ Ogre::SceneNode* scene_node_;
+ ogre_tools::PointCloud* cloud_;
+
+ message_filters::Subscriber<nav_msgs::GridCells> sub_;
+ tf::MessageFilter<nav_msgs::GridCells> tf_filter_;
+ nav_msgs::GridCells::ConstPtr current_message_;
+
+ ColorPropertyWPtr color_property_;
+ ROSTopicStringPropertyWPtr topic_property_;
+ FloatPropertyWPtr alpha_property_;
+};
+
+} // namespace rviz
+
+#endif /* RVIZ_GRID_CELLS_DISPLAY_H */
+
Property changes on: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/grid_cells_display.h
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/visualization/rviz/src/rviz/displays/poly_line_2d_display.h:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
/pkg/branches/josh/rviz_handles/rviz/src/rviz/displays/poly_line_2d_display.h:18490-18807
/pkg/branches/josh/rviz_selection/rviz/src/rviz/displays/poly_line_2d_display.h:12152-13640
/pkg/branches/josh/visualization_scratch/rviz/src/rviz/displays/poly_line_2d_display.h:14041-14428,16191-17859
Copied: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.cpp (from rev 21616, pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp)
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.cpp (rev 0)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.cpp 2009-08-12 18:55:19 UTC (rev 21672)
@@ -0,0 +1,246 @@
+/*
+ * 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.
+ */
+
+#include "path_display.h"
+#include "visualization_manager.h"
+#include "properties/property.h"
+#include "properties/property_manager.h"
+#include "common.h"
+
+#include "ogre_tools/arrow.h"
+
+#include <tf/transform_listener.h>
+
+#include <boost/bind.hpp>
+
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreManualObject.h>
+#include <OGRE/OgreBillboardSet.h>
+
+namespace rviz
+{
+
+PathDisplay::PathDisplay( const std::string& name, VisualizationManager* manager )
+: Display( name, manager )
+, color_( 0.1f, 1.0f, 0.0f )
+, tf_filter_(*manager->getTFClient(), "", 10, update_nh_)
+{
+ scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+
+ static int count = 0;
+ std::stringstream ss;
+ ss << "Path" << count++;
+ manual_object_ = scene_manager_->createManualObject( ss.str() );
+ manual_object_->setDynamic( true );
+ scene_node_->attachObject( manual_object_ );
+
+ setAlpha( 1.0f );
+
+ tf_filter_.connectInput(sub_);
+ tf_filter_.registerCallback(boost::bind(&PathDisplay::incomingMessage, this, _1));
+}
+
+PathDisplay::~PathDisplay()
+{
+ unsubscribe();
+ clear();
+
+ scene_manager_->destroyManualObject( manual_object_ );
+ scene_manager_->destroySceneNode(scene_node_->getName());
+}
+
+void PathDisplay::clear()
+{
+ manual_object_->clear();
+}
+
+void PathDisplay::setTopic( const std::string& topic )
+{
+ unsubscribe();
+
+ topic_ = topic;
+
+ subscribe();
+
+ propertyChanged(topic_property_);
+
+ causeRender();
+}
+
+void PathDisplay::setColor( const Color& color )
+{
+ color_ = color;
+
+ propertyChanged(color_property_);
+
+ processMessage(current_message_);
+ causeRender();
+}
+
+void PathDisplay::setAlpha( float alpha )
+{
+ alpha_ = alpha;
+
+ propertyChanged(alpha_property_);
+
+ processMessage(current_message_);
+ causeRender();
+}
+
+void PathDisplay::subscribe()
+{
+ if ( !isEnabled() )
+ {
+ return;
+ }
+
+ sub_.subscribe(update_nh_, topic_, 10);
+}
+
+void PathDisplay::unsubscribe()
+{
+ sub_.unsubscribe();
+}
+
+void PathDisplay::onEnable()
+{
+ scene_node_->setVisible( true );
+ subscribe();
+}
+
+void PathDisplay::onDisable()
+{
+ unsubscribe();
+ clear();
+ scene_node_->setVisible( false );
+}
+
+void PathDisplay::fixedFrameChanged()
+{
+ clear();
+
+ tf_filter_.setTargetFrame( fixed_frame_ );
+}
+
+void PathDisplay::update(float wall_dt, float ros_dt)
+{
+}
+
+void PathDisplay::processMessage(const nav_msgs::Path::ConstPtr& msg)
+{
+ if (!msg)
+ {
+ return;
+ }
+
+ clear();
+
+ std::string frame_id = msg->header.frame_id;
+ if (frame_id.empty())
+ {
+ frame_id = "/map";
+ }
+
+ tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0.0f, 0.0f, 0.0f ), btVector3( 0.0f, 0.0f, 0.0f ) ),
+ ros::Time(), frame_id);
+
+ if (vis_manager_->getTFClient()->canTransform(fixed_frame_, frame_id, ros::Time()))
+ {
+ try
+ {
+ vis_manager_->getTFClient()->transformPose( fixed_frame_, pose, pose );
+ }
+ catch(tf::TransformException& e)
+ {
+ ROS_ERROR( "Error transforming from frame 'map' to frame '%s'", 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 );
+
+ scene_node_->setPosition( position );
+ scene_node_->setOrientation( orientation );
+
+ manual_object_->clear();
+
+ Ogre::ColourValue color( color_.r_, color_.g_, color_.b_, alpha_ );;
+
+ uint32_t num_points = msg->poses.size();
+ manual_object_->estimateVertexCount( num_points );
+ manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
+ for( uint32_t i=0; i < num_points; ++i)
+ {
+ Ogre::Vector3 pos(msg->poses[i].pose.position.x, msg->poses[i].pose.position.y, msg->poses[i].pose.position.z);
+ robotToOgre(pos);
+ manual_object_->position(pos);
+ manual_object_->colour( color );
+ }
+
+ manual_object_->end();
+}
+
+void PathDisplay::incomingMessage(const nav_msgs::Path::ConstPtr& msg)
+{
+ processMessage(msg);
+}
+
+void PathDisplay::reset()
+{
+ clear();
+}
+
+void PathDisplay::createProperties()
+{
+ color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &PathDisplay::getColor, this ),
+ boost::bind( &PathDisplay::setColor, this, _1 ), category_, this );
+ alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &PathDisplay::getAlpha, this ),
+ boost::bind( &PathDisplay::setAlpha, this, _1 ), category_, this );
+
+ topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &PathDisplay::getTopic, this ),
+ boost::bind( &PathDisplay::setTopic, this, _1 ), category_, this );
+ ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
+ topic_prop->setMessageType(nav_msgs::Path::__s_getDataType());
+}
+
+const char* PathDisplay::getDescription()
+{
+ return "Displays data from a nav_msgs::Path message as lines.";
+}
+
+} // namespace rviz
+
Property changes on: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
/pkg/branches/josh/rviz_handles/rviz/src/rviz/displays/poly_line_2d_display.cpp:18490-18807
/pkg/branches/josh/rviz_selection/rviz/src/rviz/displays/poly_line_2d_display.cpp:12152-13640
/pkg/branches/josh/visualization_scratch/rviz/src/rviz/displays/poly_line_2d_display.cpp:14041-14428,16191-17859
Copied: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.h (from rev 21616, pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.h)
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.h (rev 0)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.h 2009-08-12 18:55:19 UTC (rev 21672)
@@ -0,0 +1,119 @@
+/*
+ * 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.
+ */
+
+
+#ifndef RVIZ_PATH_DISPLAY_H
+#define RVIZ_PATH_DISPLAY_H
+
+#include "display.h"
+#include "helpers/color.h"
+#include "properties/forwards.h"
+
+#include <nav_msgs/Path.h>
+
+#include <message_filters/subscriber.h>
+#include <tf/message_filter.h>
+
+#include <boost/shared_ptr.hpp>
+
+namespace ogre_tools
+{
+class PointCloud;
+}
+
+namespace Ogre
+{
+class SceneNode;
+class ManualObject;
+}
+
+namespace rviz
+{
+
+/**
+ * \class PathDisplay
+ * \brief Displays a nav_msgs::Path message
+ */
+class PathDisplay : public Display
+{
+public:
+ PathDisplay( const std::string& name, VisualizationManager* manager );
+ virtual ~PathDisplay();
+
+ void setTopic( const std::string& topic );
+ const std::string& getTopic() { return topic_; }
+
+ void setColor( const Color& color );
+ const Color& getColor() { return color_; }
+
+ void setAlpha( float alpha );
+ float getAlpha() { return alpha_; }
+
+ // Overrides from Display
+ virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged();
+ virtual void createProperties();
+ virtual void update(float wall_dt, float ros_dt);
+ virtual void reset();
+
+ static const char* getTypeStatic() { return "Path"; }
+ virtual const char* getType() const { return getTypeStatic(); }
+ static const char* getDescription();
+
+protected:
+ void subscribe();
+ void unsubscribe();
+ void clear();
+ void incomingMessage(const nav_msgs::Path::ConstPtr& msg);
+ void processMessage(const nav_msgs::Path::ConstPtr& msg);
+
+ // overrides from Display
+ virtual void onEnable();
+ virtual void onDisable();
+
+ std::string topic_;
+ Color color_;
+ float alpha_;
+
+ Ogre::SceneNode* scene_node_;
+ Ogre::ManualObject* manual_object_;
+
+ message_filters::Subscriber<nav_msgs::Path> sub_;
+ tf::MessageFilter<nav_msgs::Path> tf_filter_;
+ nav_msgs::Path::ConstPtr current_message_;
+
+ ColorPropertyWPtr color_property_;
+ ROSTopicStringPropertyWPtr topic_property_;
+ FloatPropertyWPtr alpha_property_;
+};
+
+} // namespace rviz
+
+#endif /* RVIZ_PATH_DISPLAY_H */
+
Property changes on: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/path_display.h
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/visualization/rviz/src/rviz/displays/poly_line_2d_display.h:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
/pkg/branches/josh/rviz_handles/rviz/src/rviz/displays/poly_line_2d_display.h:18490-18807
/pkg/branches/josh/rviz_selection/rviz/src/rviz/displays/poly_line_2d_display.h:12152-13640
/pkg/branches/josh/visualization_scratch/rviz/src/rviz/displays/poly_line_2d_display.h:14041-14428,16191-17859
Deleted: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-08-12 18:42:45 UTC (rev 21671)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-08-12 18:55:19 UTC (rev 21672)
@@ -1,375 +0,0 @@
-/*
- * 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.
- */
-
-#include "poly_line_2d_display.h"
-#include "visualization_manager.h"
-#include "properties/property.h"
-#include "properties/property_manager.h"
-#include "common.h"
-
-#include "ogre_tools/arrow.h"
-
-#include <tf/transform_listener.h>
-
-#include <boost/bind.hpp>
-
-#include <OGRE/OgreSceneNode.h>
-#include <OGRE/OgreSceneManager.h>
-#include <OGRE/OgreManualObject.h>
-#include <OGRE/OgreBillboardSet.h>
-
-#include <ogre_tools/point_cloud.h>
-
-namespace rviz
-{
-
-PolyLine2DDisplay::PolyLine2DDisplay( const std::string& name, VisualizationManager* manager )
-: Display( name, manager )
-, color_( 0.1f, 1.0f, 0.0f )
-, render_operation_( poly_line_render_ops::Lines )
-, loop_( false )
-, override_color_( false )
-, tf_filter_(*manager->getTFClient(), "", 10, update_nh_)
-{
- scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
-
- static int count = 0;
- std::stringstream ss;
- ss << "PolyLine" << count++;
- manual_object_ = scene_manager_->createManualObject( ss.str() );
- manual_object_->setDynamic( true );
- scene_node_->attachObject( manual_object_ );
-
- cloud_ = new ogre_tools::PointCloud();
- cloud_->setRenderMode( ogre_tools::PointCloud::RM_BILLBOARDS_COMMON_FACING );
- cloud_->setCommonDirection( Ogre::Vector3::UNIT_Y );
- cloud_->setCommonUpVector( Ogre::Vector3::NEGATIVE_UNIT_Z );
- scene_node_->attachObject(cloud_);
- setAlpha( 1.0f );
- setPointSize( 0.05f );
- setZPosition( 0.0f );
-
- tf_filter_.connectInput(sub_);
- tf_filter_.registerCallback(boost::bind(&PolyLine2DDisplay::incomingMessage, this, _1));
-}
-
-PolyLine2DDisplay::~PolyLine2DDisplay()
-{
- unsubscribe();
- clear();
-
- scene_manager_->destroyManualObject( manual_object_ );
- scene_manager_->destroySceneNode(scene_node_->getName());
- delete cloud_;
-}
-
-void PolyLine2DDisplay::clear()
-{
- manual_object_->clear();
- cloud_->clear();
-}
-
-void PolyLine2DDisplay::setTopic( const std::string& topic )
-{
- unsubscribe();
-
- topic_ = topic;
-
- subscribe();
-
- propertyChanged(topic_property_);
-
- causeRender();
-}
-
-void PolyLine2DDisplay::setColor( const Color& color )
-{
- color_ = color;
-
- propertyChanged(color_property_);
-
- processMessage(current_message_);
- causeRender();
-}
-
-void PolyLine2DDisplay::setOverrideColor( bool override )
-{
- override_color_ = override;
-
- propertyChanged(override_color_property_);
-
- processMessage(current_message_);
- causeRender();
-}
-
-void PolyLine2DDisplay::setRenderOperation( int op )
-{
- render_operation_ = op;
-
- propertyChanged(render_operation_property_);
-
- processMessage(current_message_);
- causeRender();
-}
-
-void PolyLine2DDisplay::setLoop( bool loop )
-{
- loop_ = loop;
-
- propertyChanged(loop_property_);
-
- processMessage(current_message_);
- causeRender();
-}
-
-void PolyLine2DDisplay::setPointSize( float size )
-{
- point_size_ = size;
-
- propertyChanged(point_size_property_);
-
- cloud_->setDimensions( size, size, size );
- causeRender();
-}
-
-void PolyLine2DDisplay::setZPosition( float z )
-{
- z_position_ = z;
-
- propertyChanged(z_position_property_);
-
- scene_node_->setPosition( 0.0f, z, 0.0f );
- causeRender();
-}
-
-void PolyLine2DDisplay::setAlpha( float alpha )
-{
- alpha_ = alpha;
-
- cloud_->setAlpha( alpha );
-
- propertyChanged(alpha_property_);
-
- processMessage(current_message_);
- causeRender();
-}
-
-void PolyLine2DDisplay::subscribe()
-{
- if ( !isEnabled() )
- {
- return;
- }
-
- sub_.subscribe(update_nh_, topic_, 10);
- metadata_sub_ = update_nh_.subscribe("map_metadata", 1, &PolyLine2DDisplay::incomingMetadataMessage, this);
-}
-
-void PolyLine2DDisplay::unsubscribe()
-{
- sub_.unsubscribe();
- metadata_sub_.shutdown();
-}
-
-void PolyLine2DDisplay::onEnable()
-{
- scene_node_->setVisible( true );
- subscribe();
-}
-
-void PolyLine2DDisplay::onDisable()
-{
- unsubscribe();
- clear();
- scene_node_->setVisible( false );
-}
-
-void PolyLine2DDisplay::fixedFrameChanged()
-{
- clear();
-
- tf_filter_.setTargetFrame( fixed_frame_ );
-}
-
-void PolyLine2DDisplay::update(float wall_dt, float ros_dt)
-{
-}
-
-void PolyLine2DDisplay::processMessage(const visualization_msgs::Polyline::ConstPtr& msg)
-{
- if (!msg)
- {
- return;
- }
-
- clear();
-
- std::string frame_id = msg->header.frame_id;
- if (frame_id.empty())
- {
- frame_id = "/map";
- }
-
- tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0.0f, 0.0f, 0.0f ), btVector3( 0.0f, 0.0f, z_position_ ) ),
- ros::Time(), frame_id);
-
- if (vis_manager_->getTFClient()->canTransform(fixed_frame_, frame_id, ros::Time()))
- {
- try
- {
- vis_manager_->getTFClient()->transformPose( fixed_frame_, pose, pose );
- }
- catch(tf::TransformException& e)
- {
- ROS_ERROR( "Error transforming from frame 'map' to frame '%s'", 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 );
-
- scene_node_->setPosition( position );
- scene_node_->setOrientation( orientation );
-
- manual_object_->clear();
-
- Ogre::ColourValue color;
- if ( override_color_ )
- {
- color = Ogre::ColourValue( color_.r_, color_.g_, color_.b_, alpha_ );
- }
- else
- {
- color = Ogre::ColourValue( msg->color.r, msg->color.g, msg->color.b, alpha_ );
- }
-
- uint32_t num_points = msg->get_points_size();
- if ( render_operation_ == poly_line_render_ops::Points )
- {
- typedef std::vector< ogre_tools::PointCloud::Point > V_Point;
- V_Point points;
- points.resize( num_points );
- for(uint32_t i = 0; i < num_points; i++)
- {
- ogre_tools::PointCloud::Point& current_point = points[ i ];
-
- current_point.x = -msg->points[i].y;
- current_point.y = -msg->points[i].z; ///\todo Josh please check this orientation --Tully
- current_point.z = -msg->points[i].x;
- current_point.setColor(color.r, color.g, color.b);
- }
-
- cloud_->clear();
-
- if ( !points.empty() )
- {
- cloud_->addPoints( &points.front(), points.size() );
- }
- }
- else
- {
- manual_object_->estimateVertexCount( num_points );
- manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
- for( uint32_t i=0; i < num_points; ++i)
- {
- manual_object_->position(-msg->points[i].y, 0.0f, -msg->points[i].x);
- manual_object_->colour( color );
- }
-
- if ( loop_ && num_points > 0 )
- {
- manual_object_->position(-msg->points[num_points - 1].y, 0.0f, -msg->points[num_points - 1].x);
- manual_object_->colour( color );
- manual_object_->position(-msg->points[0].y, 0.0f, -msg->points[0].x);
- manual_object_->colour( color );
- }
-
- manual_object_->end();
- }
-}
-
-void PolyLine2DDisplay::incomingMessage(const visualization_msgs::Polyline::ConstPtr& msg)
-{
- processMessage(msg);
-}
-
-void PolyLine2DDisplay::incomingMetadataMessage(const nav_msgs::MapMetaData::ConstPtr& msg)
-{
- setPointSize( msg->resolution );
-}
-
-
-void PolyLine2DDisplay::reset()
-{
- clear();
-}
-
-void PolyLine2DDisplay::createProperties()
-{
- override_color_property_ = property_manager_->createProperty<BoolProperty>( "Override Color", property_prefix_, boost::bind( &PolyLine2DDisplay::getOverrideColor, this ),
- boost::bind( &PolyLine2DDisplay::setOverrideColor, this, _1 ), category_, this );
- color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &PolyLine2DDisplay::getColor, this ),
- boost::bind( &PolyLine2DDisplay::setColor, this, _1 ), category_, this );
-
- loop_property_ = property_manager_->createProperty<BoolProperty>( "Loop", property_prefix_, boost::bind( &PolyLine2DDisplay::getLoop, this ),
- boost::bind( &PolyLine2DDisplay::setLoop, this, _1 ), category_, this );
-
- render_operation_property_ = property_manager_->createProperty<EnumProperty>( "Render Operation", property_prefix_, boost::bind( &PolyLine2DDisplay::getRenderOperation, this ),
- boost::bind( &PolyLine2DDisplay::setRenderOperation, this, _1 ), category_, this );
- EnumPropertyPtr enum_prop = render_operation_property_.lock();
- enum_prop->addOption( "Lines", poly_line_render_ops::Lines );
- enum_prop->addOption( "Points", poly_line_render_ops::Points );
-
- /*point_size_property_ = property_manager_->createProperty<FloatProperty>( "Point Size", property_prefix_, boost::bind( &PolyLine2DDisplay::getPointSize, this ),
- boost::bind( &PolyLine...
[truncated message content] |
