laser_geometry ros2 example
Hi, I'm trying to get the at the end of the laser_pipeline tutorial page to work in ROS2 Foxy. I've been complementing the code with the tf2 message filter tutorial page as well. My code right now looks like this:
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/message_filter.h"
#ifdef TF2_CPP_HEADERS
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#else
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#endif
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "laser_geometry/laser_geometry.hpp"
#include "message_filters/subscriber.h"
using std::placeholders::_1;
using namespace std::chrono_literals;
class LaserTransformerPublisher : public rclcpp::Node
{
public:
LaserTransformerPublisher()
: Node("laser_tf_pub") {
typedef std::chrono::duration<int> seconds_type;
seconds_type buffer_timeout(1);
// Publisher for pointcloud messages
pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("laser_proj", 10);
// TF2 message buffer
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
this->get_node_base_interface(),
this->get_node_timers_interface()
);
tf_buffer_->setCreateTimerInterface(timer_interface);
transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
laser_sub_.subscribe(this, "/laser/full");
tf_filter_ = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
laser_sub_, *tf_buffer_, target_frame_, 100, this->get_node_logging_interface(),
this->get_node_clock_interface(), buffer_timeout
);
tf_filter_->registerCallback(&LaserTransformerPublisher::on_laser_scan_callback, this);
}
private:
std::string target_frame_;
std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_{nullptr};
message_filters::Subscriber<sensor_msgs::msg::LaserScan> laser_sub_;
std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> tf_filter_;
laser_geometry::LaserProjection projector_;
void on_laser_scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr scan_in) {
sensor_msgs::msg::PointCloud2 cloud;
try {
projector_.transformLaserScanToPointCloud("/robot", *scan_in, cloud, transform_listener_);
} catch (tf2::TransformException & ex) {
RCLCPP_WARN(this->get_logger(), "Failure %s\n", ex.what());
}
}
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<LaserTransformerPublisher>());
rclcpp::shutdown();
return 0;
}
However, I get the following build error:
/home/vizca-nuc/dev_ws/src/vizca/vizca_laser_projector/src/laser_proj.cpp: In member function ‘void LaserTransformerPublisher::on_laser_scan_callback(sensor_msgs::msg::LaserScan_<std::allocator<void> >::SharedPtr)’:
/home/vizca-nuc/dev_ws/src/vizca/vizca_laser_projector/src/laser_proj.cpp:87:82: error: cannot convert ‘std::shared_ptr<tf2_ros::TransformListener>’ to ‘tf2::BufferCore&’
87 | projector_.transformLaserScanToPointCloud("/robot", *scan_in, cloud, transform_listener_);
| ^~~~~~~~~~~~~~~~~~~
| |
| std::shared_ptr<tf2_ros::TransformListener>
In file included from /home/vizca-nuc/dev_ws/src/vizca/vizca_laser_projector/src/laser_proj.cpp:24:
/opt/ros/foxy/include/laser_geometry/laser_geometry.hpp:158:23: note: initializing argument 4 of ‘void laser_geometry::LaserProjection::transformLaserScanToPointCloud(const string&, const LaserScan&, sensor_msgs::msg::PointCloud2&, tf2::BufferCore&, double, int)’
158 | tf2::BufferCore & tf,
| ~~~~~~~~~~~~~~~~~~^~
make[2]: *** [CMakeFiles/laser_proj.dir/build.make:63: CMakeFiles/laser_proj.dir/src/laser_proj.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:78: CMakeFiles/laser_proj.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed <<< vizca_laser_projector [3.87s, exited with code 2]
I'm not really sure why passing transformer_listener_
, a tf2_ros::TransformListener
, to transformLaserScanToPointCloud
fails, given that the function expects a tf2::BufferCore&
variable, which is a parent class that tf2_ros::TransformListener
inherits from. Should I be casting transformer_listener_
to something else?