ros中获取小车当前位置
ros中获取小车当前位置
一.tf2获取小车当前位置：
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
tf2_ros::Buffer buffer2;
tf2_ros::TransformListener tfListener(buffer2);
bool uwbmain::getuwb_tf(string &pframe_id,string &cframe_id,geometry_msgs::TransformStamped &ps_out) {
try{
ps_out = buffer2.lookupTransform(pframe_id, cframe_id, ros::Time(0),ros::Duration(1));
ROS_INFO("相对关系信息:父级：%s,子级：%s 偏移量(%.2f,%.2f,%.2f) 4元素(qx %.5f,qy %.5f,qz %.5f,qw %.5f)",
ps_out.header.frame_id.c_str(), // son2
ps_out.child_frame_id.c_str(), // son1
ps_out.transform.translation.x,
ps_out.transform.translation.y,
ps_out.transform.translation.z,
ps_out.transform.rotation.x,
ps_out.transform.rotation.y,
ps_out.transform.rotation.z,
ps_out.transform.rotation.w
);
}catch (exception e){
std::cout << "=======getuwb_urdf=====异常===================" << std::endl;
return false;
}
return true;
}
//====================================使用=========================================
bool getuwb_tfmapTbase_linkFig=false;
try {
//获取矩阵
geometry_msgs::TransformStamped mapTbase_link;
string pframe_id ="map";//基坐标
string cframe_id ="base_link";
getuwb_tfmapTbase_linkFig = getuwb_tf(pframe_id,cframe_id,mapTbase_link);
if(getuwb_tfmapTbase_linkFig){
} else{
}
}catch (exception e){
}下面是一个简单的 C++ 例子，展示了如何使用单独的线程填充 tf2 缓存，并在主线程中查询变换数。
#include
#include
int main(int argc, char** argv)
{
ros::init(argc, argv, "tf2_listener");
// 创建 tf2 缓存
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
// 创建一个单独的线程来填充 tf2 缓存
std::thread tfThread([&]() {
ros::Rate rate(10.0); // 读取消息的频率为 10 Hz
while (ros::ok()) {
try {
// 填充 tf2 缓存
tfBuffer.canTransform("target_frame", "source_frame", ros::Time(0));
} catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
}
rate.sleep();
}
});
// 在主线程中查询变换数
while (ros::ok()) {
try {
geometry_msgs::TransformStamped transformStamped;
transformStamped = tfBuffer.lookupTransform("target_frame", "source_frame", ros::Time(0));
ROS_INFO("Got transform: %f %f %f", transformStamped.transform.translation.x,
transformStamped.transform.translation.y, transformStamped.transform.translation.z);
} catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
}
ros::Duration(1.0).sleep(); // 暂停一秒钟
}
// 等待线程结束并退出
tfThread.join();
return 0;
}
在这个例子中，我们首先创建了一个 tf2_ros::Buffer 和一个 tf2_ros::TransformListener 对象来处理 tf2 数据。然后，我们使用一个单独的线程来填充 tf2 缓存，这个线程每隔 100 毫秒就会尝试读取 tf2 消息并填充到缓存中。在主线程中，我们使用 tfBuffer.lookupTransform() 函数查询变换，并在控制台上打印出结果。注意，在主线程中我们使用了 ros::Duration(1.0).sleep() 函数来暂停一秒钟，以避免查询变换的频率过快。
需要注意的是，在实际使用中，你可能需要使用更高级别的线程控制和错误处理技术，以确保程序的健壮性和可靠性。
二.tf1获取小车当前位置：
//
// Created by sukai on 22-9-20.
// 发布机器人当前坐标
//
#include
#include
#include
#include
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"
#include
#include
#include
using namespace std;
bool is_stamped =false;//is_stamped
void mySigintHandler(int sig)
{
// Do some custom action.
// For example, publish a stop message to some other nodes.
// All the default sigint handler does is call shutdown()
ros::shutdown();
}
// rosrun contnav currency_robotpose_node
int main(int argc,char *argv[]){
setlocale(LC_ALL,""); //中文
//节点名
string nodeName = "currency_robotpose_node";
//初始化节点
ros::init(argc,argv,nodeName);
//创建节点
ros::NodeHandle node;
signal(SIGINT, mySigintHandler);
//topic名字 外参标定时发送小车实时pose
string topicName_odom = "/robot_pose_me";
ros::Publisher p_pub,pose2d_pub;
if(is_stamped)
p_pub = node.advertise(topicName_odom, 1);
else
p_pub = node.advertise(topicName_odom, 1);//js -> geometry_msgs/Pose
pose2d_pub = node.advertise("/robot_pose2d", 1);//js -> geometry_msgs/Pose
tf::TransformListener listener;
listener.waitForTransform("/map", "/base_link", ros::Time(), ros::Duration(1.0));
ros::Rate rate(300);
while (ros::ok()) {
tf::StampedTransform transform;
try {
//监听小车在基坐标下的相对坐标 /slamware_map /map
// listener.lookupTransform("/odom", "/base_link", ros::Time(0), transform);
listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
/**
* 转欧拉角
const tf::Quaternion &quaternion = transform.getRotation();
double roll, pitch, yaw;//定义存储r\p\y的容器
tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);//进行转换
*/
//=======================================
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.frame_id = "/map";
pose_stamped.header.stamp = ros::Time::now();
pose_stamped.pose.orientation.x = transform.getRotation().getX();
pose_stamped.pose.orientation.y = transform.getRotation().getY();
pose_stamped.pose.orientation.z = transform.getRotation().getZ();
pose_stamped.pose.orientation.w = transform.getRotation().getW();
pose_stamped.pose.position.x = transform.getOrigin().getX();
pose_stamped.pose.position.y = transform.getOrigin().getY();
pose_stamped.pose.position.z = transform.getOrigin().getZ();
if(is_stamped)
p_pub.publish(pose_stamped);
else
p_pub.publish(pose_stamped.pose);
//转 pose2D
double roll,pitch,yaw;
geometry_msgs::Pose2D pose2D;
tf::Matrix3x3(transform.getRotation()).getRPY(roll,pitch,yaw);
pose2D.x=transform.getOrigin().getX();
pose2D.y=transform.getOrigin().getY();
pose2D.theta=yaw;
pose2d_pub.publish(pose2D);
} catch (exception e) {
// ROS_INFO_STREAM(e.what());
rate.sleep();
continue;
}
rate.sleep();
ros::spinOnce();
}
return 0;
}