用c++写ros小乌龟边缘转向代码

在ROS (Robot Operating System) 中编写控制小乌龟边缘转向的C++代码通常会涉及到使用`geometry_msgs/Twist`消息来发送移动命令，并通过`odom`话题监听小乌龟的运动状态。下面是一个简化的例子，展示了如何让小乌龟基于边缘检测来调整方向：

#include <ros/ros.h>
#include < turtlesim/srv/Move.h> // 小乌龟服务
#include <geometry_msgs/Twist.h> // 消息类型
#include <sensor_msgs/LaserScan.h> // 边缘检测传感器

class TurtleEdgeTurn {
public:
    TurtleEdgeTurn()
        : twistPub_(nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 10)) // 发布器
    {}

    void turnToEdge(const sensor_msgs::LaserScan& scan)
    {
        double min_distance = std::numeric_limits<double>::max();
        int edge_direction = -1; // 可能是LEFT (-1), RIGHT (1)

        for (int i = 0; i < scan.ranges.size(); ++i) {
            if (scan.ranges[i] < min_distance && scan.ranges[i] > 0.1) { // 考虑有效的距离
                min_distance = scan.ranges[i];
                if (i < scan.ranges.size() / 2) { // 判断左边还是右边
                    edge_direction = 1;
                } else {
                    edge_direction = -1;
                }
            }
        }

        geometry_msgs::Twist twist;
        twist.linear.x = 0.2; // 速度设置
        twist.angular.z = edge_direction * 0.5; // 转向角度，正转右转，反转左转

        twistPub_.publish(twist); // 发送转向指令
    }

private:
    ros::NodeHandle nh;
    ros::Publisher twistPub_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "turtle_edge_turn");
    TurtleEdgeTurn turtle;

    ros::Rate loop_rate(10); // 控制循环频率

    while (ros::ok()) {
        // 获取激光扫描数据
        sensor_msgs::LaserScan scan = ...; // 使用Subscriber获取并解析实际数据
        turtle.turnToEdge(scan);

        loop_rate.sleep();
    }

    return 0;
}