ROS1-noetic-乌龟跟随-进阶1

最新推荐文章于 2024-08-13 18:47:29 发布
最新推荐文章于 2024-08-13 18:47:29 发布
阅读量803 收藏 7
点赞数 12
分类专栏： ROS1 文章标签： 机器人 自动驾驶 人工智能
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/CSDN_sunhaohao/article/details/138350062
版权

目录

前言

效果演示

1.产生一只新乌龟

2.发布两个乌龟的相对坐标关系

3.订阅两个乌龟的坐标关系，发布乌龟2的速度信息

4.集成launch文件

5.配置文件

6.运行

前言

本文章是根据赵虚左老师的代码改进的，原本的乌龟跟随是重合，我这里使它跟随乌龟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

依据此思路可以做一个四个保镖环绕的案例，等待后续吧。

文章难免有错误遗漏之处，还请读者批评指正。

懒洋洋-软绵绵
关注
专栏目录
ROS小乌龟控制1
Mr_rustylake的博客
12-06 1222
我们知道，ROS内置了一个小乌龟的样例程序。我们不仅可以通过键盘控制小乌龟的运动，也可以通过编码改变小乌龟的运动
Ubuntu20.04安装ROS——实现小海龟仿真
zhoushuaiyxlmwan的博客
03-02 901
ROS (Robot Operating System, 机器人操作系统) 提供一系列程序库和工具以帮助软件开发者创建机器人应用软件。它提供了硬件抽象、设备驱动、库函数、可视化、消息传递和软件包管理等诸多功能。
ROS学习笔记：ROS1+noetic实现各通信机制
m0_62187438的博客
04-27 1114
本文基于ROS1+noetic环境，通过vscode集成开发环境实现了话题通信、服务通信和参数服务器三种通信机制的demo。
ros1——noetic版本 细节性配置记录（python）（后续会不断更新）
wangxiangyao1的博客
08-04 306
在功能包下创建msg文件夹，添加msg文件，文件名+msg。
【ROS1-Noetic】学习记录01-准备阶段和辅助功能
最新发布
qq_51016804的博客
08-13 1182
一键下载ros小鱼的一键安装指令。
ROS1 Noetic主从机通信使用详解
m0_56661101的博客
12-27 1473
2023/12/27是一个机器人爱好者开源组织。由于笔者水平有限，以下内容可能存在事实性错误。本文分为设置步骤相关知识和参考资料三部分。如果你只希望设置，请参考设置步骤。如果你希望了解相关概念，请参考相关知识。如果你希望深入了解ROS的环境变量设置和多机通讯，请参考参考资料。
ros-noetic-octomap-mapping：ros-noetic-octomap-mapping AUR包
02-17
在这个场景中，"ros-noetic-octomap-mapping" 是 ROS Noetic 版本下的一个特定模块，主要涉及到三维空间建图和导航。 "octomap" 是 OctoMap 的简称，它是一种高效的三维概率 occupancy grid（占用网格）映射库。...
ros-noetic-rosmsg：ros-noetic-rosmsg AUR软件包
02-13
在实际使用`ros-noetic-rosmsg`时，确保你的系统已经安装了ROS Noetic的基础环境，包括`ros-noetic-ros-base`，并配置好了ROS环境变量。同时，为了在Arch Linux上顺利运行ROS，你可能还需要安装额外的依赖，如`...
ros-noetic:ros-noetic的vinca配置文件
03-11
RoboStack（用于ROS Noetic） 为什么选择ROS和Conda？ 欢迎使用RoboStack，它将ROS与跨平台，与语言无关的软件包管理器Conda紧密地结合在一起。 我们为Linux，macOS，Windows和ARM（Linux）提供ROS二进制文件。 ...
ros-noetic-rqt-service-caller:ros-noetic-rqt-service-caller AUR软件包
03-28
"ros-noetic-rqt-service-caller" 是ROS Noetic版本中的一个工具，它是RQT（Robot Qt Toolkit）的一部分。RQT是基于Qt库的ROS图形用户界面插件集合，提供了多种用于调试、监控和交互的工具。"rqt-service-caller" ...
ros-noetic-depth-image-proc:ros-noetic-depth-image-proc AUR软件包
03-28
在这个特定的场景中，我们关注的是 "ros-noetic-depth-image-proc" 软件包，它是ROS Noetic版本的一部分。Noetic是ROS的一个长期支持版本，代号为" Ninjutsu"，发布于2020年，基于Ubuntu Focal Fossa。 "depth-...
ROS1 noetic + depthai_ros教程
oakchina的博客
05-02 1415
之前有小伙伴表示OAK相机在C++上用起来有问题，我们的技术小哥们也立马安排起来了！C++的开发环境终于打包好啦！
ROS----好多小乌龟
qq_25105061的博客
01-08 2424
ROS----好多小乌龟 当我们运行完rosrun turtlesim turtlesim_node后在桌面上出现一只小乌龟，显得那么孤单和寂寞，所以我们就来杀死这个单身龟，再创建一群小乌龟。 之前乌龟的控制是通过topic的发布和订阅来实现，此处的乌龟的死亡和新生是通过service来实现。 启动小乌龟，然后rosservice list看一下，有哪些服务被启动。可以看到一个/clear、/ki...
ros1 noetic安装(亲测可用)
海涛高软
07-24 201
Ubuntu 安装 ROS 详细教程（以最后一个ROS1版本Noetic为例）-WinFrom控件库|.net开源控件库|HZHControls官网
ROS1系列（二） Noetic安装
万法自然的博客
11-23 1646
ros wiki 设置软件源 sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list' 添加秘钥 sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80'
11.redis集群
guachiqi2340的博客
11-28 337
介绍 redis集群就是将多个redis服务放在一起，通过某种策略让他们各司其职。当用户访问redis时通过某种算法将请求发送到不同的redis服务。 主从复制解决了读写压力的问题，那么集群解决了内存压力的问题。 redis集群实现了对redis的水平扩容，即启动N个redis节点，将整个数据库分布存储在这N个节点中，每个节点存储总数据的1/N。 redis集群通过分区（Partition）来提供一定程度的可用性（Availability），即使集群中有一部分节点失效或者无法进行通讯，集群也可以继续处理命令
ROS服务通信应用（1）：创建服务客户端，生成多只乌龟
weixin_52029211的博客
01-28 3551
需求： 在turtlesim仿真环境中，写一个C++程序，创建客户端，请求/spawn服务，生成多只乌龟 实现效果： 服务通信常用命令行工具: rosservice list 服务列表 rosservice type /spawn 消息类型 rossrv info turtlesim/Spawn 消息格式 rosservice call /clear 清除功能(可以清理小乌龟走过的线路) rosservice call /kill “name: ‘turtle1’” 杀死小乌龟 ross
ROS学习——运行小乌龟
m0_51561759的博客
10-20 3114
ROS入门之操作小乌龟
ROS入门之小海龟
cjhz2333的博客
03-05 7331
ROS（机器人操作系统）提供一系列程序库和工具以帮助软件开发者创建机器人应用软件。它提供了硬件抽象、设备驱动、库函数、可视化、消息传递和软件包管理等诸多功能。ROS即通讯机制+开发工具+应用功能+生态系统四位一体。特点：点对点、分布式、多语言支持、轻量级、免费和开源。本次实验主要是了解了ROS的消息和话题的收发机制，用小海龟demo直观的感受了ROS，并编程实现了控制小海龟进行圆周运动。
ubuntu20.04安装ros1-noetic
09-19
在Ubuntu 20.04中安装ROS Noetic的步骤如下： 1. 配置Ubuntu软件仓库，可以使用以下命令： ``` sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list' ``` 2. 设置ROS密钥： ``` sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 ``` 3. 安装ROS Noetic： ``` sudo apt update sudo apt install ros-noetic-desktop-full ``` 4. 初始化rosdep： ``` sudo rosdep init rosdep update ``` 5. 设置环境变量： ``` echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc source ~/.bashrc ``` 6. 安装ROS工具和依赖项： ``` sudo apt install python3-rosinstall python3-rosinstall-generator python3-wstool build-essential ```
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值