Updated CMakeLists to incorporate new AStar test. Updated AStarSearchMechanism to accept new type of is_safe function -- to be changed once Object class is incorporated into the core_skill_execution_server.
Added new variable to AStarSearchMechanism for debugging: elements_evaluated_. Tracks how many elements are in the closed set at the end of the search-- a good indicator of search efficiency.

Fixed major bug that persisted from original "MAStar" version: Search heuristic was not actually being used properly, and still isn't in any derivative works. H needs to contain an underestimate of the distance to the goal -- It's unclear how the original value was chosen.