Centralize Path Definition Parameters

launch/move_reconstruction_path.launch

@@ -3,6 +3,6 @@
     <!-- Launch Python Scripts for Motion Control & Pose Array Using input JSON -->
     <node pkg="photogrammetry-capture-motion" type="photogrammetry_capture_runner.py" name="motion_input" output="screen" launch-prefix="gnome-terminal --command" />
 
-    <node pkg="photogrammetry-capture-motion" type="rviz_pose_array.py" name="camera_positions_array" output="screen" />
+    <node pkg="photogrammetry-capture-motion" type="rviz_pose_array.py" name="pose_arrray" output="screen" launch-prefix="gnome-terminal --command" />
 
 </launch>

scripts/detected_object.json

@@ -1,7 +1,22 @@
 {
-  "type":"steppedrings",
-  "size":[0.06, 0.04, 0.14],
-  "posn":[0.7, 0, 0.3],
-  "orientation":[[0.866, -0.5, 0],[0.5, 0.866, 0],[0, 0, 1]],
-  "comment":"For steppedrings with rotated camera, use size=[0.06, 0.04, 0.14], posn=[0.06, 0.04, 0.14]"
+  "PathType":"SteppedRings",
+  "ObjectInfo":{
+    "size":[0.06, 0.04, 0.14],
+    "position":[0.7, 0, 0.3],
+    "orientation":[[0.866, -0.5, 0],[0.5, 0.866, 0],[0, 0, 1]]
+  },
+  "SteppedRings": {
+    "scale":null,
+    "offset":null,
+    "level_count": null,
+    "density": null
+  },
+  "InclinedPlanes": {
+    "count":null,
+    "clearance":null,
+    "plane_scale": null,
+    "slope": null,
+    "offset": null
+  },
+  "comment":"For SteppedRings with rotated camera, use size=[0.06, 0.04, 0.14], posn=[0.06, 0.04, 0.14]"
 }

scripts/photogrammetry_capture_runner.py

@@ -4,8 +4,10 @@
 from path_plans import InclinedPlane
 from path_plans import SteppedRings
 from path_plans import OrthogonalCapture
+from utils import get_path_from_json
 import numpy as np
 import geometry_msgs.msg
+import argparse
 import rospy
 import time
 import json
@@ -19,6 +21,10 @@
 
 
 if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description="Input JSON file name")
+    parser.add_argument('-json_name', '--json-name', type=str, default='detected_object.json')
+    args = parser.parse_args()
+    json_name = args.json_name
     try:
         print "----------------------------------------------------------"
         print "            Photogrammetry Image Capture Tool             "
@@ -40,28 +46,24 @@
 
         ## PLANNING ##
         # Example detected object definition
-
-        fname = os.path.join(current, "detected_object.json")
+        fname = os.path.join(current, json_name)
         with open(fname, "r") as read_file:
             detected_object = json.load(read_file)
 
-        object_size = detected_object['size']
-        object_posn = detected_object['posn']
-        orientation = np.array(detected_object['orientation'])
+        path = get_path_from_json(detected_object)
+
+        object_size = detected_object['ObjectInfo']['size']
+        object_posn = detected_object['ObjectInfo']['position']
+        orientation = np.array(detected_object['ObjectInfo']['orientation'])
 
         # Add Object to Collision Planning Space
         robot.add_box_object(object_posn, object_size)
 
-        # create path plan
-        # plan = InclinedPlane(object_size, object_posn, np.identity(3), count=(3,3), slope=0.2, clearance=0.06, offset=0.02)
-        # plan = SteppedRings(object_size, object_posn, orientation, scale=1.01, offset=0.01, level_count=2, density=7)
-        plan = OrthogonalCapture(object_size, object_posn, orientation)
-
         # Attempt Incline Plane Motion
         print("\nEXECUTE INCLINED PLANES RASTER MOTION")
 
         try:
-            for msg in plan.path_as_messages:
+            for msg in path.path_as_messages:
                 robot.goto_Quant_Orient(msg)
                 time.sleep(1)
                 # capture_photo()

scripts/rviz_pose_array.py

@@ -13,6 +13,8 @@
 from path_plans import SteppedRings
 from path_plans import OrthogonalCapture
 from transformations import Transformations
+from utils import get_path_from_json
+import argparse
 import json
 import os
 import rospy
@@ -117,42 +119,26 @@ def node_cameraPoseArray(inputArray):
 
 #####################################################
 ## MAIN CODE ##
-def main():    
-    # Example detected object definition
-    # TODO: Eliniate theis hacky logic and put a proper way to load a desired json file
-    if False: # For manually defining the position in the code
-        tf = Transformations()
-        object_size = [0.06, 0.14, 0.14]
-        object_posn = [0.48, 0.0, 0.32]
+def main():
+    parser = argparse.ArgumentParser(description="Input JSON file name")
+    parser.add_argument('-json_name', '--json-name', type=str, default='detected_object.json')
+    args = parser.parse_args()
 
-        orientation = tf.create_rotation_matrix([0],'z')
+    json_name = args.json_name
+    current = os.path.dirname(os.path.realpath(__file__))
 
-        ## Sample Use: Inclined Plane
-        # demo_blade = InclinedPlane(object_size, object_posn, np.identity(3), count=(3,3), slope=0.2, clearance=0.06, offset=0.02)
-
-        ## Sample Use: Stepped Rings
-        demo_blade = SteppedRings(object_size, object_posn, orientation, scale=1.01, offset=0.1, level_count=2, density=7)
-    else: # for loading from the JSON file
-        current = os.path.dirname(os.path.realpath(__file__))
-        fname = os.path.join(current, "detected_object.json")
-        with open(fname, "r") as read_file:
-            detected_object = json.load(read_file)
-
-        object_size = detected_object['size']
-        object_posn = detected_object['posn']
-        orientation = np.array(detected_object['orientation'])
-
-        if detected_object['type'] == 'steppedrings':
-            demo_blade = SteppedRings(object_size, object_posn, orientation) #, scale=1.01, offset=0.01, level_count=2, density=7)
-        else:
-            print 'Warning: invalid path type in rviz_pose_array.py'
+    fname = os.path.join(current, json_name)
+    with open(fname, "r") as read_file:
+        detected_object = json.load(read_file)
 
+    path = get_path_from_json(detected_object)
+    object_posn = detected_object['ObjectInfo']['position']
 
 
     ## Visualization in RVIZ
     # Generate PoseArray for ROS Node Publisher
     pose_geom = [rosmsg_geoPose([object_posn[0],object_posn[1],object_posn[2],0,0,0,0])]
-    for i in demo_blade.path_as_poses:
+    for i in path.path_as_poses:
         pose_geom.append(rosmsg_geoPose(i))
 
     # Try launching ros node

scripts/utils.py

@@ -0,0 +1,77 @@
+from path_plans import InclinedPlane
+from path_plans import SteppedRings
+from path_plans import OrthogonalCapture
+import numpy as np
+import argparse
+import inspect
+import json
+import sys
+import os
+current = os.path.dirname(os.path.realpath(__file__))
+
+
+def get_path_from_json(structure):
+    """ Accepts a dictionary from a JSON file with information about an object
+    and settings for a path plan and returns the specified path plan.
+
+    Assumes any parameters set to None (null in JSON notation) should be
+    the default value.
+
+    @param structure: dictionary of of a loaded json file.
+    """
+    size = structure['ObjectInfo']['size']
+    position = structure['ObjectInfo']['position']
+    orientation = np.array(structure['ObjectInfo']['orientation'])
+    path_type = structure['PathType']
+    if path_type == 'SteppedRings':
+        a = inspect.getargspec(SteppedRings.__init__)
+        defaults = dict(zip(a.args[-len(a.defaults):],a.defaults))
+
+        opt = structure['SteppedRings']
+        for k in opt.keys():
+            if opt[k] is None:
+                opt[k] = defaults[k]
+
+        path = SteppedRings(size,
+                            position,
+                            orientation,
+                            scale=opt['scale'],
+                            offset=opt['offset'],
+                            level_count=opt['level_count'],
+                            density=opt['density'],
+                            )
+    elif path_type == 'InclinedPlane':
+        a = inspect.getargspec(InclinedPlane.__init__)
+        defaults = dict(zip(a.args[-len(a.defaults):],a.defaults))
+
+        opt = structure['InclinedPlane']
+        for k in opt.keys():
+            if opt[k] is None:
+                opt[k] = defaults[k]
+
+        path = InclinedPlane(size,
+                             position,
+                             orientation,
+                             count=opt['count'],
+                             clearance=opt['clearance'],
+                             plane_scale=opt['plane_scale'],
+                             slope=opt['slope'],
+                             offset=opt['offset'],
+                             )
+    else:
+        path = None
+        exit('[ERROR] Invalid path type in JSON file.')
+
+    return path
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description="Input JSON file name")
+    parser.add_argument('-json_name', '--json-name', type=str, default='detected_object.json')
+    args = parser.parse_args()
+    json_name = args.json_name
+    fname = os.path.join(current, json_name)
+    with open(fname, "r") as read_file:
+        json_structure = json.load(read_file)
+
+    path_plan = get_path_from_json(json_structure)