some question about how to push node into open set in kinodynamic_astar.cpp

[tiemuhua]

In file kinodynamic_astar.cpp, the code to judge whether a node has been in the closed set is: Eigen::Vector3i pro_id = posToIndex(pro_state.head(3)); PathNodePtr pro_node = expandednodes.find(pro_id); The code only consider about the positon of the node while ignore the vecility. Is it possible that, a node in the most optimal path can not be push into the open set because there has been another node with same position and different vecility?

[tiemuhua]

我不知道我表达清楚了没有。 就是代码里面只考虑了某个位置有没有被visited，但是没有考虑速度。 那么有没有可能有一个node1以速度v1经过了当前位置p，（p,v1）这个状态不在最优运动轨迹里面的。 而状态(p,v2)在最优运动轨迹里面，但是按照代码，(p,v2)是没法加到open_set里面的

[ZbyLGsc]

Of course this is possible. We have already stated in the paper that theoretically our kinodynamic astar can not guarantee completeness and optimality. On the one hand, it only searches in the discrete control space, therefore the optimality and completeness is restricted by the resolution. On the other hand, some states in the state lattice induced by motion primitives are pruned away, so states possibly forming a optimal path may be missed. However, given the searched path around the optimum, the optimization refines it and generates a locally optimal trajectory.

I reply in English so that friends from around the world can share our discussion. Besides, I suggest you to comment here in the future (instead of opening more issues), if you only intend to ask some questions but not report really important bugs or issues of the project. :)

[ZbyLGsc]

I will keep this issue open temporarily, feel free to throw more questions.

[tiemuhua]

can you tell me what the function fillESDF do in sdf_map.cpp? no idea at all

[ZbyLGsc]

@tiemuhua The function compute the distance transform along 1 dimension, which operates by finding the lower envelope of a set of parabolas. In combination with updateESDF3d it computes distance transform of the 3D map. The method is detailed in paper 'P. F. Felzenszwalb andD. P.Huttenlocher, “Distance transforms ofsampled functions,” Theory Comput.'

[tiemuhua]

can you tell me what the class RayCaster do? the function rayCast in class RayCaster seems not used

[ZbyLGsc]

RayCaster implement a fast raycasting method for occupancy grid map, which is essential for fusing point cloud into the map. The main procedure is contained in step() function. @tiemuhua