1. Transformación de coordenadas en el robot.
El paquete de funciones TF se utiliza para gestionar todos los sistemas de coordenadas. Puede registrar la relación entre todos los sistemas de coordenadas en diez segundos y puede mostrar la posición del objeto agarrado en relación con el sistema de coordenadas central del robot.
2. Ejemplo: Pequeña tortuga siguiendo el experimento
1. Siguen las pequeñas tortugas
Después de que aparecen las dos tortugas, una tortuga está en el punto central y la otra tortuga aparece debajo. La tortuga en el centro puede controlarse para moverse, y la tortuga de abajo seguirá automáticamente a la tortuga que controlamos para moverse.
sudo apt-get install ros-noetic-turtle-tf
roslaunch turtle_tf turtle_tf_demo.launch
#rosrun turtlesim turtle_teleop_key
Entre ellos, roslaunch se utiliza para iniciar archivos de script e iniciar muchos de
ellos. Noetic es el número de versión de ROS.
Presione las teclas de flecha en el terminal para controlar la tortuga que está siendo seguida.
Si hay un error en la versión noética de ubuntu20.04, puede consultar el siguiente método para solucionarlo
cd /usr/bin sudo rm -r python # 有的可能没有这个文件，就省略这一步 sudo cp python3 python
2. Ver la relación tf
rosrun tf view_frames
Espere 5 segundos para generar un archivo pdf, ábralo para ver la relación posicional de las coordenadas tf en el sistema actual.
El mundo es el sistema de coordenadas global, y el otro turtle1 y turtle2 son los sistemas de coordenadas de las dos tortugas. El propósito de la rutina es hacer que los dos sistemas de coordenadas se superpongan en coordenadas.
Si hay un error en este paso, necesita modificar el archivo que reportó el error. Simplemente agregue una oración arriba de la
sudo gedit /opt/ros/noetic/lib/tf/view_frames
línea 88 .print(vstr)vstr=str(vstr)
3. Relación de coordenadas tf_echo
rosrun tf tf_echo turtle1 turtle2
Muestra la relación entre los dos sistemas de coordenadas, describiendo cómo el sistema de coordenadas turtle2 se transforma en el sistema de coordenadas turtle1. Incluyendo traslación de traslación y rotación de rotación (cuaternión, radianes, ángulo de tres formas para describir la rotación).
4. plataforma de visualización de visualización rviz 3D
rosrun rviz rviz -d 'rospack find turtle_tf' /rviz/turtle_rviz.rviz
Primero cambie el marco fijo de la izquierda a mundo
Haga clic en Agregar en la parte inferior izquierda para agregar un TF para mostrar la relación posicional de TF
Controle el movimiento de la tortuga, puede ver que los dos sistemas de coordenadas en la imagen se mueven
3. Realización de programación de transmisión y monitoreo del sistema de coordenadas TF
1. Cree un paquete de funciones
cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim
2. Crea un código de emisora tf
Abra el learning_tf/src/catálogo y cree unturtle_tf_broadcaster.cpp
Su contenido es:
/**
* 该例程产生tf数据，并计算、发布turtle2的速度指令
* REFERENCE:www.guyuehome.com
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// 创建tf的广播器
static tf::TransformBroadcaster br;
// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster");
// 输入参数作为海龟的名字
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
// 订阅海龟的位姿话题
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
// 循环等待回调函数
ros::spin();
return 0;
};
3. Cree el código de escucha del oyente.
Del mismo modo, crea otro turtle_tf_listener.cppcon el contenido
/**
* 该例程监听tf数据，并计算、发布turtle2的速度指令
* REFERENCE:www.guyuehome.com
*/
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_listener");
// 创建节点句柄
ros::NodeHandle node;
// 请求产生turtle2
ros::service::waitForService("/spawn");
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
// 创建发布turtle2速度控制指令的发布者
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
// 创建tf的监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transform;
try
{
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
4. Configure las reglas de compilación del código de radiodifusión y escucha de tf
En la configuración learning_tf, CMakeLists.txtagregue el siguiente código en la posición de la figura
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
Es decir, los dos archivos cpp se compilan en dos archivos ejecutables y luego se vincula la biblioteca.
5. Compilar
cd ~/catkin_ws
catkin_make
6. Ejecute el programa
Cada línea del siguiente programa requiere una terminal separada para ejecutarse.
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key
