[Feature] Add Grid-based pathfinding

[chrehall68]

• Should create a pathfinder to pathfind from current position to goal given that both are Point objects.
• Should listen to the map (occupancy grid) and output a Path message
• Assume that the grid contains both the current point and the target point
• Implementation should be fast enough to deal with a 4000x4000 grid in < 1s, preferable <.25s. This may require making this package in c++
• This should be its own package

[RK22000]

Relevent message descriptions

The map (on topic "\map") will be an Occupancy grid

Occupancy grid

std_msgs/Header header
nav_msgs/MapMetaData info
int8[] data

Header

# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data 
# in a particular coordinate frame.
# 
# sequence ID: consecutively increasing ID 
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
string frame_id

MapMetaData

time map_load_time
float32 resolution #meters/cell
uint32 width
uint32 height
geometry_msgs/Pose origin

Pose

geometry_msgs/Point position
geometry_msgs/Quaternion orientation

Point

float64 x
float64 y
float64 z

Quaternion

float64 x
float64 y
float64 z
float64 w