CollisionObjects: Allow import of URDFs and update file location finder

[egordon]

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

[egordon]

Update: add 2 functions:

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