ROS回顾与整理《四、ROS中坐标系管理、广播、监听(编程)》
这门课学习,时隔三年,哈哈哈,我也没想到,不多说,开鲁。
一、TF功能包
机器人本体上,坐标系可能很多,例如T1到T10,仿射了10次,手动计算比较麻烦,但是可以通过TF直接查询。
如下图,base_laser检测到障碍物P的位置t(0.3, 0, 0),咱们通过TF可以自动计算P在base_link坐标系下的位置。
二、坐标系的广播与监听
创建功能包:
catkin_creat_pkg learning_tf roscpp rospy tf turtlesim
创建tf广播器:
定义tf广播器
创建坐标变换值;
发布坐标变换
1 /** 2 * 该例程产生tf数据,并计算、发布turtle2的速度指令 3 */ 4 5 #include <ros/ros.h> 6 #include <tf/transform_broadcaster.h> 7 #include <turtlesim/Pose.h> 8 9 std::string turtle_name; 10 11 void poseCallback(const turtlesim::PoseConstPtr& msg) 12 { 13 // 创建tf的广播器 14 static tf::TransformBroadcaster br; 15 16 // 初始化tf数据 17 //定义存放转换信息(平动,转动)的变量 18 19 //设置坐标原点 20 //(1)setOrigin()函数的参数类型需要为tf::Vector3类型 21 //(2)假设是要发布一个子坐标系为”turtle1”父坐标系为“world”, 22 // 那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点 23 // 在“world”坐标系下的坐标。 24 tf::Transform transform; 25 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); 26 27 //setRPY()函数的参数为”turtle1”在“world”坐标系下的 28 //roll(绕X轴),pitch(绕Y轴),yaw(绕Z轴) 29 tf::Quaternion q; 30 q.setRPY(0, 0, msg->theta); 31 transform.setRotation(q); 32 33 // 广播world与海龟坐标系之间的tf数据 34 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); 35 } 36 37 int main(int argc, char** argv) 38 { 39 // 初始化ROS节点 40 ros::init(argc, argv, "my_tf_broadcaster"); 41 42 // 输入参数作为海龟的名字 43 if (argc != 2) 44 { 45 ROS_ERROR("need turtle name as argument"); 46 return -1; 47 } 48 49 turtle_name = argv[1]; 50 51 // 订阅海龟的位姿话题 52 ros::NodeHandle node; 53 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); 54 55 // 循环等待回调函数 56 ros::spin(); 57 58 return 0; 59 };
创建tf监听器:
1 /** 2 * 该例程监听tf数据,并计算、发布turtle2的速度指令 3 */ 4 5 #include <ros/ros.h> 6 #include <tf/transform_listener.h> 7 #include <geometry_msgs/Twist.h> 8 #include <turtlesim/Spawn.h> 9 10 int main(int argc, char** argv) 11 { 12 // 初始化ROS节点 13 ros::init(argc, argv, "my_tf_listener"); 14 15 // 创建节点句柄 16 ros::NodeHandle node; 17 18 // 请求产生turtle2 19 ros::service::waitForService("/spawn"); 20 ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn"); 21 turtlesim::Spawn srv; 22 add_turtle.call(srv); 23 24 // 创建发布turtle2速度控制指令的发布者 25 ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10); 26 27 // 创建tf的监听器 28 tf::TransformListener listener; 29 30 ros::Rate rate(10.0); 31 while (node.ok()) 32 { 33 // 获取turtle1与turtle2坐标系之间的tf数据 34 tf::StampedTransform transform; 35 try 36 { 37 listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0)); 38 listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); 39 } 40 catch (tf::TransformException &ex) 41 { 42 ROS_ERROR("%s",ex.what()); 43 ros::Duration(1.0).sleep(); 44 continue; 45 } 46 47 // 根据turtle1与turtle2坐标系之间的位置关系,发布turtle2的速度控制指令 48 geometry_msgs::Twist vel_msg; 49 vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), 50 transform.getOrigin().x()); 51 vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + 52 pow(transform.getOrigin().y(), 2)); 53 turtle_vel.publish(vel_msg); 54 55 rate.sleep(); 56 } 57 return 0; 58 };
其中lookupTransform:
a.得到从“/turtle1”到“/turtle2”(from “turtle1” to “/turtle2”)的变换,在实际中使用时,转换得出的坐标是在“/turtle2”坐标系下的。
b.不可以把ros::Time(0)改成ros::time::now(),因为监听做不到实时,会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据,ros::time::now()则指当下,如果非要使用ros::time::now,则需要结合waitForTransform()使用。
c.使用waitForTransform进行等待,在duration时间内直到需要获取的transform是可用的。
修改CMakeLists.txt
1 add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp) 2 target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES}) 3 4 add_executable(turtle_tf_listener src/turtle_tf_listener.cpp) 5 target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
编译运行(每个窗口都要source一下窗口):
catkin_make roscore rosrun turtlesim turtlesin_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
Note:上面同一个节点运行了两次(两只乌龟嘛),会有节点名称冲突,所以采用__name:方式修改节点名称,可以避免冲突。
CV&DL