Skip to content

Commit db1a7e4

Browse files
Simon Schmeisserrhaschke
Simon Schmeisser
authored andcommitted
tf2: port LaserScanDisplay
1 parent 92b44c1 commit db1a7e4

File tree

1 file changed

+6
-17
lines changed

1 file changed

+6
-17
lines changed

src/rviz/default_plugin/laser_scan_display.cpp

+6-17
Original file line numberDiff line numberDiff line change
@@ -69,9 +69,7 @@ void LaserScanDisplay::onInitialize()
6969

7070
void LaserScanDisplay::processMessage( const sensor_msgs::LaserScanConstPtr& scan )
7171
{
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 );
7573

7674
// Compute tolerance necessary for this scan
7775
ros::Duration tolerance(scan->time_increment * scan->ranges.size());
@@ -83,23 +81,14 @@ void LaserScanDisplay::processMessage( const sensor_msgs::LaserScanConstPtr& sca
8381

8482
try
8583
{
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,
9887
laser_geometry::channel_option::Intensity );
9988
}
100-
catch (tf::TransformException& e)
89+
catch (tf2::TransformException& e)
10190
{
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).",
10392
qPrintable( getName() ), e.what() );
10493
return;
10594
}

0 commit comments

Comments
 (0)