CollisionObjects: Allow import of URDFs and update file location finder

[Ethan K. Gordon]

Currently, the only available functions for handling CollisionObjects are add_collision_mesh(<path to mesh>) and remove_collision_mesh(<id>)

This interface needs to be expanded:

• remove_collision_mesh(<id>) -> remove_collision(<id>) (deprecate previous)
• get_collision_ids(): return list of all collision object string IDs currently in the planning scene. Use the service /get_planning_scene: setting components.components = WORLD_OBJECT_NAMES and returning [obj.id for obj in scene.world.collision_objects]
• get_collision(<id list, default = []>), return CollisionObjects currently in the planning scene with the specified IDs (default == all collision objects). Use the service /get_planning_scene: setting components.components = WORLD_OBJECT_NAMES & WORLD_OBJECT_GEOMETRY and return [obj for obj in scene.world.collision_objects where obj.id in id_list]
• add_collision(<CollisionObject>): just publishes a raw collision object to the planning scene. Prevents code reuse between all add functions.
• add_collision_primitive([<SolidPrimitive>](http://docs.ros.org/en/noetic/api/shape_msgs/html/msg/SolidPrimitive.html)): Constructs a collision object from a single primitive.
• Create a file path resolver util using pathlib
• • Check parts[0]: resolve file: to /. resolve package: plus the next token to ROS2 package share directory using get_package_share_directory()
• • Call expanduser() to resolve ~
• • Optionally call with list of accepted file extensions, e.g., dae and stl for mesh, urdf for URDF
• • Update add_collision_mesh() to us e the new resolver
• Create add_collision_urdf(<path to urdf>) with new resolver
• • Parse URDF with urdf_parser_py
• • Validation: fixed joints ONLY
• • Start at the link get_root(). Link.origin is CollisionObject.pose is the object frame
• • DFS through tree (via child_map) and recursively calculate the origin of each link in the object frame
• • Loop through all Link collisions, and create the corresponding SolidPrimitive or Mesh object, concat origin with link origin to get pose in object frame
• • Construct CollisionObject out of SolidPrimitive and Mesh objects, add to planning scene

[Ethan K. Gordon]

Update: add 2 functions:

• add_collision_box(name, pose, dims)
• add_collision_sphere(name, pose, radius)
• make sure delete_collision_obj(name) works