How does Pathfinder's pathfinder_generate work?

What does pathfinder do when you say pathfinder_generate(&candidate, trajectory);?

  • pathfinder creates a hermite spline line and calculates the trajectory of the robot based off the robot distances,
  • in creating that you have to have a lot of different what they call a Segment which includes x, y, heading and movement
  • However, in the function above all the data that you get comes from what they call a TrajectoryCanidate which includes a Spline struct
    and other structs.
  • So what I don’t understand is how does pathfinder fit a series values inside of their one instance of the TrajectoryCanidate struct

I am looking at the method generatePath in okapi’s asyncMotionProfileController(starting with line 53)

I realize that most people won’t care or understand this but it might be a good topic to have on the forum.

1 Like

I believe this is the line references in the question, for context. If you look a bit earlier, on line 123, you’ll see this:

auto *trajectory = static_cast<Segment *>(malloc(length * sizeof(Segment)));

This means to dynamically allocate a block of memory large enough to hold length number of Segment objects, and to treat it as an array of Segment objects.

Here is the implementation of the pathfinder_generate function. If you look inside the for loop, you’ll notice that, for each spline in the trajectory, it fills in the corresponding Segment object in the output.

The TrajectoryCandidate object is able to store multiple values in much the same way. If you look at the definition of the pathfinder_prepare function here, you’ll see that it also allocates two dynamic arrays, and stores their addresses in the saptr and laptr fields of the TrajectoryCandidate object. So the arbitrarily large set of values isn’t stored in the object, rather it’s stored somewhere else in memory in a dynamically allocated array, and the object has a reference to that array.

This is a fairly common pattern in programming in general. In all most programming languages, types have a finite, defined size, and so you can’t store an arbitrarily large number of values inside their objects, so additional dynamically allocated memory is needed. This is how vectors (i.e. array lists), strings, and other containers in C++ and other programming languages are internally implemented.

Hopefully this makes sense.


Oh my goodness. Thank you so much :smile: That makes so much more sense. Thank you. I’m glad that you understood my problem :sweat_smile:

1 Like

Ok so i have another question if you don’t mind.

So I have taken a modified version of the AsyncMotionProfileController's generatePathmethod and kind of changed it a little bit.

However, I want to fill the PosePath struct ( which is declared in the link below) rather than a Segment array.

To do this I tried:

    for( auto &segment: segments ){
        //do something

However it decided to send src/auto/pathGenerator.cpp:101:25: error: 'begin' was not declared in this scope for( auto &segment: segments ){

How do I fix this?

C++'s range-based for loops (the syntax that you’ve used) only work in two scenarios:

  • the container has a begin and end function, returning iterators; these are exposed by all standard library container types
  • the container is a statically sized array

In your case, segments is neither; it is a pointer to a dynamically allocated array, and so the for loop has no way of knowing how long it is (which is kind of important for being able to loop over it).

The solution is to use a “normal” for loop, i.e.:

for (size_t i = 0; i < length; i++) {
  // use segments[i] to refer to the current segment.

Here, you need to replace length with some way of checking the number of segments; based on looking at this code, you can get it from the length field of the TrajectoryCandidate object.

Hopefully this makes sense. If you want to read up more on the range-based for syntax, see the CppReference article. It reads like legalese, but it’s the most comprehensive reference out there for C++ syntax.

Sort-of related: when using type inference (i.e. auto) with a range-based for or anything else in C++, the most general way (which is often considered best practice since it was introduced in C++11) is to use universal references, which are similar to what you put, but use two ampersands instead of one (i.e. auto &&). This blog post explains the reasoning in a lot of detail if you’re interested.

1 Like

ok that makes sense. Thank you again