|
From: <tf...@us...> - 2009-06-08 23:58:00
|
Revision: 16843
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16843&view=rev
Author: tfoote
Date: 2009-06-08 23:57:45 +0000 (Mon, 08 Jun 2009)
Log Message:
-----------
moving transform broadcaster to NodeHandle API
Modified Paths:
--------------
pkg/trunk/common/tf/include/tf/transform_broadcaster.h
pkg/trunk/common/tf/src/transform_broadcaster.cpp
pkg/trunk/common/tf/src/transform_sender.cpp
pkg/trunk/common/tf/test/testBroadcaster.cpp
pkg/trunk/common/tf/test/test_notifier.cpp
pkg/trunk/common/tf/tutorials/earth_tracker.cpp
pkg/trunk/common/tf/tutorials/space_shuttle_tracker.cpp
pkg/trunk/deprecated/amcl_player/amcl_player.cc
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/simulators/stage/src/stageros.cpp
pkg/trunk/vision/outlet_detection/src/tracker_base.cpp
pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
pkg/trunk/visualization/rviz/src/test/cloud_test.cpp
pkg/trunk/visualization/rviz/src/test/marker_test.cpp
Modified: pkg/trunk/common/tf/include/tf/transform_broadcaster.h
===================================================================
--- pkg/trunk/common/tf/include/tf/transform_broadcaster.h 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/include/tf/transform_broadcaster.h 2009-06-08 23:57:45 UTC (rev 16843)
@@ -33,10 +33,17 @@
#ifndef TF_TRANSFORMBROADCASTER_H
#define TF_TRANSFORMBROADCASTER_H
-#include "ros/node.h"
#include "tf/tf.h"
#include "tf/tfMessage.h"
+//Forward declaring not working
+#include "ros/ros.h"
+/*namespace ros
+{
+class NodeHandle;
+class Publisher;
+}
+*/
namespace tf
{
@@ -48,7 +55,7 @@
class TransformBroadcaster{
public:
/** \brief Constructor (needs a ros::Node reference) */
- TransformBroadcaster(ros::Node& anode);
+ TransformBroadcaster();
/** \brief Send a Stamped<Transform> with parent parent_id
* The stamped data structure includes frame_id, and time, and parent_id already. */
@@ -59,7 +66,8 @@
private:
/// Internal reference to ros::Node
- ros::Node & node_;
+ ros::NodeHandle node_;
+ ros::Publisher publisher_;
std::string tf_prefix_;
Modified: pkg/trunk/common/tf/src/transform_broadcaster.cpp
===================================================================
--- pkg/trunk/common/tf/src/transform_broadcaster.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/src/transform_broadcaster.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -31,14 +31,14 @@
/** \author Tully Foote */
+#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
namespace tf {
-TransformBroadcaster::TransformBroadcaster(ros::Node& anode):
- node_(anode)
+TransformBroadcaster::TransformBroadcaster()
{
- node_.advertise<tfMessage>("/tf_message", 100);
+ publisher_ = node_.advertise<tfMessage>("/tf_message", 100);
node_.param(std::string("~tf_prefix"),tf_prefix_, std::string(""));
};
@@ -50,7 +50,7 @@
msgtf.header.frame_id = tf::remap(tf_prefix_, msgtf.header.frame_id);
msgtf.parent_id = tf::remap(tf_prefix_, msgtf.parent_id);
message.transforms.push_back(msgtf);
- node_.publish("/tf_message", message);
+ publisher_.publish(message);
}
@@ -64,7 +64,7 @@
msgtf.parent_id = remap(tf_prefix_, parent_id);
TransformTFToMsg(transform, msgtf.transform);
message.transforms.push_back(msgtf);
- node_.publish("/tf_message", message);
+ publisher_.publish(message);
}
Modified: pkg/trunk/common/tf/src/transform_sender.cpp
===================================================================
--- pkg/trunk/common/tf/src/transform_sender.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/src/transform_sender.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -32,10 +32,9 @@
class TransformSender
{
public:
- ros::Node node_;
+ ros::NodeHandle node_;
//constructor
TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string& frame_id, const std::string& parent_id) :
- node_("transform_sender", ros::Node::ANONYMOUS_NAME),broadcaster(node_),
transform_(btTransform(btQuaternion(yaw,pitch,roll), btVector3(x,y,z)), time, frame_id , parent_id){};
//Clean up ros connections
~TransformSender() { }
@@ -59,7 +58,7 @@
int main(int argc, char ** argv)
{
//Initialize ROS
- ros::init(argc, argv);
+ ros::init(argc, argv,"transform_sender", ros::init_options::AnonymousName);
if(argc != 10)
{
Modified: pkg/trunk/common/tf/test/testBroadcaster.cpp
===================================================================
--- pkg/trunk/common/tf/test/testBroadcaster.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/test/testBroadcaster.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -28,12 +28,13 @@
*/
#include "tf/transform_broadcaster.h"
+#include "ros/ros.h"
-class testBroadcaster : public ros::Node
+class testBroadcaster
{
public:
//constructor
- testBroadcaster() : ros::Node("broadcaster"),broadcaster(*this),count(2){};
+ testBroadcaster() : count(2){};
//Clean up ros connections
~testBroadcaster() { }
@@ -68,7 +69,8 @@
//Construct/initialize the server
testBroadcaster myTestBroadcaster;
- while(myTestBroadcaster.ok())
+ ros::NodeHandle node; //\todo replace with ros::ok() after 0.5.2 release
+ while(node.ok())//Check if a Ctrl-C or other shutdown command has been recieved
{
//Send some data
myTestBroadcaster.test();
Modified: pkg/trunk/common/tf/test/test_notifier.cpp
===================================================================
--- pkg/trunk/common/tf/test/test_notifier.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/test/test_notifier.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -567,7 +567,7 @@
g_node->advertise<robot_msgs::PointStamped>("test_message2", 0);
g_tf = new TransformListener(*g_node);
- g_broadcaster = new TransformBroadcaster(*g_node);
+ g_broadcaster = new TransformBroadcaster();
int ret = RUN_ALL_TESTS();
Modified: pkg/trunk/common/tf/tutorials/earth_tracker.cpp
===================================================================
--- pkg/trunk/common/tf/tutorials/earth_tracker.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/tutorials/earth_tracker.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -30,13 +30,14 @@
/**\author Tully Foote/tfoote at willowgarage.com */
#include "tf/transform_broadcaster.h"
+#include "ros/ros.h"
#include <cmath>
class EarthTracker
{
public:
//constructor
- EarthTracker(ros::Node& node) : broadcaster(node),count(2){};
+ EarthTracker(): count(2){};
//Clean up ros connections
~EarthTracker() { }
@@ -64,12 +65,12 @@
int main(int argc, char ** argv)
{
//Initialize ROS
- ros::init(argc, argv);
- ros::Node node("earth_tracker");
+ ros::init(argc, argv, "earth_tracker");
//Construct/initialize the server
- EarthTracker myEarthTracker(node);
+ EarthTracker myEarthTracker;
+ ros::NodeHandle node; //\todo replace with ros::ok() after 0.5.2 release
while(node.ok())//Check if a Ctrl-C or other shutdown command has been recieved
{
//Send the position of the earth with respect to the sun
Modified: pkg/trunk/common/tf/tutorials/space_shuttle_tracker.cpp
===================================================================
--- pkg/trunk/common/tf/tutorials/space_shuttle_tracker.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/tutorials/space_shuttle_tracker.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -36,10 +36,10 @@
{
public:
//constructor
- SpaceShuttleTracker(ros::Node& node) : broadcaster(node),count(2){};
+ SpaceShuttleTracker() : count(2){}
//Clean up ros connections
~SpaceShuttleTracker() { }
-
+
//A pointer to the rosTFServer class
tf::TransformBroadcaster broadcaster;
@@ -83,11 +83,11 @@
int main(int argc, char ** argv)
{
//Initialize ROS
- ros::init(argc, argv);
- ros::Node node("nasa_tracking");
+ ros::init(argc, argv,"nasa_tracking");
+ ros::NodeHandle node;//\todo replace with ros::ok() when 0.5.2 is released
//Construct/initialize the server
- SpaceShuttleTracker mySpaceShuttleTracker(node);
+ SpaceShuttleTracker mySpaceShuttleTracker;
while(node.ok())//Check if a Ctrl-C or other shutdown command has been recieved
{
Modified: pkg/trunk/deprecated/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/deprecated/amcl_player/amcl_player.cc 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/deprecated/amcl_player/amcl_player.cc 2009-06-08 23:57:45 UTC (rev 16843)
@@ -436,7 +436,7 @@
this->setPose(startX, startY, startTH);
cloud_pub_interval.fromSec(1.0);
- this->tf = new tf::TransformBroadcaster(*this);
+ this->tf = new tf::TransformBroadcaster();
this->tfL = new tf::TransformListener(*this);
advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",2);
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-06-08 23:57:45 UTC (rev 16843)
@@ -108,7 +108,7 @@
tf::TransformBroadcaster tf;
ErraticNode() : ros::Node("erratic_player"),
- tf(*this), watts_charging_(10), watts_unplugged_(-10), charging_threshold_(12.98)
+ watts_charging_(10), watts_unplugged_(-10), charging_threshold_(12.98)
{
// libplayercore boiler plate
player_globals_init();
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -77,7 +77,7 @@
advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose", 1);
advertise<robot_msgs::PoseWithRatesStamped>("base_pose_ground_truth", 1) ;
- m_tfServer = new tf::TransformBroadcaster(*this);
+ m_tfServer = new tf::TransformBroadcaster();
subscribe("phase_space_snapshot", snapshot_, &PhaseSpaceLocalization::snapshotCallback, 10) ;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -91,7 +91,7 @@
ros::Node::instance()->advertise<deprecated_msgs::RobotBase2DOdom>("odom", 1);
deprecated_msgs::RobotBase2DOdom odom;
- tf::TransformBroadcaster tfb(*ros::Node::instance());
+ tf::TransformBroadcaster tfb;
ros::Duration d; d.fromSec(0.01);
Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -56,7 +56,6 @@
: ros::Node(node_name),
node_name_(node_name),
robot_state_(*this, true),
- odom_broadcaster_(*this),
vo_notifier_(NULL),
vel_desi_(2),
vel_active_(false),
Modified: pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -66,7 +66,7 @@
string file_name_, cloud_topic_;
double rate_;
- PCDGenerator (ros::Node& anode) : tf_frame_ ("base_link"), node_ (anode), broadcaster_ (anode),
+ PCDGenerator (ros::Node& anode) : tf_frame_ ("base_link"), node_ (anode),
transform_ (btTransform (btQuaternion (0, 0, 0), btVector3 (0, 0, 0)), ros::Time::now (), tf_frame_, tf_frame_)
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
Modified: pkg/trunk/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -396,7 +396,7 @@
(M_PI/12.0) * (M_PI/12.0));
cloud_pub_interval.fromSec(1.0);
- tfb_ = new tf::TransformBroadcaster(*ros::Node::instance());
+ tfb_ = new tf::TransformBroadcaster();
tf_ = new tf::TransformListener(*ros::Node::instance());
map_ = requestMap();
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -93,7 +93,7 @@
{
advertise<robot_msgs::PoseWithCovariance>("amcl_pose",1);
advertise<robot_msgs::ParticleCloud>("particlecloud",1);
- m_tfServer = new tf::TransformBroadcaster(*this);
+ m_tfServer = new tf::TransformBroadcaster();
m_tfListener = new tf::TransformListener(*this);
m_lastUpdate = ros::Time::now();
Modified: pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
===================================================================
--- pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -279,7 +279,7 @@
// Constructor
RosVisualNavigator::RosVisualNavigator (double exit_point_radius, uint scan_period) :
- node_("visual_navigator"), tf_listener_(node_), tf_sender_(node_), map_received_(false),
+ node_("visual_navigator"), tf_listener_(node_), map_received_(false),
odom_received_(false), exit_point_radius_(exit_point_radius), have_goal_(false),
num_active_markers_(0), scan_counter_(1), scan_period_(scan_period)
{
Modified: pkg/trunk/simulators/stage/src/stageros.cpp
===================================================================
--- pkg/trunk/simulators/stage/src/stageros.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/simulators/stage/src/stageros.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -199,8 +199,7 @@
}
StageNode::StageNode(int argc, char** argv, bool gui, const char* fname) :
- ros::Node("stageros"),
- tf(*this)
+ ros::Node("stageros")
{
this->sim_time.fromSec(0.0);
this->base_last_cmd.fromSec(0.0);
Modified: pkg/trunk/vision/outlet_detection/src/tracker_base.cpp
===================================================================
--- pkg/trunk/vision/outlet_detection/src/tracker_base.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/vision/outlet_detection/src/tracker_base.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -15,7 +15,7 @@
TrackerBase::TrackerBase(ros::Node &node, std::string prefix)
: node_(node), img_(res_.image), cam_info_(res_.cam_info),
pose_topic_name_("~" + prefix + "_pose"),
- tf_broadcaster_(node), tf_listener_(node),
+ tf_listener_(node),
object_frame_id_(prefix + "_pose"), K_(NULL),
display_topic_name_("~" + prefix + "_image"),
save_count_(0), save_prefix_(prefix)
Modified: pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -95,7 +95,7 @@
std_msgs::String msg_prefix;
- PublishScene (ros::Node& anode) : tf_frame_ ("base_link"), node_ (anode), broadcaster_ (anode),
+ PublishScene (ros::Node& anode) : tf_frame_ ("base_link"), node_ (anode),
transform_ (btTransform (btQuaternion (0, 0, 0), btVector3 (0, 0, 0)), ros::Time::now (), tf_frame_, tf_frame_)
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
Modified: pkg/trunk/visualization/rviz/src/test/cloud_test.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/test/cloud_test.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/visualization/rviz/src/test/cloud_test.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -20,7 +20,7 @@
node->advertise<robot_msgs::PointCloud>( "intensity_cloud_test", 0 );
node->advertise<robot_msgs::PointCloud>( "million_points_cloud_test", 0 );
- tf::TransformBroadcaster tf_broadcaster(*node);
+ tf::TransformBroadcaster tf_broadcaster;
usleep( 1000000 );
Modified: pkg/trunk/visualization/rviz/src/test/marker_test.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/test/marker_test.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/visualization/rviz/src/test/marker_test.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -20,7 +20,7 @@
node->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
node->advertise<visualization_msgs::MarkerArray>( "visualization_marker_array", 0 );
- tf::TransformBroadcaster tf_broadcaster(*node);
+ tf::TransformBroadcaster tf_broadcaster;
usleep( 1000000 );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|