Fixed several things. Add code to nodes to not publish anything to a topic until they receive data from topics they subscribe in order to avoid uninitialized variables. The parameter R_min wasn't being used so I fixed that. Lastly, the function dubins_parameters was calculating 'ell' based of the size of the vectors (cle,crs,cls,cle) instead of their norm.
Fixed several things. Add code to nodes to not publish anything to a topic until they receive data from topics they subscribe in order to avoid uninitialized variables. The parameter R_min wasn't being used so I fixed that. Lastly, the function dubins_parameters was calculating 'ell' based of the size of the vectors (cle,crs,cls,cle) instead of their norm.