目录
前言
本文章是根据赵虚左老师的代码改进的，原本的乌龟跟随是重合，我这里使它跟随乌龟1后1米处，思路就是发布一个乌龟1后1米处的静态坐标，然后根据这个静态坐标和乌龟2的坐标关系计算乌龟2的速度发布即可。
效果演示
1.产生一只新乌龟
demo04_server_spawn.cpp
/*
功能：生成一只新乌龟
*/
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 这里是2个参数
ROS_INFO("argc=%d",argc);
for(int i=0;i<argc;i++)
{
ROS_INFO("argv[i]=%s",argv[i]);
}
/*
argv[0]=/home/shh/shh_ros/ros_noetic_ws/ne_ws05/devel/lib/ws05_pkg01_tf/demo04_server_spawn
第一个参数为路径，后面的参数才是用户输入的参数
*/
if(argc!=5)
{
ROS_INFO("参数不足或错误");
}
ros::init(argc,argv,"demo04_server_spawn");
ros::NodeHandle nh;
ros::ServiceClient client=nh.serviceClient<turtlesim::Spawn>("/spawn");
ros::service::waitForService("/spawn");
turtlesim::Spawn spawn;
// spawn.request.x=1;
// spawn.request.y=1;
// spawn.request.theta=0;
// spawn.request.name="shh";
spawn.request.x=atof(argv[1]);
spawn.request.y=atof(argv[2]);
spawn.request.theta=atof(argv[3]);
spawn.request.name=argv[4];
bool flag=client.call(spawn);
if(flag)
{
ROS_INFO("客户端：请求成功");
}
else
{
ROS_INFO("客户端：请求失败");
}
ros::spin();
return 0;
}
/*
shh@shh-HP:~/shh_ros/ros_noetic_ws/ne_ws06$ rosservice info /spawn
Node: /turtlesim_node
URI: rosrpc://shh-HP:57427
Type: turtlesim/Spawn
Args: x y theta name
*/2.发布两个乌龟的相对坐标关系
demo04_pub.cpp
#include "ros/ros.h"
#include "turtlesim/Pose.h"
// 发布 订阅
#include"tf2_ros/transform_broadcaster.h"
// 坐标系 点
#include"geometry_msgs/TransformStamped.h"
// 欧拉角四元数
#include"tf2/LinearMath/Quaternion.h"
// 定义乌龟名称
std::string turtle_name;
void doSub(const turtlesim::Pose::ConstPtr &pose)
{
static tf2_ros::TransformBroadcaster broad;
geometry_msgs::TransformStamped tfs;
tfs.header.stamp=ros::Time::now();
tfs.header.frame_id="world";
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();
broad.sendTransform(tfs);
// ROS_INFO("demo02_dynamic_pub正在发布坐标变换");
}
int main(int argc, char *argv[])
{
// 这里参数个数变多了，不是2个，是4个
setlocale(LC_ALL,"");
ROS_INFO("argc=%d",argc);
for(int i=0;i<argc;i++)
{
ROS_INFO("argv[i]=%s",argv[i]);
}
if(argc!=4)
{
ROS_INFO("参数错误，请输入乌龟名称");
return 0;
}
turtle_name=argv[1];
ROS_INFO("乌龟名称%s",turtle_name.c_str());
/*
[ INFO] [1714401526.362387384]: argc=4
[ INFO] [1714401526.362412943]: argv[i]=/home/shh/shh_ros/ros_noetic_ws/ne_ws05/devel/lib/ws05_pkg01_tf/demo04_pub
[ INFO] [1714401526.362417739]: argv[i]=sun
[ INFO] [1714401526.362421765]: argv[i]=__name:=demo04_pub1
[ INFO] [1714401526.362425710]: argv[i]=__log:=/home/shh/.ros/log/82e32f96-0632-11ef-835f-f359408b161b/demo04_pub1-4.log
*/
ros::init(argc,argv,"demo04_pub");
ros::NodeHandle nh;
// 创建订阅者对象，订阅乌龟的信息
ros::Subscriber sub=nh.subscribe<turtlesim::Pose>(turtle_name+"/pose",10,doSub);
ros::spin();
return 0;
}
/*
shh@shh-HP:~$ rosmsg info turtlesim/Pose
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity
*/
3.订阅两个乌龟的坐标关系，发布乌龟2的速度信息
demo05_sub.cpp
/*
实现思路，在demo04_sub的基础上，发布一个乌龟1后方1米的静态坐标系
计算乌龟2和turtle11的坐标关系
*/
#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/Twist.h"
std::string turtle_name1;
std::string turtle_name2;
std::string turtle_name3;
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ROS_INFO("argc=%d",argc);
for(int i=0;i<argc;i++)
{
ROS_INFO("demo05_subargv[i]=%s",argv[i]);
}
if(argc!=6)
{
ROS_INFO("demo05_sub参数错误，请输入乌龟名称");
return 0;
}
turtle_name1=argv[1];
turtle_name2=argv[2];
turtle_name3=argv[3];
ROS_INFO("demo05_sub乌龟1名称%s",turtle_name1.c_str());
ROS_INFO("demo05_sub乌龟2名称%s",turtle_name2.c_str());
ROS_INFO("demo05_sub乌龟3名称%s",turtle_name3.c_str());
ros::init(argc,argv,"demo05_sub");
ros::NodeHandle nh;
// 创建一个buffer缓存，
tf2_ros::Buffer buffer;
// 创建订阅者对象，订阅坐标相对关系 将订阅的数据缓存到buffer中
tf2_ros::TransformListener listener(buffer);
// 创建发布者对象，发布乌龟sun的速度
ros::Publisher pub=nh.advertise<geometry_msgs::Twist>(turtle_name2+"/cmd_vel",10);
ros::Rate rate(10);
while (ros::ok())
{
try
{
geometry_msgs::TransformStamped p1to2=buffer.lookupTransform(turtle_name2,turtle_name3,ros::Time(0));
// 根据两个坐标关系，生成速度关系
geometry_msgs::Twist twist;
twist.linear.x=0.5*sqrt(pow(p1to2.transform.translation.x,2)+pow(p1to2.transform.translation.y,2));
twist.linear.y=0;
twist.linear.z=0;
twist.angular.x=0;
twist.angular.y=0;
twist.angular.z=4*atan2(p1to2.transform.translation.y,p1to2.transform.translation.x);
// 发布速度
pub.publish(twist);
// ROS_INFO("demo05_sub正在发布速度消息");
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("异常消息 %s",e.what());
}
rate.sleep();
ros::spinOnce();
}
return 0;
}
/*
/turtle1/cmd_vel
shh@shh-HP:~$ rosmsg info geometry_msgs/Twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
*/
4.集成launch文件
demo05_turtlesim_following.launch
<launch>
<!-- 启动乌龟节点 -->
<include file="$(find ws05_pkg01_tf)/launch/turtlesim_start.launch" />
<!-- 设置乌龟参数名 -->
<arg name="turtle_name1" default="turtle1" />
<arg name="turtle_name2" default="sun" />
<arg name="turtle_name3" default="turtle11" />
<!-- 产生新乌龟 要设置乌龟的位置，名字请在这里设置 -->
<node pkg="ws05_pkg01_tf" type="demo04_server_spawn" name="demo04_server_spawn" args="1 9 0 sun" output="screen" />
<!-- 发布两个乌龟相对于世界的坐标关系 -->
<node pkg="ws05_pkg01_tf" type="demo04_pub" name="demo04_pub2" args="$(arg turtle_name1)" output="screen" />
<node pkg="ws05_pkg01_tf" type="demo04_pub" name="demo04_pub1" args="$(arg turtle_name2)" output="screen" />
<!-- 发布一个乌龟1后方1米的坐标系 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="-1 0 0 0 0 0 /$(arg turtle_name1) /$(arg turtle_name3)" output="screen" />
<!-- 发布乌龟2的速度指令 -->
<node pkg="ws05_pkg01_tf" type="demo05_sub" name="demo05_sub" args="$(arg turtle_name1) $(arg turtle_name2) $(arg turtle_name3)" output="screen" />
<!-- 打开rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ws05_pkg01_tf)/config/tf.rviz" />
</launch>
<!--
先执行本launch文件
在执行键盘控制节点
-->
5.配置文件
add_executable(demo04_pub src/demo04_pub.cpp)
add_executable(demo04_server_spawn src/demo04_server_spawn.cpp)
add_executable(demo05_sub src/demo05_sub.cpp)
add_dependencies(demo04_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(demo04_server_spawn ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(demo05_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(demo04_pub
${catkin_LIBRARIES}
)
target_link_libraries(demo04_server_spawn
${catkin_LIBRARIES}
)
target_link_libraries(demo05_sub
${catkin_LIBRARIES}
)6.运行
source devel/setup.bash
roslaunch ws05_pkg01_tf demo05_turtlesim_following.launch
// 启动键盘控制节点
rosrun turtlesim turtle_teleop_key依据此思路可以做一个四个保镖环绕的案例，等待后续吧。
文章难免有错误遗漏之处，还请读者批评指正。
307
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
