ros实现三只乌龟跟随

以下是一个简单的ROS程序，用于实现三只乌龟跟随：

1. 首先，在终端中启动ROS核心：

roscore

2. 然后，打开三个终端并分别输入以下命令，以启动三只乌龟：

rosrun turtlesim turtlesim_node

rosrun turtlesim turtlesim_node __name:=turtle2

rosrun turtlesim turtlesim_node __name:=turtle3

3. 接下来，在一个新的终端中，输入以下命令，以启动一个ROS节点，该节点将订阅第一只乌龟的位置信息，并向其他两只乌龟发布速度控制信息：

rosrun my_turtle_follow turtle_follow_node

4. 最后，在另一个新的终端中，输入以下命令，以手动控制第一个乌龟的运动：

rosrun turtlesim turtle_teleop_key

5. 现在，当您使用键盘控制第一个乌龟移动时，其他两只乌龟将跟随它的运动。

实现代码：

turtle_follow_node.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>

class TurtleFollow
{
public:
  TurtleFollow();

private:
  void poseCallback(const turtlesim::Pose::ConstPtr& pose);
  void follow();

  ros::NodeHandle nh_;
  ros::Publisher vel_pub_;
  ros::Subscriber pose_sub_;

  turtlesim::Pose current_pose_;
  turtlesim::Pose target_pose_;

  double linear_vel_;
  double angular_vel_;
};

TurtleFollow::TurtleFollow()
{
  vel_pub_ = nh_.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 1);
  pose_sub_ = nh_.subscribe("turtle1/pose", 1, &TurtleFollow::poseCallback, this);

  linear_vel_ = 1.0;
  angular_vel_ = 2.0;
}

void TurtleFollow::poseCallback(const turtlesim::Pose::ConstPtr& pose)
{
  current_pose_ = *pose;
  follow();
}

void TurtleFollow::follow()
{
  double dx = current_pose_.x - target_pose_.x;
  double dy = current_pose_.y - target_pose_.y;

  double distance = sqrt(dx * dx + dy * dy);
  double angle = atan2(dy, dx);

  geometry_msgs::Twist vel_msg;

  if (distance > 0.1)
  {
    vel_msg.linear.x = linear_vel_ * distance;
    vel_msg.angular.z = angular_vel_ * angle;
  }

  vel_pub_.publish(vel_msg);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "turtle_follow");

  TurtleFollow turtle_follow;

  ros::spin();

  return 0;
}

请注意，此代码将第一只乌龟的位置信息订阅为turtlesim/Pose类型。然后，它将计算当前乌龟和目标乌龟之间的距离和角度，并为第二只乌龟发布一个速度控制消息，以使其跟随第一只乌龟的运动。

在运行此代码之前，请确保已编译并安装了my_turtle_follow软件包。