ROS——多个海龟追踪一个海龟实验

目标

通过键盘控制一个海龟（领航龟）的移动，其余生成的海龟通过监听实现追踪
定期获取领航龟和其余龟的坐标信息，通过广播告知其余龟，进行相应移动
其余龟负责监听

疑惑点（已解决）

int main(int argc, char** argv) 是 C 和 C++ 程序中主函数 (main) 的常见定义形式。这个函数是程序的入口点，操作系统在启动程序时首先调用 main 函数。

参数解释：

int argc：代表 “argument count”，是一个整型变量，表示传递给程序的命令行参数的数量（包括程序名称本身）。例如，如果你的程序名为 myprog，并且在命令行下以 myprog arg1 arg2 的方式运行，那么 argc 将为 3（程序名 myprog 计为第一个参数）。
char** argv：代表 “argument vector”，是一个字符指针数组的指针，每个元素都是指向一个字符串的指针。这个数组包含了所有通过命令行传递给程序的参数。argv[0] 总是指向程序的名称，而 argv[1]、argv[2] 等分别指向后续的各个参数。每个参数都是一个以空字符 ‘\0’ 结尾的字符串。
main 函数返回类型为 int，通常用来表示程序的退出状态。返回值为 0 表示程序正常结束，非零值则表示有错误发生，具体的数值可以根据需要定义，但通常的约定是返回负数或非零正数表示异常终止。

生成海龟

在这里插入图片描述

生成执行文件及相关依赖

add_executable(fllow_broadcaster src/turtle_broadcaster.cpp)
target_link_libraries(fllow_broadcaster ${catkin_LIBRARIES})add_executable(fllow_listener src/turtle_listener.cpp)
target_link_libraries(fllow_listener ${catkin_LIBRARIES})add_executable(create_turtle src/spawn_turtle.cpp)
target_link_libraries(create_turtle ${catkin_LIBRARIES})

xml配置

<launch><node pkg="turtlesim" type="turtlesim_node" name="sim" /><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" /><!-- 生成多个海龟，这里的type指的是可执行文件的自定义名字--><node pkg="Fllowing_test" type="create_turtle" name="c_turtle" args="a b c d e f g" /> <!-- 构建多个海龟广播--><node pkg="Fllowing_test" type="fllow_broadcaster" args="turtle1" name="turtle1_broadcaster" /> <node pkg="Fllowing_test" type="fllow_broadcaster" args="a" name="turtle2_broadcaster" /><node pkg="Fllowing_test" type="fllow_broadcaster" args="b" name="turtle3_broadcaster" /><node pkg="Fllowing_test" type="fllow_broadcaster" args="c" name="turtle4_broadcaster" /><node pkg="Fllowing_test" type="fllow_broadcaster" args="d" name="turtle5_broadcaster" /><node pkg="Fllowing_test" type="fllow_broadcaster" args="e" name="turtle6_broadcaster" /><node pkg="Fllowing_test" type="fllow_broadcaster" args="f" name="turtle7_broadcaster" /> <node pkg="Fllowing_test" type="fllow_broadcaster" args="g" name="turtle8_broadcaster" /> <!-- 设置多个海龟之间的监听关系 --><node pkg="Fllowing_test" type="fllow_listener" args="turtle1 a" name="turtle1_tf_listener" /><node pkg="Fllowing_test" type="fllow_listener" args="a b" name="turtle2_tf_listener" /><node pkg="Fllowing_test" type="fllow_listener" args="b c" name="turtle3_tf_listener" /><node pkg="Fllowing_test" type="fllow_listener" args="c d" name="turtle4_tf_listener" /><node pkg="Fllowing_test" type="fllow_listener" args="d e" name="turtle5_tf_listener" /><node pkg="Fllowing_test" type="fllow_listener" args="e f" name="turtle6_tf_listener" /><node pkg="Fllowing_test" type="fllow_listener" args="f g" name="turtle7_tf_listener" />
</launch>

spawn_turtle.cpp

测试指令：rosrun Fllowing_test create_turtle a b c d e （后续字母 + i 是自定义海龟名字）

#include <ros/ros.h>
#include <turtlesim/Spawn.h>int main(int argc ,char **argv){int i;if(argc < 2) {ROS_ERROR("Turtle's name can't be empty");return -1;}ros::init(argc,argv,"Create_turtle");ros::NodeHandle node;ros::service::waitForService("spawn");ros::ServiceClient create_turtle_client = node.serviceClient<turtlesim::Spawn>("spawn");turtlesim::Spawn srv;  //用于配置海龟信息for(i = 1;i < argc;++i){srv.request.x = i;srv.request.y = i;srv.request.theta = 0;srv.request.name = *(argv+i);create_turtle_client.call(srv);     //发送请求生成海龟的服务}return 0;
}

turtle_broadcaster

// 例程产生 tf 数据，并计算、发布 turtle2的速度指令
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>std::string turtle_name;//pose回调函数
void poseCallback(const turtlesim::PoseConstPtr& msg){//创建tf的广播器static tf::TransformBroadcaster br;//初始化tf数据tf::Transform transform;transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));   //当前海龟的坐标信息tf::Quaternion q;q.setRPY(0,0,msg->theta);   //当前海龟的RPY旋转状态transform.setRotation(q);//广播world与海龟坐标系之间的tf数据br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
}int main(int argc,char** argv){ros::init(argc,argv,"turtle_broadcaster");//输入参数作为海龟的名字if(argc < 2){ROS_ERROR("need turtle name as argument!");return -1;}turtle_name = argv[1];   //argv[0]是程序文件名，argv[1]以及后续才是参数列表//订阅海龟位置ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10,&poseCallback);// 循环等待回调函数ros::spin();return 0;
}

turtle_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
#include <string>
using namespace std;int main(int argc,char **argv){//拼接海龟驱动指令string driver_cmd("/");driver_cmd.append(argv[2]);driver_cmd.append("/cmd_vel");if(argc < 3){ROS_ERROR("need turtle name as argument!");return -1;}ros::init(argc,argv,"turtle_listener");ros::NodeHandle node;//请求产生海龟对象ros::service::waitForService("/spawn");ros::ServiceClient turtle = node.serviceClient<turtlesim::Spawn>("/spawn");turtlesim::Spawn srv_add_turtle;turtle.call(srv_add_turtle);//创建发布速度控制指令的发布者,控制海龟移动的ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>(driver_cmd,10);//创建tf的监听器tf::TransformListener listener;ros::Rate rate(10.0);while(node.ok()){//获取坐标系之间的tf数据tf::StampedTransform transform;try{// 判断坐标系中是否存在相应海龟节点，持续三秒，向广播者发送位置信息请求listener.waitForTransform(argv[2],argv[1],ros::Time(0),ros::Duration(3.0));// 获取实时位置，将四元数存到transform变量中listener.lookupTransform(argv[2],argv[1],ros::Time(0),transform);}catch(tf::TransformException &ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}//根据turtle1与turtle2坐标系之间的位置关系，配置移动信息，并发布turtle2的速度指令geometry_msgs::Twist vel_msg;vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().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;
}