Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[12667] pkg/trunk
|
From: <jfa...@us...> - 2009-03-19 00:46:31
|
Revision: 12667
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12667&view=rev
Author: jfaustwg
Date: 2009-03-19 00:46:16 +0000 (Thu, 19 Mar 2009)
Log Message:
-----------
* Add a Header to the Polyline2D message.
* Fix point cloud and laser scan displays to clear themselves on fixed frame change
* Clear all the displays before deleting the render panel to fix a crash on exit
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
pkg/trunk/visualization/rviz/src/rviz/displays/laser_scan_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/point_cloud_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
pkg/trunk/visualization/rviz/src/rviz/visualization_frame.cpp
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -1,13 +1,13 @@
/*********************************************************************
* Software License Agreement (BSD License)
- *
+ *
* 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
@@ -17,7 +17,7 @@
* * Neither the name of the Willow Garage 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
@@ -101,7 +101,7 @@
* - @b "~costmap_2d/zLB" : an upper bound on the height of observations used for free space projection
* - @b "~costmap_2d/zUB" : a upper bound on the height of observations used for free space projection
* - @b "~costmap_2d/raytrace_window" : the size of the window in which free space will be cleared
- * - @b "~costmap_2d/raytrace_range" : the range after which points will be considered
+ * - @b "~costmap_2d/raytrace_range" : the range after which points will be considered
* - @b "~costmap_2d/obstacle_range" : obstacles outside this range will not be included in the cost map
* - @b "~trajectory_rollout/map_size" : the size of the window around the robot for evaluating local trajectories
* - @b "~trajectory_rollout/yaw_goal_tolerance" : control parameter for trajectory selection
@@ -173,7 +173,7 @@
ros::Node::instance()->param("/global_frame_id", global_frame_, std::string("/map"));
ros::Node::instance()->param("~costmap_2d/base_laser_max_range", baseLaserMaxRange_, baseLaserMaxRange_);
ros::Node::instance()->param("~costmap_2d/tilt_laser_max_range", tiltLaserMaxRange_, tiltLaserMaxRange_);
-
+
// Unsigned chars cannot be stored in parameter server
int tmpLethalObstacleThreshold;
ros::Node::instance()->param("~costmap_2d/lethal_obstacle_threshold", tmpLethalObstacleThreshold, int(lethalObstacleThreshold));
@@ -204,7 +204,7 @@
robotWidth_ = inscribedRadius * 2;
xy_goal_tolerance_ = robotWidth_ / 2;
- // Obtain parameters for sensors and allocate observation buffers accordingly. Rates are in Hz.
+ // Obtain parameters for sensors and allocate observation buffers accordingly. Rates are in Hz.
double base_laser_update_rate(2.0);
double tilt_laser_update_rate(2.0);
double low_obstacle_update_rate(0.2);
@@ -229,7 +229,7 @@
ros::Node::instance()->param("~costmap_2d/body_part_scale", bodypartScale, bodypartScale);
ros::Node::instance()->param("~costmap_2d/robot_name", robotName, robotName);
ros::Node::instance()->param("~costmap_2d/filter_robot_points", useFilter, useFilter);
-
+
if (useFilter) {
filter_ = new robot_filter::RobotFilter((ros::Node*)this, robotName, true, bodypartScale);
filter_->loadRobotDescription();
@@ -239,24 +239,24 @@
}
// Then allocate observation buffers
- baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
- ros::Duration().fromSec(base_laser_keepalive),
+ baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
+ ros::Duration().fromSec(base_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- baseCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
- ros::Duration().fromSec(base_laser_keepalive),
+ baseCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
+ ros::Duration().fromSec(base_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
- ros::Duration().fromSec(tilt_laser_keepalive),
+ tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
+ ros::Duration().fromSec(tilt_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
- ros::Duration().fromSec(low_obstacle_keepalive),
+ lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
+ ros::Duration().fromSec(low_obstacle_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
inscribedRadius, -10.0, maxZ_, filter_);
- stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
- ros::Duration().fromSec(stereo_keepalive),
+ stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
+ ros::Duration().fromSec(stereo_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
@@ -291,9 +291,9 @@
ros::Node::instance()->param("~costmap_2d/obstacle_range", obstacleRange, 10.0);
costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
- inputData , resp.map.resolution,
+ inputData , resp.map.resolution,
lethalObstacleThreshold, maxZ_, zLB, zUB,
- inflationRadius, circumscribedRadius, inscribedRadius, weight,
+ inflationRadius, circumscribedRadius, inscribedRadius, weight,
obstacleRange, rayTraceRange, raytraceWindow);
// set up costmap service response
@@ -339,7 +339,7 @@
pt.y = 0;
footprint_.push_back(pt);
- controller_ = new trajectory_rollout::TrajectoryControllerROS(*ros::Node::instance(), tf_, global_frame_, *local_map_accessor_,
+ controller_ = new trajectory_rollout::TrajectoryControllerROS(*ros::Node::instance(), tf_, global_frame_, *local_map_accessor_,
footprint_, inscribedRadius, circumscribedRadius);
// Advertize messages to publish cost map updates
@@ -370,13 +370,13 @@
// The cost map is populated with either laser scans in the case that we are unable to use a
// world model source, or point clouds if we are. We shall pick one, and will be dominated by
// point clouds
- baseScanNotifier_ = new tf::MessageNotifier<laser_scan::LaserScan>(&tf_, ros::Node::instance(),
- boost::bind(&MoveBase::baseScanCallback, this, _1),
- "base_scan", global_frame_, 50);
- baseCloudNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(&tf_, ros::Node::instance(),
- boost::bind(&MoveBase::baseCloudCallback, this, _1),
- "base_scan_filtered", global_frame_, 50);
- tiltLaserNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(&tf_, ros::Node::instance(),
+ baseScanNotifier_ = new tf::MessageNotifier<laser_scan::LaserScan>(&tf_, ros::Node::instance(),
+ boost::bind(&MoveBase::baseScanCallback, this, _1),
+ "base_scan", global_frame_, 50);
+ baseCloudNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(&tf_, ros::Node::instance(),
+ boost::bind(&MoveBase::baseCloudCallback, this, _1),
+ "base_scan_filtered", global_frame_, 50);
+ tiltLaserNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(&tf_, ros::Node::instance(),
boost::bind(&MoveBase::tiltCloudCallback, this, _1),
"tilt_laser_cloud_filtered", global_frame_, 50);
ros::Node::instance()->subscribe("dcam/cloud", stereoCloudMsg_, &MoveBase::stereoCloudCallback, this, 1);
@@ -403,7 +403,7 @@
if(local_map_accessor_ != NULL)
delete local_map_accessor_;
-
+
if(global_map_accessor_ != NULL)
delete global_map_accessor_;
@@ -421,7 +421,7 @@
delete tiltLaserNotifier_;
}
- void MoveBase::updateGlobalPose(){
+ void MoveBase::updateGlobalPose(){
tf::Stamped<tf::Pose> robotPose;
robotPose.setIdentity();
robotPose.frame_id_ = "base_link";
@@ -494,7 +494,7 @@
// Get the current robot pose in the map frame
updateGlobalPose();
- // Assign state data
+ // Assign state data
stateMsg.pos.x = global_pose_.getOrigin().x();
stateMsg.pos.y = global_pose_.getOrigin().y();
double uselessPitch, uselessRoll, yaw;
@@ -572,7 +572,7 @@
try
{
tf::Stamped<btVector3> v_in(btVector3(odomMsg_.vel.x, odomMsg_.vel.y, 0), ros::Time(), odomMsg_.header.frame_id), v_out;
- tf_.transformVector("base_link", ros::Time(), v_in, odomMsg_.header.frame_id, v_out);
+ tf_.transformVector("base_link", ros::Time(), v_in, odomMsg_.header.frame_id, v_out);
base_odom_.vel.x = v_in.x();
base_odom_.vel.y = v_in.y();
base_odom_.vel.th = odomMsg_.vel.th;
@@ -647,6 +647,7 @@
void MoveBase::publishFootprint(double x, double y, double th){
std::vector<deprecated_msgs::Point2DFloat32> footprint = controller_->drawFootprint(x, y, th);
robot_msgs::Polyline2D footprint_msg;
+ footprint_msg.header.frame_id = global_frame_;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
footprint_msg.color.g = 0;
@@ -661,6 +662,7 @@
void MoveBase::publishPath(bool isGlobal, const std::list<deprecated_msgs::Pose2DFloat32>& path) {
robot_msgs::Polyline2D guiPathMsg;
+ guiPathMsg.header.frame_id = global_frame_;
guiPathMsg.set_points_size(path.size());
unsigned int i = 0;
@@ -699,7 +701,7 @@
// and we have stopped the robot, then we are done
double uselessPitch, uselessRoll, yaw;
global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
- if(plan_.empty() &&
+ if(plan_.empty() &&
fabs(angles::shortest_angular_distance(yaw , stateMsg.goal.th)) < this->yaw_goal_tolerance_){ /// @todo: this is still wrong, should use bt or similar to check shortest angular distance of roll/pitch/yaw.
ROS_DEBUG("Goal achieved at: (%f, %f, %f) for (%f, %f, %f)\n",
@@ -736,7 +738,7 @@
bool MoveBase::checkWatchDog() const {
bool ok = baseScanBuffer_->isCurrent() && tiltScanBuffer_->isCurrent() && stereoCloudBuffer_->isCurrent() && lowObstacleBuffer_->isCurrent();
- if(!ok)
+ if(!ok)
ROS_INFO("Missed required cost map update. Should not allow commanding now. Check cost map data source.\n");
return ok;
@@ -747,14 +749,14 @@
* at least, this leads to poor performance since planning ks slow (seconds) and thus the robot stops alot. If we leave the
* velocity controller flexibility to work around the path in collision then that seems to work better. Note that this is still
* sensitive to the goal (exit point of the path in the window) being in collision which would require an alternate metric
- * to allow more flexibility to get near the goal - essentially treating the goal as a waypoint.
+ * to allow more flexibility to get near the goal - essentially treating the goal as a waypoint.
*/
bool MoveBase::dispatchCommands(){
// First criteria is that we have had a sufficiently recent sensor update to trust perception and that we have a valid plan. This latter
- // case is important since we can end up with an active controller that becomes invalid through the planner looking ahead.
+ // case is important since we can end up with an active controller that becomes invalid through the planner looking ahead.
// We want to be able to stop the robot in that case
bool planOk = checkWatchDog() && isValid();
- robot_msgs::PoseDot cmdVel; // Commanded velocities
+ robot_msgs::PoseDot cmdVel; // Commanded velocities
// if we have achieved all our waypoints but have yet to achieve the goal, then we know that we wish to accomplish our desired
// orientation
@@ -903,6 +905,7 @@
// First publish raw obstacles in red
robot_msgs::Polyline2D pointCloudMsg;
+ pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
pointCloudMsg.color.a = 0.0;
@@ -915,8 +918,8 @@
pointCloudMsg.points[i].y = rawObstacles[i].second;
}
- if (!ros::Node::instance()->ok()) {
- return;
+ if (!ros::Node::instance()->ok()) {
+ return;
}
ros::Node::instance()->publish("raw_obstacles", pointCloudMsg);
@@ -933,8 +936,8 @@
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
}
- if (!ros::Node::instance()->ok()) {
- return;
+ if (!ros::Node::instance()->ok()) {
+ return;
}
ros::Node::instance()->publish("inflated_obstacles", pointCloudMsg);
}
@@ -968,6 +971,7 @@
// First publish raw obstacles in red
robot_msgs::Polyline2D pointCloudMsg;
+ pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
pointCloudMsg.color.a = 0.0;
@@ -1014,14 +1018,14 @@
MoveBase::footprint_t const & MoveBase::getFootprint() const{
return footprint_;
}
-
+
void MoveBase::updateCostMap() {
if (reset_cost_map_) {
costMap_->revertToStaticMap(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
}
ROS_DEBUG("Starting cost map update/n");
-
+
lock();
// Aggregate buffered observations across all sources. Must be thread safe
std::vector<costmap_2d::Observation> observations, raytrace_obs;
@@ -1049,7 +1053,7 @@
}
unlock();
-
+
ROS_DEBUG("Applying update with %d observations/n", stored_observations.size());
// Apply to cost map
ros::Time start = ros::Time::now();
@@ -1059,7 +1063,7 @@
costMap_->updateDynamicObstacles(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), stored_observations, &raytrace_obs.front());
double t_diff = (ros::Time::now() - start).toSec();
ROS_DEBUG("Updated map in %f seconds for %d observations/n", t_diff, stored_observations.size());
-
+
// Finally, we must extract the cost data that we have computed and:
// 1. Refresh the local_map_accessor for the controller
// 2. Refresh the global_map accessor for the planner
Modified: pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -38,12 +38,12 @@
using namespace trajectory_rollout;
namespace trajectory_rollout{
- GovernorNode::GovernorNode(std::vector<deprecated_msgs::Point2DFloat32> footprint_spec) :
- ros::Node("governor_node"), map_(MAP_SIZE_X, MAP_SIZE_Y),
- tf_(*this, true, ros::Duration(10)),
+ GovernorNode::GovernorNode(std::vector<deprecated_msgs::Point2DFloat32> footprint_spec) :
+ ros::Node("governor_node"), map_(MAP_SIZE_X, MAP_SIZE_Y),
+ tf_(*this, true, ros::Duration(10)),
ma_(map_, OUTER_RADIUS),
cm_(ma_),
- tc_(cm_, ma_, footprint_spec, ROBOT_SIDE_RADIUS, OUTER_RADIUS, MAX_ACC_X, MAX_ACC_Y, MAX_ACC_THETA,
+ tc_(cm_, ma_, footprint_spec, ROBOT_SIDE_RADIUS, OUTER_RADIUS, MAX_ACC_X, MAX_ACC_Y, MAX_ACC_THETA,
SIM_TIME, SIM_RES, VEL_SAMPLES, VEL_SAMPLES, PDIST_SCALE, GDIST_SCALE, OCCDIST_SCALE),
cycle_time_(0.1)
{
@@ -151,6 +151,7 @@
printf("Robot Vel - vx: %.2f, vy: %.2f, vth: %.2f\n", robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), yaw);
if(path.cost_ >= 0){
+ path_msg.header.frame_id = "/map";
//let's print debug output to the screen
path_msg.set_points_size(path.getPointsSize());
path_msg.color.r = 0;
@@ -163,12 +164,12 @@
for(unsigned int i = 0; i < path.getPointsSize(); ++i){
double pt_x, pt_y, pt_th;
path.getPoint(i, pt_x, pt_y, pt_th);
- path_msg.points[i].x = pt_x;
+ path_msg.points[i].x = pt_x;
path_msg.points[i].y = pt_y;
//so we can draw the footprint on the map
if(i == 0){
- x = pt_x;
+ x = pt_x;
y = pt_y;
th = pt_th;
}
@@ -178,6 +179,7 @@
vector<deprecated_msgs::Point2DFloat32> footprint = tc_.drawFootprint(x, y, th);
//let's also draw the footprint of the robot for the last point on the selected trajectory
+ footprint_msg.header.frame_id = "/map";
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
footprint_msg.color.g = 0;
@@ -238,7 +240,7 @@
pt.x = -1 * ROBOT_FRONT_RADIUS;
pt.y = ROBOT_SIDE_RADIUS;
footprint_spec.push_back(pt);
-
+
GovernorNode gn(footprint_spec);
ros::Time start;
@@ -256,6 +258,6 @@
//printf("Cycle Time: %.2f\n", t_diff);
}
-
+
return(0);
}
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-03-19 00:46:16 UTC (rev 12667)
@@ -260,7 +260,7 @@
wn.doOneCycle();
wn.sleep(curr.tv_sec+curr.tv_usec/1e6);
}
-
+
return(0);
}
@@ -519,6 +519,7 @@
// Draw the points
+ this->pointcloudMsg.header.frame_id = "/map";
this->pointcloudMsg.set_points_size(this->laser_hitpts_len);
this->pointcloudMsg.color.a = 0.0;
this->pointcloudMsg.color.r = 0.0;
@@ -725,6 +726,7 @@
assert(this->plan->path_count);
+ this->polylineMsg.header.frame_id = "/map";
this->polylineMsg.set_points_size(this->plan->path_count);
this->polylineMsg.color.r = 0;
this->polylineMsg.color.g = 1.0;
Modified: pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg 2009-03-19 00:46:16 UTC (rev 12667)
@@ -1,2 +1,3 @@
+Header header
deprecated_msgs/Point2DFloat32[] points
std_msgs/ColorRGBA color
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/laser_scan_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/laser_scan_display.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/laser_scan_display.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -132,6 +132,8 @@
void LaserScanDisplay::fixedFrameChanged()
{
notifier_->setTargetFrame( fixed_frame_ );
+
+ PointCloudBase::fixedFrameChanged();
}
void LaserScanDisplay::createProperties()
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/point_cloud_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/point_cloud_display.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/point_cloud_display.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -119,6 +119,8 @@
void PointCloudDisplay::fixedFrameChanged()
{
notifier_->setTargetFrame( fixed_frame_ );
+
+ PointCloudBase::fixedFrameChanged();
}
void PointCloudDisplay::createProperties()
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -36,6 +36,7 @@
#include <ros/node.h>
#include <tf/transform_listener.h>
+#include <tf/message_notifier.h>
#include <boost/bind.hpp>
@@ -82,6 +83,8 @@
setAlpha( 1.0f );
setPointSize( 0.05f );
setZPosition( 0.0f );
+
+ notifier_ = new tf::MessageNotifier<robot_msgs::Polyline2D>(tf_, ros_node_, boost::bind(&PolyLine2DDisplay::incomingMessage, this, _1), "", "", 10);
}
PolyLine2DDisplay::~PolyLine2DDisplay()
@@ -92,6 +95,7 @@
scene_manager_->destroyManualObject( manual_object_ );
delete cloud_;
+ delete notifier_;
}
void PolyLine2DDisplay::clear()
@@ -218,7 +222,7 @@
if ( !topic_.empty() )
{
- ros_node_->subscribe( topic_, message_, &PolyLine2DDisplay::incomingMessage, this, 1 );
+ notifier_->setTopic(topic_);
}
ros_node_->subscribe( "map_metadata", metadata_message_, &PolyLine2DDisplay::incomingMetadataMessage, this, 1 );
@@ -226,10 +230,7 @@
void PolyLine2DDisplay::unsubscribe()
{
- if ( !topic_.empty() )
- {
- ros_node_->unsubscribe( topic_, &PolyLine2DDisplay::incomingMessage, this );
- }
+ notifier_->setTopic("");
ros_node_->unsubscribe( "map_metadata", &PolyLine2DDisplay::incomingMetadataMessage, this );
}
@@ -250,16 +251,18 @@
void PolyLine2DDisplay::fixedFrameChanged()
{
clear();
+
+ notifier_->setTargetFrame( fixed_frame_ );
}
void PolyLine2DDisplay::update( float dt )
{
if ( new_message_ )
{
+ new_message_ = false;
+
processMessage();
- new_message_ = false;
-
causeRender();
}
@@ -271,14 +274,30 @@
void PolyLine2DDisplay::processMessage()
{
- message_.lock();
+ boost::shared_ptr<robot_msgs::Polyline2D> msg;
+ {
+ boost::mutex::scoped_lock lock(message_mutex_);
+ msg = message_;
+ }
+
clear();
+ if (!msg)
+ {
+ return;
+ }
+
+ 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(), "map" );
+ ros::Time(), frame_id);
- if (tf_->canTransform(fixed_frame_, "map", ros::Time()))
+ if (tf_->canTransform(fixed_frame_, frame_id, ros::Time()))
{
try
{
@@ -312,10 +331,10 @@
}
else
{
- color = Ogre::ColourValue( message_.color.r, message_.color.g, message_.color.b, alpha_ );
+ color = Ogre::ColourValue( msg->color.r, msg->color.g, msg->color.b, alpha_ );
}
- uint32_t num_points = message_.get_points_size();
+ 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;
@@ -325,9 +344,9 @@
{
ogre_tools::PointCloud::Point& current_point = points[ i ];
- current_point.x_ = -message_.points[i].y;
+ current_point.x_ = -msg->points[i].y;
current_point.y_ = 0.0f;
- current_point.z_ = -message_.points[i].x;
+ current_point.z_ = -msg->points[i].x;
current_point.r_ = color.r;
current_point.g_ = color.g;
current_point.b_ = color.b;
@@ -346,27 +365,27 @@
manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for( uint32_t i=0; i < num_points; ++i)
{
- manual_object_->position(-message_.points[i].y, 0.0f, -message_.points[i].x);
+ 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(-message_.points[num_points - 1].y, 0.0f, -message_.points[num_points - 1].x);
+ manual_object_->position(-msg->points[num_points - 1].y, 0.0f, -msg->points[num_points - 1].x);
manual_object_->colour( color );
- manual_object_->position(-message_.points[0].y, 0.0f, -message_.points[0].x);
+ manual_object_->position(-msg->points[0].y, 0.0f, -msg->points[0].x);
manual_object_->colour( color );
}
manual_object_->end();
}
-
- message_.unlock();
}
-void PolyLine2DDisplay::incomingMessage()
+void PolyLine2DDisplay::incomingMessage(const boost::shared_ptr<robot_msgs::Polyline2D>& msg)
{
+ boost::mutex::scoped_lock lock(message_mutex_);
new_message_ = true;
+ message_ = msg;
}
void PolyLine2DDisplay::incomingMetadataMessage()
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h 2009-03-19 00:46:16 UTC (rev 12667)
@@ -37,6 +37,8 @@
#include <robot_msgs/Polyline2D.h>
#include <robot_msgs/MapMetaData.h>
+#include <boost/shared_ptr.hpp>
+
namespace ogre_tools
{
class PointCloud;
@@ -48,6 +50,11 @@
class ManualObject;
}
+namespace tf
+{
+template<class Message> class MessageNotifier;
+}
+
namespace rviz
{
@@ -119,7 +126,7 @@
void subscribe();
void unsubscribe();
void clear();
- void incomingMessage();
+ void incomingMessage(const boost::shared_ptr<robot_msgs::Polyline2D>& msg);
void incomingMetadataMessage();
void processMessage();
@@ -141,7 +148,9 @@
ogre_tools::PointCloud* cloud_;
bool new_message_;
- robot_msgs::Polyline2D message_;
+ boost::shared_ptr<robot_msgs::Polyline2D> message_;
+ boost::mutex message_mutex_;
+ tf::MessageNotifier<robot_msgs::Polyline2D>* notifier_;
bool new_metadata_;
robot_msgs::MapMetaData metadata_message_;
Modified: pkg/trunk/visualization/rviz/src/rviz/visualization_frame.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/visualization_frame.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/visualization_frame.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -93,6 +93,8 @@
{
saveConfigs();
+ manager_->removeAllDisplays();
+
render_panel_->Destroy();
delete manager_;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
