一、项目简介
本项目主要实现两只小乌龟的跟随运动，主要分三个步骤：
-
- 创建turtle2,用于跟随turtle1
-
- 发布相对于世界坐标系的乌龟坐标
-
- 根据坐标计算turtle2的线速度和角速度
二、代码实现
功能包依赖：
tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
2.1 C++实现
2.1.1 创建turtle2（turtle2.cpp）
/*
创建第二只小乌龟
*/
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 节点初始化
ros::init(argc,argv,"turtle2");
ros::NodeHandle nh;
// 创建服务通信客户端,/spawn为请求的话题名称
ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
// 等待服务端启动
ros::service::waitForService("/spawn");
// 创建第二只小乌龟，并设定小乌龟位姿
turtlesim::Spawn spawn;
spawn.request.name = "turtle2";
spawn.request.x = 1.0;
spawn.request.y = 2.0;
spawn.request.theta = 1.57;
bool flag = client.call(spawn);
if(flag){
ROS_INFO("乌龟%s创建成功!",spawn.request.name.c_str());
}
else{
ROS_INFO("创建失败！");
}
ros::spin();
return 0;
}
2.1.2 组织乌龟坐标发布(test02_pub_turtle.cpp)
/*
组织两只乌龟相对世界坐标系的发布：
在发布的时候，由于两只乌龟只是名字不同，需要发布的
坐标信息相同，所以采用代码复用的方式。
*/
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TwistStamped.h"
// 保存乌龟名称
std::string turtle_name;
void doPose(const turtlesim::Pose::ConstPtr &pose){
// 6.1 创建TF广播器
static tf2_ros::TransformBroadcaster broadcaster;
// 6.2 将pose信息转换为TransformStamped
geometry_msgs::TransformStamped tfs;
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
tfs.child_frame_id = turtle_name;
tfs.transform.translation.x = pose->x;
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0;
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 6.3 发布
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.节点初始化
ros::init(argc,argv,"pub_tf");
// 3.解析传入的命名空间
if (argc != 2)
{
ROS_ERROR("请输入正确的参数！\n");
}
else{
turtle_name = argv[1];
ROS_INFO("%s坐标发送启动！\n",turtle_name.c_str());
}
// 4.创建ROS句柄
ros::NodeHandle nh;
// 5.创建订阅对象，订阅乌龟的坐标
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
// 6.回调函数处理pose信息
// 7.spin
ros::spin();
return 0;
}
2.1.3 控制turtle2跟随turtle1（test03_control_turtle2.cpp）
// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL, "");
// 2.节点初始化
ros::init(argc, argv, "sub_tf");
// 3.创建ros句柄
ros::NodeHandle nh;
// 4.创建TF订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
// 5.处理订阅到的TF,/turtle2/cmd_vel位发布的话题名称
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);
ros::Rate rate(10);
while (ros::ok())
{
try
{
// 5.1获取turtle1相对turtle2的坐标信息
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
// 5.2 根据坐标信息生成速度信息
geometry_msgs::Twist twist;
twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x, 2) + pow(tfs.transform.translation.y, 2));
twist.angular.z = 4 * atan2(tfs.transform.translation.y, tfs.transform.translation.x);
// 5.3 发布速度信息
pub.publish(twist);
}
catch (const std::exception &e)
{
ROS_INFO("错误提示：%s", e.what());
}
rate.sleep();
// 6.spin
ros::spinOnce();
}
return 0;
}
2.1.4 launch文件
<!-- 实现turtle2跟随turtle1案例 -->
<launch>
<!-- 1.启动turtle1和键盘控制 -->
<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen" />
<node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
<!--2.启动turtle2 -->
<node pkg = "pkg04_tf_twoturtle" type = "turtle2" name = "turtle2" output = "screen" />
<!-- 3.启动两只乌龟相对于世界坐标系的发布 -->
<node pkg = "pkg04_tf_twoturtle" type = "test02_pub_turtle" name = "pub1" output = "screen" args = "turtle1" />
<node pkg = "pkg04_tf_twoturtle" type = "test02_pub_turtle" name = "pub2" output = "screen" args = "turtle2" />
<!--
4. 将turtle1和turtle2相对于世界坐标系的关系转化为turtle1相对turtle2的坐标关系，
再生成坐标信息
-->
<node pkg = "pkg04_tf_twoturtle" type = "test03_control_turtle2" name = "control" output = "screen" />
</launch>
2.1.5 CMakeLists.txt文件
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(turtle2 src/turtle2.cpp)
add_executable(test02_pub_turtle src/test02_pub_turtle.cpp)
add_executable(test03_control_turtle2 src/test03_control_turtle2.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(turtle2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(test02_pub_turtle ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(test03_control_turtle2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(turtle2
${catkin_LIBRARIES}
)
target_link_libraries(test02_pub_turtle
${catkin_LIBRARIES}
)
target_link_libraries(test03_control_turtle2
${catkin_LIBRARIES}
)
2.2 Python实现
2.2.1 创建turtle2（test01_new_turtle_p.py）
#! /usr/bin/env python
# 1.导包
from tempfile import SpooledTemporaryFile
from urllib import response
import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse
if __name__ == "__main__":
# 2. 节点初始化
rospy.init_node("turtle2_p")
# 3. 创建服务通信客户端
client = rospy.ServiceProxy("/spawn",Spawn)
# 4. 等待服务启动
client.wait_for_service()
# 5. 创建小乌龟
req = SpawnRequest()
req.x = 1.0
req.y = 2.0
req.theta = 1.5
req.name = "turtle2"
# 6. 发送请求，并处理相应
try:
response = client.call(req)
rospy.loginfo("%s创建成功！\n",response.name)
except Exception as e:
rospy.loginfo("服务调用失败!\n")
2.2.2 发布两只小乌龟相对世界坐标系的坐标(test02_pub_turtle_p.py)
#! /usr/bin/env python
# 1. 导包
from turtle import pos
from numpy import broadcast
import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions
turtle_name = ""
def doPose(pose):
#1.创建坐标系广播器
broadcaster = tf2_ros.TransformBroadcaster()
#2.将pose信息转化为TransformStamped
tfs = TransformStamped()
tfs.header.frame_id = "world"
tfs.header.stamp = rospy.Time.now()
tfs.child_frame_id = turtle_name
tfs.transform.translation.x = pose.x
tfs.transform.translation.y = pose.y
tfs.transform.translation.z = 0
qtn = tf_conversions.transformations.quaternion_from_euler(0,0,pose.theta)
tfs.transform.rotation.x = qtn[0]
tfs.transform.rotation.y = qtn[1]
tfs.transform.rotation.z = qtn[2]
tfs.transform.rotation.w = qtn[3]
# 3. 广播器发送tfs
broadcaster.sendTransform(tfs)
if __name__ == "__main__":
# 2. 节点初始化
rospy.init_node("sub_tfs_p")
# 3. 解析传入的命名空间
rospy.loginfo("----------------------%d",len(sys.argv))
if len(sys.argv) < 4:
rospy.loginfo("参数传入错误")
else:
turtle_name = sys.argv[1]
rospy.loginfo("乌龟名字：%s",turtle_name)
rospy.Subscriber(turtle_name + "/pose",Pose,doPose)
rospy.spin()
2.2.3 控制turtle2运动(test03_control_turtle_p.py)
#! /usr/bin/env python
# 1. 导包
from rospkg import RosPack
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math
if __name__ == "__main__":
# 2. 节点初始化
rospy.init_node("sub_tfs_p")
# 3. 创建TF订阅对象
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)
# 4. 处理订阅到的TF
rate = rospy.Rate(10)
pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
while not rospy.is_shutdown():
rate.sleep()
try:
trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
twist = Twist()
twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
pub.publish(twist)
except Exception as e:
rospy.logwarn("警告：%s",e)
2.2.4 launch文件
<launch>
<!-- 1. 准备工作，生成turtle1 -->
<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen" />
<node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
<!-- 2. 调用服务生成第二只乌龟 -->
<node pkg = "pkg04_tf_twoturtle" type = "test01_new_turtle_p.py" name = "turtle2" output = "screen" />
<!-- 3. 组织发布两字乌龟的坐标信息 -->
<node pkg = "pkg04_tf_twoturtle" type = "test02_pub_turtle_p.py" name = "pub1" args = "turtle1" output = "screen" />
<node pkg = "pkg04_tf_twoturtle" type = "test02_pub_turtle_p.py" name = "pub2" args = "turtle2" output = "screen" />
<!-- 4. 启动turtle2控制节点 -->
<node pkg = "pkg04_tf_twoturtle" type = "test03_control_turtle_p.py" name = "control" output = "screen" />
</launch>
2.2.5 CMakeLists.txt文件
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/test01_new_turtle_p.py
scripts/test02_pub_turtle_p.py
scripts/test03_control_turtle_p.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)