@@ -69,9 +69,7 @@ void LaserScanDisplay::onInitialize()
69
69
70
70
void LaserScanDisplay::processMessage ( const sensor_msgs::LaserScanConstPtr& scan )
71
71
{
72
- sensor_msgs::PointCloudPtr cloud ( new sensor_msgs::PointCloud );
73
-
74
- std::string frame_id = scan->header .frame_id ;
72
+ sensor_msgs::PointCloud2Ptr cloud ( new sensor_msgs::PointCloud2 );
75
73
76
74
// Compute tolerance necessary for this scan
77
75
ros::Duration tolerance (scan->time_increment * scan->ranges .size ());
@@ -83,23 +81,14 @@ void LaserScanDisplay::processMessage( const sensor_msgs::LaserScanConstPtr& sca
83
81
84
82
try
85
83
{
86
- // TODO(wjwwood): remove this and use tf2 interface instead
87
- #ifndef _WIN32
88
- # pragma GCC diagnostic push
89
- # pragma GCC diagnostic ignored "-Wdeprecated-declarations"
90
- #endif
91
-
92
- auto tf_client = context_->getTFClient ();
93
-
94
- #ifndef _WIN32
95
- # pragma GCC diagnostic pop
96
- #endif
97
- projector_->transformLaserScanToPointCloud ( fixed_frame_.toStdString (), *scan, *cloud, *tf_client,
84
+ auto tf = context_->getTF2BufferPtr ();
85
+
86
+ projector_->transformLaserScanToPointCloud ( fixed_frame_.toStdString (), *scan, *cloud, *tf,
98
87
laser_geometry::channel_option::Intensity );
99
88
}
100
- catch (tf ::TransformException& e)
89
+ catch (tf2 ::TransformException& e)
101
90
{
102
- ROS_DEBUG ( " LaserScan [%s]: failed to transform scan: %s. This message should not repeat (tolerance should now be set on our tf ::MessageFilter)." ,
91
+ ROS_DEBUG ( " LaserScan [%s]: failed to transform scan: %s. This message should not repeat (tolerance should now be set on our tf2 ::MessageFilter)." ,
103
92
qPrintable ( getName () ), e.what () );
104
93
return ;
105
94
}
0 commit comments