c++代码实现实现地图路径碰撞检测接口的ros功能包

以下是一个简单的 C++ 代码示例，用于实现地图路径碰撞检测接口的ROS功能包：

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <vector>

using namespace std;

// 定义点的结构体
struct Point {
    double x;
    double y;
};

// 定义地图类
class Map {
public:
    Map(vector<Point> &path) : path_(path) {}

    // 判断点是否在路径上，如果在返回 true，否则返回 false
    bool isCollision(Point &p) {
        for (int i = 0; i < path_.size() - 1; ++i) {
            if (isPointOnLine(path_[i], path_[i + 1], p)) {
                return true;
            }
        }
        return false;
    }

private:
    vector<Point> path_;

    // 判断点是否在直线上
    bool isPointOnLine(Point &p1, Point &p2, Point &p) {
        // 如果点的横坐标小于直线两个端点的横坐标的最小值或大于最大值，那么点一定不在直线上
        if (p.x < min(p1.x, p2.x) || p.x > max(p1.x, p2.x)) {
            return false;
        }
        // 如果点的纵坐标小于直线两个端点的纵坐标的最小值或大于最大值，那么点一定不在直线上
        if (p.y < min(p1.y, p2.y) || p.y > max(p1.y, p2.y)) {
            return false;
        }
        // 如果点不在直线两个端点的纵坐标之间，那么点一定不在直线上
        if ((p.y - p1.y) * (p2.x - p1.x) != (p2.y - p1.y) * (p.x - p1.x)) {
            return false;
        }
        return true;
    }
};

class MapCollisionDetector {
public:
    MapCollisionDetector(ros::NodeHandle &nh) {
        // 初始化地图路径
        vector<Point> path = {{1.0, 1.0}, {2.0, 2.0}, {3.0, 3.0}, {4.0, 4.0}};

        // 创建地图实例
        map_ = Map(path);

        // 创建点订阅器
        point_sub_ = nh.subscribe("point", 10, &MapCollisionDetector::pointCallback, this);

        // 创建结果发布器
        result_pub_ = nh.advertise<geometry_msgs::PointStamped>("result", 10);
    }

private:
    Map map_;
    ros::Subscriber point_sub_;
    ros::Publisher result_pub_;

    // 回调函数，处理接收到的点
    void pointCallback(const geometry_msgs::PointStamped::ConstPtr &msg) {
        Point p = {msg->point.x, msg->point.y};
        bool collision = map_.isCollision(p);

        // 创建结果消息并发布
        geometry_msgs::PointStamped result_msg;
        result_msg.header.stamp = ros::Time::now();
        result_msg.point.x = p.x;
        result_msg.point.y = p.y;
        result_msg.point.z = collision ? 1 : 0;
        result_pub_.publish(result_msg);
    }
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "map_collision_detector");
    ros::NodeHandle nh;

    MapCollisionDetector detector(nh);

    ros::spin();

    return 0;
}

该代码首先定义了一个点的结构体，然后定义了一个地图类，该类包含一个路径向量，并且实现了一个`isCollision`方法，用于判断一个给定的点是否在路径上。接下来，该代码实现了一个`MapCollisionDetector`类，该类包含一个地图实例、一个点订阅器和一个结果发布器。该类在构造函数中初始化了地图实例、创建了点订阅器和结果发布器，并且将点订阅器的回调函数设置为`pointCallback`方法。回调函数将接收到的点转化为地图上的点，并调用`isCollision`方法判断该点是否在路径上，最后将结果发布到结果发布器上。最后，该代码在`main`函数中初始化了ROS节点，并创建了一个`MapCollisionDetector`实例，然后进入ROS循环。