def __init__(self):
        self.json_file = rospy.get_param('~json', None)
        self.is_apc2016 = rospy.get_param('~is_apc2016', True)
        self.gripper = rospy.get_param('~gripper', 'gripper2016')
        self.max_weight = rospy.get_param('~max_weight', -1)
        if self.json_file is None:
            rospy.logerr('must set json file path to ~json')
            return
        data = json.load(open(self.json_file))
        self.work_order = {}
        work_order_list = []
        for order in data['work_order']:
            bin_ = order['bin'].split('_')[1].lower()
            self.work_order[bin_] = order['item']
            work_order_list.append({'bin': bin_, 'item': order['item']})
        rospy.set_param('~work_order', work_order_list)

        self.object_data = None
        if self.is_apc2016:
            self.object_data = jsk_apc2016_common.get_object_data()

        self.bin_contents = jsk_apc2016_common.get_bin_contents(param='~bin_contents')

        self.msg = self.get_work_order_msg()

        self.pub_left = rospy.Publisher(
            '~left_hand', WorkOrderArray, queue_size=1)
        self.pub_right = rospy.Publisher(
            '~right_hand', WorkOrderArray, queue_size=1)
        rospy.Service('~update_target', UpdateTarget, self._update_target_cb)
        self.updated = False
        rospy.Timer(rospy.Duration(rospy.get_param('~duration', 1)), self._timer_cb)
Exemple #2
0
    def bin_info_publlish(self, event):
        json = rospy.get_param('~json', None)

        # update bin_info_arr only when rosparam: json is changd
        if self.json_file != json:
            if not os.path.isfile(json) or json[-4:] != 'json':
                rospy.logwarn('wrong json file name')
                return
            self.json_file = json

            # get bbox from rosparam
            self.from_shelf_param('upper')
            self.from_shelf_param('lower')

            # get contents of bin from json
            self.bin_contents_dict = jsk_apc2016_common.get_bin_contents(
                self.json_file)
            self.targets_dict = jsk_apc2016_common.get_work_order(
                self.json_file)

            # create bin_msg
            self.create_bin_info_arr()

        self.bin_info_arr.header.stamp = rospy.Time.now()
        self.pub_bin_info_arr.publish(self.bin_info_arr)
Exemple #3
0
 def sort_work_order(self, event):
     bin_contents = jsk_apc2016_common.get_bin_contents(self.output_json_file)
     msg = {}
     target_object = {}
     target_object['left'] = rospy.get_param('/left_hand/target_object')
     target_object['right'] = rospy.get_param('/right_hand/target_object')
     for arm in ['left', 'right']:
         if arm == 'left':
             sorted_bin_list = [x for x in 'abdegj']
         else:
             sorted_bin_list = [x for x in 'cfhikl']
         msg[arm] = WorkOrderArray()
         if target_object[arm] in self.black_list:
             order = WorkOrder(bin="tote", object=target_object[arm])
             msg[arm].array.append(order)
         else:
             if target_object[arm] in self.volume_first:
                 sorted_bin_list = sorted(
                         sorted_bin_list,
                         key=lambda bin_: self.bin_point_dict[bin_],
                         reverse=True
                         )
                 sorted_bin_list = self.sort_by_volume(
                         sorted_bin_list,
                         bin_contents,
                         target_object[arm]
                         )
             else:
                 sorted_bin_list = self.sort_by_volume(
                         sorted_bin_list,
                         bin_contents,
                         target_object[arm]
                         )
                 sorted_bin_list = sorted(
                         sorted_bin_list,
                         key=lambda bin_: self.bin_point_dict[bin_],
                         reverse=True
                         )
             for bin_ in sorted_bin_list:
                 order = WorkOrder(bin=bin_, object=target_object[arm])
                 msg[arm].array.append(order)
         self.pub[arm].publish(msg[arm])
Exemple #4
0
    def __init__(self):
        self.bbox_dict = {}
        self.bin_contents_dict = {}
        self.targets_dict = {}
        self.cam_direction_dict = {}
        self.json_file = None

        pub_bin_info_arr = rospy.Publisher('~bin_array', BinInfoArray, queue_size=1)
        pub_bbox_arr = rospy.Publisher('~bbox_array', BoundingBoxArray, queue_size=1)
        rate = rospy.Rate(rospy.get_param('rate', 1))
        while not rospy.is_shutdown():
            json = rospy.get_param('~json', None)

            # update bin_info_arr only when rosparam: json is changd
            if self.json_file != json:
                if not os.path.isfile(json) or json[-4:] != 'json':
                    rospy.logwarn('wrong json file name')
                    rate.sleep()
                    continue
                self.json_file = json

                # get bbox from rosparam
                self.from_shelf_param('upper')
                self.from_shelf_param('lower')

                # create bounding box array
                self.bbox_array = self.get_bounding_box_array(self.bbox_dict)

                # get contents of bin from json
                self.bin_contents_dict = jsk_apc2016_common.get_bin_contents(self.json_file)
                self.targets_dict = jsk_apc2016_common.get_work_order(self.json_file)

                # create bin_msg
                self.create_bin_info_arr()

            self.bbox_array.header.stamp = rospy.Time.now()
            self.bin_info_arr.header.stamp = rospy.Time.now()
            pub_bbox_arr.publish(self.bbox_array)
            pub_bin_info_arr.publish(self.bin_info_arr)
            rate.sleep()
Exemple #5
0
 def __init__(self):
     rp = rospkg.RosPack()
     self.json_file = rospy.get_param('~json', None)
     self.create_output_json = rospy.get_param('~create_output_json', True)
     if self.create_output_json:
         self.output_json_file = osp.join(
                 rp.get_path('jsk_2016_01_baxter_apc'),
                 'output',
                 'output_' + osp.basename(self.json_file)
                 )
     else:
         self.output_json_file = self.json_file
     self.black_list = rospy.get_param('~black_list', [])
     self.volume_first = rospy.get_param('~volume_first', [])
     self.limit_volume = rospy.get_param('~limit_volume', 3000)
     if self.json_file is None:
         rospy.logerr('must set json file path to ~json')
         return
     if self.output_json_file is None:
         rospy.logerr('must set output json file path to ~output_json')
         return
     self.pub = {}
     self.pub['left'] = rospy.Publisher(
             '~left_hand',
             WorkOrderArray,
             queue_size=1
             )
     self.pub['right'] = rospy.Publisher(
             '~right_hand',
             WorkOrderArray,
             queue_size=1
             )
     self.object_data = jsk_apc2016_common.get_object_data()
     initial_bin_contents = jsk_apc2016_common.get_bin_contents(
             self.json_file
             )
     self.bin_point_dict = {
             k: self._get_point(v) for k, v in initial_bin_contents.items()
             }
Exemple #6
0
    def __init__(self):
        self.json_file = rospy.get_param('~json', None)
        self.is_apc2016 = rospy.get_param('~is_apc2016', True)
        self.gripper = rospy.get_param('~gripper', 'gripper2016')
        self.max_weight = rospy.get_param('~max_weight', -1)
        if self.json_file is None:
            rospy.logerr('must set json file path to ~json')
            return
        data = json.load(open(self.json_file))
        self.work_order = {}
        work_order_list = []
        for order in data['work_order']:
            bin_ = order['bin'].split('_')[1].lower()
            self.work_order[bin_] = order['item']
            work_order_list.append({'bin': bin_, 'item': order['item']})
        rospy.set_param('~work_order', work_order_list)

        self.object_data = None
        if self.is_apc2016:
            self.object_data = jsk_apc2016_common.get_object_data()

        self.bin_contents = jsk_apc2016_common.get_bin_contents(
            param='~bin_contents')

        self.msg = self.get_work_order_msg()

        self.pub_left = rospy.Publisher('~left_hand',
                                        WorkOrderArray,
                                        queue_size=1)
        self.pub_right = rospy.Publisher('~right_hand',
                                         WorkOrderArray,
                                         queue_size=1)
        rospy.Service('~update_target', UpdateTarget, self._update_target_cb)
        self.updated = False
        rospy.Timer(rospy.Duration(rospy.get_param('~duration', 1)),
                    self._timer_cb)
Exemple #7
0
 def __init__(self):
     super(self.__class__, self).__init__()
     json_file = rospy.get_param('~json')
     self.bin_contents = jsk_apc2016_common.get_bin_contents(json_file)
     self.pub = self.advertise('~output', ObjectRecognition, queue_size=1)
 def __init__(self):
     super(self.__class__, self).__init__()
     json_file = rospy.get_param('~json')
     self.bin_contents = jsk_apc2016_common.get_bin_contents(json_file)
     self.pub = self.advertise('~output', ObjectRecognition, queue_size=1)