前言:监听第一只海龟的位置,然后让第二只海龟跟随第一只海龟。
一、新建cpp文件
新建turtle_tf_listener.cpp文件,内容如下:
#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::init(argc, argv, "my_tf_listener"); ros::NodeHandle node; ros::service::waitForService("spawn"); ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn"); turtlesim::Spawn srv; add_turtle.call(srv); ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10); tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()){ tf::StampedTransform transform; try{ listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; } 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; };
二、修改launch文件
在launch文件末尾添加
<launch> <!-- Turtlesim Node--> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <!-- Axes --> <param name="scale_linear" value="2" type="double"/> <param name="scale_angular" value="2" type="double"/> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" /> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /> <node pkg="learning_tf" type="turtle_tf_listener" name="listener" /> </launch>
三、运行
roslaunch learning_tf start_demo.launch
-END-