赞
踩
需求描述
实现分析
乌龟跟随实现的核心,是乌龟A和B都要发布相对世界坐标系的坐标信息,然后,订阅到该信息需要转换获取A相对于B坐标系的信息,最后,再生成速度信息,并控制B运动。
实现流程:C++ 与 Python 实现流程一致
创建第二只乌龟需要使用rosservice,话题使用的是 spawn
rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'"
name: "turtle_flow"
键盘是无法控制第二只乌龟运动的,因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息。
是通过话题 /乌龟名称/pose 来获取的
x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度
tf04_test
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim
<launch>
<!-- 1. 启动乌龟GUI节点 -->
<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
<!-- 键盘控制-->
<node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
<!-- 2. 生成新的乌龟节点 --> type 表示是cpp文件
<node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/>
</launch>
#include"ros/ros.h" #include"turtlesim/Spawn.h"// 消息类型 /* 需求:向服务器发送请求,生成一个新的乌龟 话题:/spawn 消息:turtlesim/Spawn 1.包含头文件 2.初始化ROS节点 3.创建节点句柄 4.创建客户端对象 5.组织数据并发送 6.处理响应 */ int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化ROS节点 ros::init(argc,argv,"service_call"); // 3.创建节点句柄 ros::NodeHandle nh; // 4.创建客户端对象 ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn"); // /spawn是话题名称 // 5.组织数据并发送 // 5.1 组织请求数据 turtlesim::Spawn spawn; spawn.request.x = 1.0; spawn.request.y = 4.0; spawn.request.theta = 1.57; spawn.request.name = "turtle2"; // 5.2 发送请求 // 判断服务器状态 // ros::service::waitForService("/spawn"); client.waitForExistence(); bool flag = client.call(spawn);//flag 接受响应状态,响应结果也会被设置进spawn对象 // 6.处理响应 if(flag) // 响应成功 { ROS_INFO("乌龟生成成功,新乌龟叫:%s",spawn.response.name.c_str()); } else { ROS_INFO("请求失败!!!"); } return 0; }
add_executable(test01_new_turtle src/test01_new_turtle.cpp)
add_dependencies(test01_new_turtle ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(test01_new_turtle
${catkin_LIBRARIES}
)
cd demo02_ws/
source ./devel/setup.bash
roslaunch tf04_test test.launch
rostopic list
/rosout
/rosout_agg
/tf_static
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
/turtle2/cmd_vel
/turtle2/color_sensor
/turtle2/pose
“linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0”
<launch> <!-- 1. 启动乌龟GUI节点 --> <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/> <!-- 键盘控制--> <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" /> <!-- 2. 生成新的乌龟节点 --> <node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/> <!-- 3. 需要设置两个乌龟相对于世界的坐标系的发布 --> <!-- 基本实现思路: 1. 节点只编写一个 2. 这个节点需要启动两次 3. 节点启动时动态传参: 第1次启动传递turtle1,第2次传turtle2 --> <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub1" args = "turtle1" output = "screen"/> <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub2" args = "turtle2" output = "screen"/> <!-- 由type类型确定和test02_pub_turtle.cpp建立联系 --> </launch>
#include"ros/ros.h" #include"turtlesim/Pose.h" // 创建发布对象 #include"tf2_ros/transform_broadcaster.h" // 组织发布数据 #include"geometry_msgs/TransformStamped.h" // 坐标系四元数 #include"tf2/LinearMath/Quaternion.h" /* 发布方: 需要订阅乌龟的位姿信息, 转换成相对于窗体的坐标关系,并发布 准 备: 话题: /turtle1/pose 消息: turtlesim/Pose 流程: 1. 包含头文件 2. 设置编码,初始化, NodeHandle 3. 创建订阅对象,订阅/turtle1/pose 4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注) 5. spin() */ // 声明变量接受传递的参数 std::string turtle_name; // 发布turtle1相对于世界坐标系的位置 // 给传进回调函数的数据定义范型 头文件2 void doPose(const turtlesim::Pose::ConstPtr& pose){ // 获取位姿信息,转换成坐标系相对关系(核心),并发布 // a. 创建发布对象 头文件3 static tf2_ros::TransformBroadcaster pub; // b. 组织被发布的数据 头文件4 geometry_msgs::TransformStamped ts; ts.header.frame_id = "world"; // 相对于世界坐标系 ts.header.stamp = ros::Time::now(); // 关键点2: 动态传入 // ts.child_frame_id = "turtle1"; ts.child_frame_id = turtle_name; // 坐标系偏移量 ts.transform.translation.x = pose->x; ts.transform.translation.y = pose->y; ts.transform.translation.z = 0; // z一直是零(2D) // 坐标系四元数 头文件5 /* 位姿信息中没有四元数,但是有偏航角度,又已知乌龟时2D,没有翻滚和俯仰角,所以得出乌龟的 欧拉角: 0 0 theta */ tf2::Quaternion qtn; qtn.setRPY(0,0,pose->theta); ts.transform.rotation.x = qtn.getX(); ts.transform.rotation.y = qtn.getY(); ts.transform.rotation.z = qtn.getZ(); ts.transform.rotation.w = qtn.getW(); // c. 发布 pub.sendTransform(ts); } int main(int argc, char *argv[]) { // 2. 设置编码,初始化, NodeHandle setlocale(LC_ALL,""); ros::init(argc,argv,"pub_turtle"); ros::NodeHandle nh; /* 解析launch文件,通过args传入的参数,第1次传入turtle1,传入第2次是turtle2 */ if (argc != 2)// 成立时应该是两个 { ROS_INFO("请传入一个参数"); return 1; }else{ turtle_name = argv[1];// argv[0]表示程序名称,argv[1]表示第一个参数就是 turtle1或者turtle2 ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str()); } // 3. 创建订阅对象,订阅/turtle1/pose // 关键点1: 订阅的话题名称,turtle1 或 turtle2 动态传入 ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,doPose); // 4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注) // 5. spin() ros::spin(); return 0; }
add_executable(test02_pub_turtle src/test02_pub_turtle.cpp)
add_dependencies(test02_pub_turtle ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(test02_pub_turtle
${catkin_LIBRARIES}
)
cd demo02_ws/
source ./devel/setup.bash
roslaunch tf04_test test.launch
<launch> <!-- 1. 启动乌龟GUI节点 --> <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/> <!-- 键盘控制--> <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" /> <!-- 2. 生成新的乌龟节点 --> <node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/> <!-- 3. 需要设置两个乌龟相对于世界的坐标系的发布 --> <!-- 基本实现思路: 1. 节点只编写一个 2. 这个节点需要启动两次 3. 节点启动时动态传参: 第1次启动传递turtle1,第2次传turtle2 --> <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub1" args = "turtle1" output = "screen"/> <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub2" args = "turtle2" output = "screen"/> <!-- 由type类型确定和test02_pub_turtle.cpp建立联系 --> <!-- 4. 需要订阅 turtle1 与 urtle2相对于世界坐标系的坐标消息,并转换成 turtle1 相对于 turtle2 的坐标关系 再生成速度消息 --> <node pkg = "tf04_test" type = "test03_control_turtle2" name = "control" output = "screen"/> </launch>
#include"ros/ros.h" #include"tf2_ros/transform_listener.h" #include"tf2_ros/buffer.h" #include"geometry_msgs/PointStamped.h" #include"tf2_geometry_msgs/tf2_geometry_msgs.h" #include"geometry_msgs/TransformStamped.h" // 求turtle1在turtle2中的坐标,查看二者的坐标关系 /* 需求1: 换算出 turtle1 相对于 turtle2 的关系 需求2: 计算角速度和线速度并发布 */ int main(int argc, char *argv[]) { // 2. 编码,初始化 NodeHandle setlocale(LC_ALL,""); ros::init(argc,argv,"tfs_sub"); ros::NodeHandle nh; // 3. 创建订阅对象 头文件2/3 tf2_ros::Buffer buffer; tf2_ros::TransformListener sub(buffer); // 4. 编写解析逻辑 ros::Rate rate(10); while(ros::ok()) { // 核心 try { // 1. 计算 turtle1 与 turtle2 的相对关系 /* A相对于B的坐标系关系 参数1: 目标坐标系 B 参数2: 源坐标系 A 参数3: ros::Time(0)取间隔最短的两个坐标关系帧来计算相对关系 返回值: geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系 */ geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0)); // 目标坐标系,源坐标系,时间0表示查找最近两个坐标系消息 ROS_INFO("turtle1 相对于 turtle2 的信息: 父级: %s,子级: %s 偏移量(%.2f,%.2f,%.2f)", son1ToSon2.header.frame_id.c_str(), // 父坐标系 turtle2 son1ToSon2.child_frame_id.c_str(), // 子坐标系 turtle1 son1ToSon2.transform.translation.x, // 子坐标系与父坐标系的相对位置 son1ToSon2.transform.translation.y, son1ToSon2.transform.translation.z ); } catch(const std::exception& e) { ROS_INFO("错误提示: %s",e.what()); } rate.sleep(); // 5. spinOnce() ros::spinOnce(); } return 0; }
订阅 turtle1 和 turtle2 的 TF 广播信息,查找并转换时间最近的 TF 信息
roslaunch tf04_test test.launch
<launch> <!-- 1. 启动乌龟GUI节点 --> <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/> <!-- 键盘控制--> <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" /> <!-- 2. 生成新的乌龟节点 --> <node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/> <!-- 3. 需要设置两个乌龟相对于世界的坐标系的发布 --> <!-- 基本实现思路: 1. 节点只编写一个 2. 这个节点需要启动两次 3. 节点启动时动态传参: 第1次启动传递turtle1,第2次传turtle2 --> <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub1" args = "turtle1" output = "screen"/> <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub2" args = "turtle2" output = "screen"/> <!-- 由type类型确定和test02_pub_turtle.cpp建立联系 --> <!-- 4. 需要订阅 turtle1 与 urtle2相对于世界坐标系的坐标消息,并转换成 turtle1 相对于 turtle2 的坐标关系 再生成速度消息 --> <node pkg = "tf04_test" type = "test03_control_turtle2" name = "control" output = "screen"/> </launch>
#include"ros/ros.h" #include"tf2_ros/transform_listener.h" #include"tf2_ros/buffer.h" #include"geometry_msgs/PointStamped.h" #include"tf2_geometry_msgs/tf2_geometry_msgs.h" #include"geometry_msgs/TransformStamped.h" #include"geometry_msgs/Twist.h" // 求turtle1在turtle2中的坐标,查看二者的坐标关系 /* 需求1: 换算出 turtle1 相对于 turtle2 的关系 需求2: 计算角速度和线速度并发布 */ int main(int argc, char *argv[]) { // 2. 编码,初始化 NodeHandle setlocale(LC_ALL,""); ros::init(argc,argv,"tfs_sub"); ros::NodeHandle nh; // 3. 创建订阅对象 头文件2/3 tf2_ros::Buffer buffer; tf2_ros::TransformListener sub(buffer); // A. 创建速度发布对象 头文件7 (有关于速度消息发布) ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100); // 4. 编写解析逻辑 ros::Rate rate(10); while(ros::ok()) { // 核心 try { // 1. 计算 turtle1 与 turtle2 的相对关系 /* A相对于B的坐标系关系 参数1: 目标坐标系 B 参数2: 源坐标系 A 参数3: ros::Time(0)取间隔最短的两个坐标关系帧来计算相对关系 返回值: geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系 */ geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0)); // 目标坐标系,源坐标系,时间0表示查找最近两个坐标系消息 // ROS_INFO("turtle1 相对于 turtle2 的信息: 父级: %s,子级: %s 偏移量(%.2f,%.2f,%.2f)", // son1ToSon2.header.frame_id.c_str(), // 父坐标系 turtle2 // son1ToSon2.child_frame_id.c_str(), // 子坐标系 turtle1 // son1ToSon2.transform.translation.x, // 子坐标系与父坐标系的相对位置 // son1ToSon2.transform.translation.y, // son1ToSon2.transform.translation.z // ); // B. 根据相对计算并组织速度消息 头文件8 geometry_msgs::Twist twist; /* 组织速度,只需要设置线速度的 x 与 角速度的 z x = 系数 * sqrt((y^2+x^2)) z = 系数 * atan2(对边/邻边) 反正切,以弧度为单位 */ // 乌龟只会前后走 所以线速度只有x方向上有 twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x,2) + pow(son1ToSon2.transform.translation.y,2)); // 2 表示平方 twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x); // C. 发布 pub.publish(twist); } catch(const std::exception& e) { ROS_INFO("错误提示: %s",e.what()); } rate.sleep(); // 5. spinOnce() ros::spinOnce(); } return 0; }
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。