Skip to content

Commit 0f9e8a4

Browse files
PetervDoorenPetervDooren
committed
Merge branch 'master' into feature/add_logging
2 parents 3ef9f1a + 196b713 commit 0f9e8a4

File tree

8 files changed

+44
-29
lines changed

8 files changed

+44
-29
lines changed

‎ed_sensor_integration/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,9 @@ find_package(catkin REQUIRED COMPONENTS
55
ed
66
ed_sensor_integration_msgs
77
geolib2
8+
geometry_msgs
89
image_geometry
910
sensor_msgs
10-
tf2
1111
tf2_ros
1212
tue_config
1313
tue_filesystem

‎ed_sensor_integration/include/ed/kinect/fitter.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -233,11 +233,11 @@ class Fitter
233233

234234
/**
235235
* @brief checkExpectedBeamThroughEntity checks if the expected center beam passes through the entity. If
236-
* not: something went wrong
236+
* not: throw a fitter error
237237
* @param model_ranges
238238
* @param entity
239239
* @param sensor_pose_xya
240-
* @param expected_center_beam
240+
* @param expected_center_beam expected index of the beam through the center of the object. range: any int. indices outside bounds will also throw an error.
241241
*/
242242
void checkExpectedBeamThroughEntity(const std::vector<double> &model_ranges, ed::EntityConstPtr entity,
243243
const geo::Pose3D &sensor_pose_xya, const int expected_center_beam) const;

‎ed_sensor_integration/include/ed/kinect/image_buffer.h‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,13 @@
22
#define ED_SENSOR_INTEGRATION_IMAGE_BUFFER_H_
33

44
#include <ros/callback_queue.h>
5-
#include <rgbd/types.h>
5+
66
#include <geolib/datatypes.h>
77

8+
#include <rgbd/types.h>
9+
10+
#include <tf2_ros/buffer.h>
11+
812
#include <forward_list>
913
#include <memory>
1014
#include <mutex>
@@ -15,7 +19,7 @@ namespace rgbd
1519
class Client;
1620
}
1721

18-
namespace tf
22+
namespace tf2_ros
1923
{
2024
class TransformListener;
2125
}
@@ -66,7 +70,8 @@ class ImageBuffer
6670

6771
std::unique_ptr<rgbd::Client> rgbd_client_;
6872

69-
std::unique_ptr<tf::TransformListener> tf_listener_;
73+
tf2_ros::Buffer tf_buffer_;
74+
std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
7075

7176
/**
7277
* @brief Newer images should be pushed on the front. This will result in the front being the most recent and the back being the oldest

‎ed_sensor_integration/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,9 @@
1616
<depend>ed</depend>
1717
<depend>ed_sensor_integration_msgs</depend>
1818
<depend>geolib2</depend>
19+
<depend>geometry_msgs</depend>
1920
<depend>image_geometry</depend>
2021
<depend>sensor_msgs</depend>
21-
<depend>tf2</depend>
2222
<depend>tf2_ros</depend>
2323
<depend>tue_config</depend>
2424
<depend>tue_filesystem</depend>

‎ed_sensor_integration/src/kinect/fitter.cpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -377,6 +377,8 @@ std::unique_ptr<OptimalFit> Fitter::findOptimum(const EstimationInputData& input
377377
}
378378
if (valid_optimum)
379379
return current_optimum;
380+
381+
ROS_ERROR_NAMED("fitter", "optimum is invalid");
380382
std::unique_ptr<OptimalFit> invalid_optimum(new OptimalFit);
381383
return invalid_optimum;
382384
}
@@ -462,7 +464,8 @@ void Fitter::checkExpectedBeamThroughEntity(const std::vector<double>& model_ran
462464
expected_ranges = model_ranges;
463465
std::vector<int> expected_identifiers(nr_data_points_, 0);
464466
renderEntity(entity, sensor_pose_xya, 1, expected_ranges, expected_identifiers);
465-
467+
if (expected_center_beam < 0 || expected_center_beam >= nr_data_points_)
468+
throw FitterError("Expected beam outside of measurement range(" + std::to_string(nr_data_points_) + "), index: " + std::to_string(expected_center_beam));
466469
if (expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model
467470
throw FitterError("Expected beam does not go through entity");
468471
}

‎ed_sensor_integration/src/kinect/image_buffer.cpp‎

Lines changed: 20 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,17 @@
11
#include "ed/kinect/image_buffer.h"
22

3+
#include <geolib/ros/msg_conversions.h>
4+
5+
#include <geometry_msgs/TransformStamped.h>
6+
37
#include <rgbd/client.h>
48
#include <rgbd/image.h>
5-
#include <tf/transform_listener.h>
6-
#include <geolib/ros/tf_conversions.h>
9+
10+
#include <tf2_ros/transform_listener.h>
711

812
// ----------------------------------------------------------------------------------------------------
913

10-
ImageBuffer::ImageBuffer() : rgbd_client_(nullptr), tf_listener_(nullptr), shutdown_(false)
14+
ImageBuffer::ImageBuffer() : rgbd_client_(nullptr), tf_buffer_(), tf_listener_(nullptr), shutdown_(false)
1115
{
1216
}
1317

@@ -32,7 +36,7 @@ void ImageBuffer::initialize(const std::string& topic, const std::string& root_f
3236
rgbd_client_->initialize(topic);
3337

3438
if (!tf_listener_)
35-
tf_listener_ = std::make_unique<tf::TransformListener>();
39+
tf_listener_ = std::make_unique<tf2_ros::TransformListener>(tf_buffer_);
3640

3741
worker_thread_ptr_ = std::make_unique<std::thread>(&ImageBuffer::workerThreadFunc, this, worker_thread_frequency);
3842
}
@@ -73,8 +77,8 @@ bool ImageBuffer::waitForRecentImage(rgbd::ImageConstPtr& image, geo::Pose3D& se
7377

7478
// - - - - - - - - - - - - - - - - - -
7579
// Wait until we have a tf
76-
if (!tf_listener_->canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()))) // Get the TF when it is available now
77-
if (!tf_listener_->waitForTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_end - ros::Time::now()))
80+
if (!tf_buffer_.canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()))) // Get the TF when it is available now
81+
if (!tf_buffer_.canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_end - ros::Time::now()))
7882
{
7983
ROS_ERROR_NAMED("image_buffer", "[IMAGE_BUFFER] timeout waiting for tf");
8084
return false;
@@ -85,11 +89,10 @@ bool ImageBuffer::waitForRecentImage(rgbd::ImageConstPtr& image, geo::Pose3D& se
8589

8690
try
8791
{
88-
tf::StampedTransform t_sensor_pose;
89-
tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose);
90-
geo::convert(t_sensor_pose, sensor_pose);
92+
geometry_msgs::TransformStamped t_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()));
93+
geo::convert(t_sensor_pose.transform, sensor_pose);
9194
}
92-
catch(tf::TransformException& ex)
95+
catch(tf2::TransformException& ex)
9396
{
9497
ROS_ERROR_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());
9598
return false;
@@ -155,20 +158,18 @@ bool ImageBuffer::getMostRecentImageTF()
155158
rgbd::ImageConstPtr& rgbd_image = *it;
156159
try
157160
{
158-
tf::StampedTransform t_sensor_pose;
159-
tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose);
160-
geo::convert(t_sensor_pose, sensor_pose);
161+
geometry_msgs::TransformStamped t_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()));
162+
geo::convert(t_sensor_pose.transform, sensor_pose);
161163
}
162-
catch (tf::ExtrapolationException& ex)
164+
catch (tf2::ExtrapolationException& ex)
163165
{
164166
try
165167
{
166168
// Now we have to check if the error was an interpolation or extrapolation error (i.e., the image is too old or
167169
// to new, respectively). If it is too old, discard it.
168-
tf::StampedTransform latest_sensor_pose;
169-
tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(0), latest_sensor_pose);
170+
geometry_msgs::TransformStamped latest_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(0));
170171
// If image time stamp is older than latest transform, the image is too old and the tf data is not available anymore
171-
if ( latest_sensor_pose.stamp_ > ros::Time(rgbd_image->getTimestamp()) )
172+
if ( latest_sensor_pose.header.stamp > ros::Time(rgbd_image->getTimestamp()) )
172173
{
173174

174175
ROS_DEBUG_STREAM_NAMED("image_buffer", "[IMAGE_BUFFER] Image too old to look-up tf. Deleting all images older than timestamp: " << std::fixed
@@ -184,14 +185,14 @@ bool ImageBuffer::getMostRecentImageTF()
184185
continue;
185186
}
186187
}
187-
catch (tf::TransformException& ex)
188+
catch (tf2::TransformException& ex)
188189
{
189190
ROS_ERROR_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what());
190191
ROS_WARN_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what());
191192
continue;
192193
}
193194
}
194-
catch (tf::TransformException& ex)
195+
catch (tf2::TransformException& ex)
195196
{
196197
ROS_ERROR_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());
197198
ROS_WARN_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());

‎ed_sensor_integration/src/laser/laser_plugin.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,14 @@
1212
#include <geolib/ros/msg_conversions.h>
1313
#include <geolib/Shape.h>
1414

15+
#include <geometry_msgs/TransformStamped.h>
16+
1517
#include <opencv2/imgproc/imgproc.hpp>
1618

1719
#include <ros/console.h>
1820
#include <ros/node_handle.h>
1921

20-
#include "tf2/transform_datatypes.h"
22+
#include <tf2_ros/transform_listener.h>
2123

2224
#include <iostream>
2325

‎ed_sensor_integration/src/laser/laser_plugin.h‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,13 +11,17 @@
1111
#include <ros/subscriber.h>
1212
#include <ros/callback_queue.h>
1313
#include <tf2_ros/buffer.h>
14-
#include <tf2_ros/transform_listener.h>
1514
#include <sensor_msgs/LaserScan.h>
1615

1716
#include <queue>
1817
#include <map>
1918
#include <memory>
2019

20+
namespace tf2_ros
21+
{
22+
class TransformListener;
23+
}
24+
2125
class LaserPlugin : public ed::Plugin
2226
{
2327

0 commit comments

Comments
 (0)