Merge pull request start-jsk#1536 from knorth55/work-order-2016

[jsk_2016_01_baxter_apc] work_order modified to apply for apc2016 objects

Author: wkentaro

jsk_2016_01_baxter_apc/launch/main.launch

@@ -9,7 +9,11 @@
 
   <!-- work order data -->
   <node pkg="jsk_2016_01_baxter_apc" type="work_order.py" name="work_order" respawn="true">
-    <param name="json" value="$(arg json)" />
+    <rosparam>
+        json: $(arg json)
+        is_apc2016: false
+        gripper: gripper2016
+    </rosparam>
   </node>
 
   <!-- parameter -->

jsk_2016_01_baxter_apc/node_scripts/work_order.py

@@ -5,24 +5,42 @@
 import json
 import rospy
 from jsk_2015_05_baxter_apc.msg import WorkOrder, WorkOrderArray
-from jsk_apc2016_common import get_bin_contents, get_work_order
+import jsk_apc2016_common
+from jsk_topic_tools.log_utils import jsk_logwarn
 
+import numpy as np
 
-def get_sorted_work_order(json_file):
+
+def get_sorted_work_order(json_file, gripper, object_data):
     """Sort work order to maximize the score"""
-    bin_contents = get_bin_contents(json_file=json_file)
-    bin_n_contents = dict(map(lambda (bin_, objects): (bin_, len(objects)), bin_contents.iteritems()))
-    sorted_work_order = []
-    work_order = get_work_order(json_file=json_file)
-    for bin_, n_contents in sorted(bin_n_contents.items(), key=lambda x: x[1]):
-        if n_contents > 5:  # Level3
-            continue
-        sorted_work_order.append((bin_, work_order[bin_]))
+    bin_contents = jsk_apc2016_common.get_bin_contents(json_file=json_file)
+    work_order = jsk_apc2016_common.get_work_order(json_file=json_file)
+    sorted_bin_list = bin_contents.keys()
+
+    if object_data is not None:
+        if all(gripper in x for x in [d['graspability'].keys() for d in object_data]):
+            def get_graspability(bin_):
+                target_object = work_order[bin_]
+                target_object_data = [data for data in object_data
+                                      if data['name'] == target_object][0]
+                graspability = target_object_data['graspability'][gripper]
+                return graspability
+            sorted_bin_list = sorted(sorted_bin_list, key=get_graspability)
+        else:
+            jsk_logwarn('Not sorted by graspability')
+            jsk_logwarn('Not all object_data have graspability key: {gripper}'
+                        .format(gripper=gripper))
+    sorted_bin_list = sorted(sorted_bin_list,
+                             key=lambda bin_: len(bin_contents[bin_]))
+    sorted_work_order = [(bin_, work_order[bin_]) for bin_ in sorted_bin_list]
     return sorted_work_order
 
 
-def get_work_order_msg(json_file):
-    work_order = get_sorted_work_order(json_file=json_file)
+def get_work_order_msg(json_file, gripper, max_weight, object_data=None):
+    work_order = get_sorted_work_order(json_file, gripper, object_data)
+    bin_contents = jsk_apc2016_common.get_bin_contents(json_file=json_file)
+    if max_weight == -1:
+        max_weight = np.inf
     msg = dict(left=WorkOrderArray(), right=WorkOrderArray())
     abandon_target_objects = [
         'genuine_joe_plastic_stir_sticks',
@@ -35,30 +53,47 @@ def get_work_order_msg(json_file):
         'rolodex_jumbo_pencil_cup',
         'oreo_mega_stuf'
     ]
-    bin_contents = get_bin_contents(json_file=json_file)
     for bin_, target_object in work_order:
-        if target_object in abandon_target_objects:
-            continue
-        bin_contents_bool = False
-        for bin_object in bin_contents[bin_]:
-            if bin_object in abandon_bin_objects:
-                bin_contents_bool = True
-        if bin_contents_bool:
+        if object_data is not None:
+            target_object_data = [data for data in object_data
+                                  if data['name'] == target_object][0]
+            if target_object_data['weight'] > max_weight:
+                jsk_logwarn('Skipping target {obj} in {bin_}: it exceeds max weight {weight} > {max_weight}'
+                            .format(obj=target_object_data['name'], bin_=bin_,
+                                    weight=target_object_data['weight'], max_weight=max_weight))
+                continue
+        else:
+            if target_object in abandon_target_objects:
+                jsk_logwarn('Skipping target {obj} in {bin_}: it is listed as abandon target'
+                            .format(obj=target_object['name'], bin_=bin_))
+                continue
+            if any(bin_object in abandon_bin_objects for bin_object in bin_contents[bin_]):
+                jsk_logwarn('Skipping {bin_}: this bin contains abandon objects'.format(bin_=bin_))
+                continue
+        if len(bin_contents[bin_]) > 5:  # Level3
+            jsk_logwarn('Skipping {bin_}: this bin contains more than 5 objects'.format(bin_=bin_))
             continue
+        order = WorkOrder(bin=bin_, object=target_object)
         if bin_ in 'abdegj':
-            msg['left'].array.append(WorkOrder(bin=bin_, object=target_object))
+            msg['left'].array.append(order)
         elif bin_ in 'cfhikl':
-            msg['right'].array.append(WorkOrder(bin=bin_, object=target_object))
+            msg['right'].array.append(order)
     return msg
 
 
 def main():
     json_file = rospy.get_param('~json', None)
+    is_apc2016 = rospy.get_param('~is_apc2016', True)
+    gripper = rospy.get_param('~gripper', 'gripper2016')
+    max_weight = rospy.get_param('~max_weight', -1)
     if json_file is None:
         rospy.logerr('must set json file path to ~json')
         return
+    object_data = None
+    if is_apc2016:
+        object_data = jsk_apc2016_common.get_object_data()
 
-    msg = get_work_order_msg(json_file)
+    msg = get_work_order_msg(json_file, gripper, max_weight, object_data)
 
     pub_left = rospy.Publisher('~left_hand',
                                WorkOrderArray,

jsk_2016_01_baxter_apc/test/work_order.test

@@ -2,7 +2,10 @@
   <node pkg="jsk_2016_01_baxter_apc" name="work_order" type="work_order.py" >
     <rosparam subst_value="True">
        json: $(find jsk_apc2016_common)/json/pick_layout_1.json
-       rate: 0.1
+       rate: 10
+       is_apc2016: true
+       gripper: gripper2016
+       max_weight: 5.0
     </rosparam>
   </node>