ROS入门小乌龟跟随示例

一、不使用TF转换的方法

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include<math.h>
#include <turtlesim/Pose.h>double x;
double y;
// 记录目标乌龟的位置
double nx;
double ny;
// 记录自身位置
void poseGet(const turtlesim::Pose& msg)
{x=msg.x;y=msg.y;
}
void selfPoseGet(const turtlesim::Pose& msg)
{nx=msg.x;ny=msg.y;
}
int main(int argc, char **argv){ros::init(argc, argv, "nodes_handle");   ros::NodeHandle n;// 初始化节点和句柄ros::Subscriber sub = n.subscribe("/turtle1/pose", 1000,poseGet);// 接收主控乌龟的位姿 ros::Subscriber subs = n.subscribe("/turtle2/pose",1000,selfPoseGet);// 接收跟随乌龟的位姿ros::Publisher cmdVelPub = n.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);// 发送跟随乌龟的速度 ros::Rate loopRate(1000);ros::spinOnce();while (ros::ok()){geometry_msgs::Twist speed;double length = sqrt((nx-x)*(nx-x)+(ny-y)*(ny-y));speed.linear.x = 0; speed.linear.y = 0;speed.linear.z = 0;speed.angular.x = 0;speed.angular.y = 0;speed.angular.z = 0; if(length>0.1){speed.linear.x = (x-nx)/length*2;speed.linear.y = (y-ny)/length*2;// 根据方向分解速度 使跟随乌龟可以靠近主控乌龟的方向 }// 确定发送的消息 cmdVelPub.publish(speed);ros::spinOnce();loopRate.sleep();}return 0;
}

运行时启动roscore，使用rosrun turtlesim turtlesim_node启动乌龟，使用rosservice call /spawn 5.0 5.0 1.0 'turtle2’启动第二只乌龟，之后启动控制程序和rosrun turtlesim turtle_teleop_key Reading from keyboard，实现用键盘控制一个乌龟，用程序跟随控制的乌龟。

这种方法相当于避开了坐标转换，直接让两只乌龟都使用世界坐标系进行计算，由于这个例子太简单，所以使用TF与不使用TF区别不是那么明显。

二、使用TF转换的方法

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>std::string turtle_name;// 代表新建小乌龟的名字
void poseCallback(const turtlesim::PoseConstPtr& msg)
{// tf广播器static tf::TransformBroadcaster br;// 根据乌龟当前的位姿 设置当前小乌龟相对于世界坐标系的坐标变换tf::Transform transform;transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );// 设置子坐标系在父坐标系下的位置信息 建立子坐标系的原点和父坐标系的对应关系tf::Quaternion q;q.setRPY(0, 0, msg->theta);transform.setRotation(q);// 发布坐标变换 四个参数分别为 tf转换 时间戳 目标坐标系 待转换坐标系br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}int main(int argc, char** argv)
{// 初始化节点ros::init(argc, argv, "my_tf_broadcaster");if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};turtle_name = argv[1];// 订阅乌龟的pose信息ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);// pose消息返回的是世界坐标系下的位置信息 直接将这个信息作为转换的目标 // 每次收到位置信息就发送给回调函数进行处理 得到当前乌龟和世界坐标系的对应关系ros::spin();return 0;
};

#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;// 通过服务调用 产生第二只乌龟turtle2ros::service::waitForService("spawn");ros::ServiceClient add_turtle =  node.serviceClient<turtlesim::Spawn>("spawn");turtlesim::Spawn srv;add_turtle.call(srv);// 定义turtle2的速度控制发布器ros::Publisher turtle_vel =  node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);// tf监听器tf::TransformListener listener;ros::Rate rate(10.0);while (node.ok()){// 监听器的状态没有问题的情况下tf::StampedTransform transform;try{listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));// 等待两个frame之间的联通 就是等待tf树上这两个点直接找到了转换的关系listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);// 查找turtle2与turtle1的坐标变换 四个参数分别为目标坐标系 源坐标系 查询的时刻 转换关系的存放位置}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}// 根据turtle1和turtle2之间的坐标变换 计算turtle2需要运动的线速度和角速度// 并发布速度控制指令 使turtle2向turtle1移动// transform代表两个坐标系之间的转换关系geometry_msgs::Twist vel_msg;vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());// 计算角速度 使用getOrigin获得的是跟随乌龟在主控乌龟坐标系下的位置信息// atan2函数返回以弧度表示的y/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><node pkg="turtlesim" type="turtlesim_node" name="sim"/><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/><node pkg="test1" type="turtle_tf_broadcaster" args="/turtle1" name="tutle1_tf_broadcaster"/><node pkg="test1" type="turtle_tf_broadcaster" args="/turtle2" name="tutle2_tf_broadcaster"/><node pkg="test1" type="turtle_tf_listener" name="listener"/>
</launch>

按照下面的文件路径组织文件之后，直接运行launch文件即可。使用TF主要还是方便进行坐标的转换，基本的思路是利用TF，建立turtle1坐标系、世界坐标系以及turtle2坐标系之间的对应关系，这样找出turtle2在turtle1坐标系中的位置，之后就可以利用这个相对位置，控制跟随的乌龟进行移动。

ROS入门小乌龟跟随示例相关推荐

ROS学习笔记之小乌龟跟随
ROS学习笔记之小乌龟跟随说明:整个案例是跟着赵虚左老师的视频和文档资料学习的,特此感谢赵虚左老师和Autolabor官方文档地址视频地址学习案例之前的预备知识:TF坐标变换大体实现流程: ...
ROS入门五 TF坐标变换
ROS入门五 TF坐标变换坐标变换简介 TF功能包是什么? TF功能包干什么 ? TF坐标变换如何实现? TF工具乌龟例程中的TF 安装功能包turtle_tf 运行实现TF的广播和监听功能 ...
ROS入门21讲 | ROS机器人入门教程【简明笔记】
古月·ROS入门21讲 | 一学就会的ROS机器人入门教程文章目录 ROS核心概念 ROS命令行工作空间与功能包订阅与发布发布者 Publisher 订阅者 Subscriber 话题消息的自 ...
SLAM+语音机器人DIY系列：（二）ROS入门——2.ROS系统整体架构
摘要 ROS机器人操作系统在机器人应用领域很流行,依托代码开源和模块间协作等特性,给机器人开发者带来了很大的方便.我们的机器人"miiboo"中的大部分程序也采用ROS进行开发,所 ...
ROS入门笔记（十一）：编写与测试简单的Service和Client (Python)
ROS入门笔记(十一):编写与测试简单的Service和Client (Python) 文章目录 01 导读 02 功能包的创建 03 在功能包中创建自定义服务类型 3.1 定义srv文件 3.2 在 ...
ROS入门笔记（七）：详解ROS文件系统
ROS入门笔记(七):详解ROS文件系统文章目录 01 Catkin编译系统 1.1 Catkin特点 1.2 Catkin工作原理 1.3 使用`catkin_make`进行编译 02 Catki ...
ROS入门笔记（四）：ROS实践（小海龟仿真）— ROS Topics
ROS入门笔记(四):ROS实践(小海龟仿真)- ROS Topics 文章目录 1 基础知识 1.1 运行ROS Master所在主机的资源地址 1.2 ROS常用命令 1.2.1 查看所有以ros ...
古月居 ROS 入门21讲--PA17 ROS中坐标系管理系统笔记
古月居 ROS 入门21讲--PA17 ROS中坐标系管理系统笔记 1.TF 功能包功能 ∙ \bullet ∙ 五秒钟之前,机器人头部坐标系相当于全局坐标系的关系是怎样的? ∙ \bullet ∙机 ...
ROS入门、ROS完整教程
ROS学习入门说明: 学习视频:古月居 ROS入门21讲代码:https://github.com/huchunxu/ros_21_tutorials 文章目录 ROS学习入门 1. ROS常用命 ...

ROS入门小乌龟跟随示例

一、不使用TF转换的方法

二、使用TF转换的方法

ROS入门小乌龟跟随示例相关推荐

最新文章

热门文章

ROS入门 小乌龟跟随示例

一、不使用TF转换的方法

二、使用TF转换的方法

ROS入门 小乌龟跟随示例相关推荐

最新文章

热门文章

ROS入门小乌龟跟随示例

ROS入门小乌龟跟随示例相关推荐