我正在制作一个ros节点，以便在1000x1000像素的地图上实现dijkstra的算法。
该地图采用行主导矩阵的形式，我以相同的形式声明了访问，距离和prev。距离存储每个索引的距离，最初用一个巨大的数字声明。访问是一个布尔数组，用于存储该索引是否已被访问。 上一页存储遵循的最短路径。
初始化结构节点，使优先级队列按递增顺序存储节点和距离。
void dijkstra 是在这个程序中执行繁重负载的函数。使用 gdb，此函数中似乎发生了分段错误，但我无法进一步跟踪它。
因此，我需要帮助来理解此运行时错误以及我可能遵循的任何不良做法。
#include <ros/ros.h>
#include <math.h>
#include <queue>
#include <vector>
#include <std_msgs/String.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/OccupancyGrid.h>
#define FMAX 999999999.99
int rows = 1000, columns = 1000, size = rows * columns;
bool visited[1000000];
float distance[1000000];
int prev[1000000];
int source = 15100, destination = 990500; // Random source and destination
int dr[] = {1, -1, 0, 0, 1, 1, -1, -1}; // Direction vectors
int dc[] = {0, 0, 1, -1, 1, -1, 1, -1};
struct node
{
int index;
float dist;
node(int index, float dist)
: index(index), dist(dist)
{
}
};
struct compareDist
{
bool operator()(node const& n1, node const& n2)
{
return n1.dist > n2.dist;
}
};
// Priority queue
std::priority_queue <node, std::vector<node>, compareDist> pq;
int index(int r, int c)
{
return (r * 1000) + c;
}
void init()
{
for(int i = 0; i < size; i++)
{
distance[i] = FMAX;
visited[i] = false;
prev[i] = 9999999;
}
}
float dist_(int index1, int index2)
{
int r1, c1, r2, c2;
r1 = index1 / columns; r2 = index2 / columns;
c1 = index1 - (r1 * 1000); c2 = index2 - (r2 * 1000);
return sqrt(pow(r1 - r2, 2) + pow(c1 - c2, 2));
}
void dijkstra(const nav_msgs::OccupancyGrid& map)
{
prev[source] = 0;
node first = {source, 0.0};
pq.push(first);
while(!pq.empty())
{
node temp = pq.top();
pq.pop();
int nodeIndex = temp.index;
float nodeDist = temp.dist;
visited[nodeIndex] = true;
int r = nodeIndex / columns;
int c = nodeIndex - (r * columns);
int rr, cc;
for(int i = 0; i < 8; i++) // to calculate neighbours
{
rr = r + dr[i];
cc = c + dc[i];
if(map.data[index(rr, cc)] == 100)
visited[index(rr, cc)] = true; // Marking blocked paths as visited
if(rr < 0 || rr >= 1000 || cc < 0 || cc >= 1000 || visited[index(rr, cc)] == true)
continue;
else
{
node neighbour(index(rr, cc), dist_(nodeIndex, index(rr, cc)));
float alt = nodeDist + neighbour.dist;
if(alt < distance[index(rr, cc)])
{
visited[index(rr, cc)] = true;
distance[index(rr, cc)] = alt;
prev[index(rr, cc)] = nodeIndex;
node next(index(rr, cc), alt);
pq.push(next);
}
if(visited[destination] == true)
break;
}
}
if(visited[destination] == true)
break;
}
std::vector <int> path;
// prev contains the path. Trace it back to get the path.
path.push_back(destination);
while(true)
{
path.push_back(prev[path.back()]);
if(path.back() == 0)
break;
}
geometry_msgs::PoseArray poseArray;
poseArray.header.frame_id = "map"; // or other frame you wish to publish relative to.
std::vector<geometry_msgs::Pose> pose_vector;
// push or insert to your vector
for(int i = 0; i < path.size(); i++)
{
geometry_msgs::Pose p;
p.position.x = path.back();
pose_vector.push_back(p);
}
poseArray.poses = pose_vector;
ros::NodeHandle n("~");
ros::Publisher pose_array_pub = n.advertise<geometry_msgs::PoseArray>("/poseArray", 1);
pose_array_pub.publish(poseArray);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "dijkstra");
ros::NodeHandle n("~");
ros::Subscriber sub = n.subscribe("/map", 1000, dijkstra);
init();
distance[source] = 0;
visited[source] = true;
ros::spin();
}
使用调试符号编译代码，然后在调试器中运行它。 这将准确地显示错误发生的位置，并允许您在崩溃时检查程序的状态。