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;
}

实验结果呈现