ROS C++教程：用键盘控制三只乌龟保持等边三角形队形

本教程将引导您使用ROS C++编写代码，实现以下功能：

创建三只乌龟A、B、C，并将其放置成等边三角形队形。
通过键盘控制乌龟A的移动。
确保乌龟B和C始终与乌龟A保持等边三角形的队形，边长为1，且朝向一致。

代码实现

以下是实现该功能的完整ROS C++代码：

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

const double PI = 3.14159265359;

class TurtleFormation {
public:
  TurtleFormation() : nh_(''), curr_turtle_('A') {
    // 初始化ROS节点句柄
    nh_.getParam('formation_length', formation_length_);
    nh_.getParam('formation_speed', formation_speed_);

    // 创建乌龟A
    spawnTurtle('A', 5.0, 5.0, 0.0);

    // 创建乌龟B和C
    spawnTurtle('B', 5.0 + formation_length_, 5.0, 0.0);
    spawnTurtle('C', 5.0 + formation_length_ / 2.0, 5.0 + formation_length_ * sqrt(3.0) / 2.0, -60.0);

    // 订阅键盘控制消息
    sub_ = nh_.subscribe('/turtle1/cmd_vel', 1, &TurtleFormation::keyCallback, this);
  }

  // 键盘控制回调函数
  void keyCallback(const geometry_msgs::Twist::ConstPtr& msg) {
    // 控制乌龟A移动
    pub_['A'].publish(msg);

    // 计算乌龟B和C应该移动的线速度和角速度
    double vx = formation_speed_ * cos(PI / 3.0);
    double vy = formation_speed_ * sin(PI / 3.0);
    double wz = formation_speed_ * sin(PI / 3.0) / formation_length_;

    // 控制乌龟B和C移动
    geometry_msgs::Twist twist;
    twist.linear.x = vx;
    twist.linear.y = vy;
    twist.angular.z = wz;
    pub_['B'].publish(twist);
    pub_['C'].publish(twist);
  }

  // 创建乌龟
  void spawnTurtle(std::string name, double x, double y, double theta) {
    ros::ServiceClient client = nh_.serviceClient<turtlesim::Spawn>('/spawn');
    turtlesim::Spawn srv;
    srv.request.x = x;
    srv.request.y = y;
    srv.request.theta = theta;
    srv.request.name = name;
    if (client.call(srv)) {
      ROS_INFO('Spawned turtle %s', name.c_str());
      pub_[name] = nh_.advertise<geometry_msgs::Twist>('/' + name + '/cmd_vel', 1);
    } else {
      ROS_ERROR('Failed to spawn turtle %s', name.c_str());
    }
  }

private:
  ros::NodeHandle nh_;
  ros::Subscriber sub_;
  std::map<std::string, ros::Publisher> pub_;
  std::string curr_turtle_;
  double formation_length_;
  double formation_speed_;
};

int main(int argc, char** argv) {
  ros::init(argc, argv, 'turtle_formation');
  TurtleFormation tf;
  ros::spin();
  return 0;
}

代码解释

初始化: 代码首先初始化ROS节点，并设置乌龟编队的边长和速度。
创建乌龟: 使用turtlesim::Spawn服务创建三只乌龟，并将它们放置在预设的等边三角形位置。
订阅键盘控制: 订阅/turtle1/cmd_vel话题，接收键盘控制指令。
控制乌龟移动: 在键盘控制回调函数中，根据接收到的键盘指令控制乌龟A移动，并计算出乌龟B和C应该移动的线速度和角速度，以保持等边三角形队形。
发布控制指令: 将计算出的线速度和角速度发布到乌龟B和C对应的/cmd_vel话题，控制它们移动。

运行代码

将代码保存为turtle_formation.cpp文件。
使用catkin编译代码。
在终端中启动 roscore。
在新的终端中运行编译后的节点：rosrun your_package_name turtle_formation
在另一个终端中启动 turtlesim 节点：rosrun turtlesim turtlesim_node
使用键盘控制乌龟A移动，观察乌龟B和C是否保持队形。

希望本教程能够帮助您理解如何使用ROS C++编写代码控制多机器人系统!