[ROS2]Are there any examples of tf2 broadcast or listener
Hello comrades, I`m just trying to get odometry from t265 from realsense2_lib examples. After compiling and running this code below, I get that target frame does not exist.
Thanks in advance.
#include <math.h>
#include <iostream>
#include <chrono>
#include "std_msgs/msg/string.hpp"
#include "rclcpp/rclcpp.hpp"
#include <rclcpp/time_source.hpp>
#include "geometry_msgs/msg/pose_stamped.h"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_ros/buffer_interface.h>
#include <tf2_ros/visibility_control.h>
#include <tf2/buffer_core.h>
#include <tf2/time.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/transform_stamped.h>
/* We do not recommend this style anymore, because composition of multiple
* nodes in the same executable is not possible. Please see one of the subclass
* examples for the "new" recommended styles. This example is only included
* for completeness because it is similar to "classic" standalone ROS nodes. */
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("tf_pub");
auto publisher = node->create_publisher<geometry_msgs::msg::PoseStamped>("tf_pub", 10);
//printf("TimePoint %d",tp);
// std::shared_ptr<rclcpp::Clock> clock_ptr;
rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::TimeSource timesource;
timesource.attachClock(clock);
// tf2_ros::Buffer tfBuffer(clock_ptr);
std::unique_ptr<tf2_ros::Buffer> buffer = nullptr;
tf2::Transform transform_;
transform_.setIdentity();
buffer = std::make_unique<tf2_ros::Buffer>(clock);
geometry_msgs::msg::PoseStamped message;
rclcpp::WallRate loop_rate(500ms);
while (rclcpp::ok()) {
try {
geometry_msgs::msg::TransformStamped transformStamped = buffer->lookupTransform("odom_frame", "camera_link", tf2::TimePointZero);
message.pose.position.x = transformStamped.transform.translation.x;
message.pose.position.y = transformStamped.transform.translation.y;
RCLCPP_INFO(node->get_logger(), "Publishing: '%s'", message.pose);
publisher->publish(message);
}
catch (tf2::TransformException & ex) {
RCLCPP_ERROR(node->get_logger(), "StaticLayer: %s", ex.what());
}
rclcpp::spin_some(node);
loop_rate.sleep();
}
rclcpp::shutdown();
return 0;
}
This probably doesn't help, but we did push a big set of changes to the documentation and how to's for Eloquent. Check them out here.
The code looks okay to me. Are you sure that the target frame (
"odom_frame"
) does indeed exist? You can confirm by runningros2 run tf2_ros tf2_monitor odom_frame camera_link
.