car's kinematic state space A star

I am trying to develop a car kinematic state-space using A*. Where the search is applied in kinematic space instead of the grid. The search is using three-state which are (-curve, length) (0.0, length) and (curve, length).

function ComputeEndPosition is used to compute the new position of the next state. the pruscode if:

Explore( startPos, goalPos):

currentState( startPos, parent = None, total_costa = eclidean_distance(startPose, goalPose), 0.001, move_index)

openList.push(currentState)

generated_state = [] // this used to cinstruct the path

while(!openList.empty())

currentState get top State

openList remove top state

generated_state.push_back(currentState)

if (close_state(currentState.pos, goal):

get_path(generated_State)

expand neighbour

for movement in MOVEMENT:

NewPos = ComputeEndPosition (currentState.pos, movemnt[0], movement[1])

if within boundary

currentState( NewPos , currentState.pos, eclidean_distance(NewPos , goalPose), cost+abs(lenght), i)

during the debug, I found that the currentPoint.position in void ExpandNeighbor changed to -107374176.

this is the class State_space.h

#include <iostream>
#include <vector>
#include <queue>
using namespace std;

#define M_PI 3.14159265358979323846264338327950288
struct Point
{
    float x;
    float y;
};

struct Position
{
    float x;
    float y;
    float theta;

    Position(float _x, float _y, float _theta) {
        x = _x;
        y = _y;
        theta = _theta;
    }
    Position() {}
};

class StateSpace
{
public:
    Position pose;
    Position parentPos;

    float total_cost;
    float cost;
    int move_index;

    StateSpace(Position _pos, Position parent_pos, float _total_cost = 0, float _cost = 0, int _move_index = -1) {
        pose.x = _pos.x;
        pose.y = _pos.y;
        pose.theta = _pos.theta;
        parentPos.x = parent_pos.x;;
        parentPos.y = parent_pos.y;
        parentPos.theta = parent_pos.theta;
        total_cost = _total_cost;
        cost = _cost;
        move_index = _move_index;
    }
    StateSpace() {}
};

Position ComputeEndPosition(Position start_posision, float curvature, float length) {

    Position end_position;
    float x = start_posision.x;
    float y = start_posision.y;
    float theta = start_posision.theta;

    if (curvature == 0)
    {
        // linear movement
        x += length * std::cos(theta);
        y += length * std::sin(theta);
        theta = 0;
    }
    else
    {
        // curve segment of raduis 1/ curvature
        float tx = std::cos(theta);
        float ty = std::sin(theta);
        float radius = 1.0 / curvature;
        // Center of circle.
        float xc = x - radius * ty;
        float yc = y + radius * tx;

        float angle = length / radius;
        float cosA = std::cos(angle);
        float sinA = std::sin(angle);

        x = xc + radius * (cosA * ty + sinA * tx);
        y = yc + radius * (sinA * ty - cosA * tx);
        theta = int((theta + angle + M_PI)) % (int)((2.0 * M_PI)) - M_PI;
    }
    end_position.x = x;
    end_position.y = y;
    end_position.theta = theta;

    return end_position;
}

class Point2D {
public:
    float x;
    float y;
};

std::vector<Point2D> Segment_points(Position start_position, float curvature,
    float length, float deltat_length)
{
    // Return points of segment, at delta_length intervals.
    float l = 0.0;
    deltat_length = std::copysign(deltat_length, length);
    std::vector<Point2D> points;


    while (std::abs(l) < std::abs(length))
    {
        Point2D point;
        Position endPosition = ComputeEndPosition(start_position, curvature, l);
        point.x = endPosition.x;
        point.y = endPosition.y;
        points.push_back(point);
    }
    return points;
}


//Allowed movements.These are given as tuples : (curvature, length)
const float MOVEMENTS[3][2] = { {1.0 / 10, 5.0}, {0, 5.0} , {-1.0 / 10, 5.0} };

float Heuristic(Position start, Position goal)
{
    return std::sqrt(std::pow(start.x - goal.x, 2) + std::pow(start.y - goal.y, 2));
}

bool State_close(Position start, Position goal)
{
    // Checks if two poses (x, y, heading) are the same within a tolerance.
    float d_angle = abs(int(start.theta - goal.theta + M_PI) % int(2 * M_PI) - M_PI);
    // For the sake of simplicity, tolerances are hardcoded here :
    // 15 degrees for the heading angle, 2.0 for the position.
    return d_angle < ((15.)* M_PI / 180) and Heuristic(start, goal) <= 4.0;
}

struct CompareDisT {
    bool operator()(StateSpace const& node1, StateSpace const& node2) {
        // f = g + h
        int f_1 = node1.cost + node1.total_cost;
        int f_2 = node2.cost + node2.total_cost;
        return  node1.total_cost > node2.total_cost;
    }
};


void StateSpaceSearch(Position start, Position goal)
{
    std::priority_queue<StateSpace, std::vector<StateSpace>, CompareDisT> openList;
    Position _temp;
    _temp.x = 0;
    _temp.y = 0;
    _temp.theta = 0;
    StateSpace currentState(start, _temp, Heuristic(start, goal), 0.001, -1);
    openList.push(currentState);

    std::vector<StateSpace> generated_state;

    while (!openList.empty())
    {
        currentState = openList.top();
        openList.pop();
        generated_state.push_back(currentState);

        std::cout << "current pos (" << currentState.pose.x << " , " << currentState.pose.y << ")\n";
        std::cout << "parent pos (" << currentState.parentPos.x << " , " << currentState.parentPos.y << ")\n";
        if (State_close(currentState.pose, goal))
        {
            // return path
            std::cout << "[INFO] Path has been found...\n";
            break;
        }

        for (int i = 0; i < 3; i++)
        {
            float curvature = MOVEMENTS[i][0];
            float length = MOVEMENTS[i][1];
            auto newPos = ComputeEndPosition(currentState.pose, curvature, length);

            if (0 <= newPos.x && 0 <= newPos.y && 200 >= newPos.x && 200 >= newPos.y)
            {
                float new_cost = currentState.cost + abs(length);
                float _total_cost = new_cost + Heuristic(newPos, goal);
                StateSpace _state(newPos, currentState.pose, _total_cost, new_cost, i);
                openList.push(_state);
            }
        }
    }
}

int main()
{
    Position start(50.0, 50.0, 0.0);
    Position goal(50, 70, -0.12);

    StateSpaceSearch(start, goal);
    
}