def __init__(self): self.object_publisher = rospy.Publisher( '/art/object_detector/object_filtered', InstancesArray, queue_size=10, latch=True) self.objects = [] obj = ObjInstance() obj.object_id = "profile_20_60" obj.object_type = "profile_20_60" obj.pose = Pose() obj.pose.position.x = 0.3 obj.pose.position.y = 0.3 obj.pose.position.z = 0 obj.pose.orientation.x = 0 obj.pose.orientation.y = 0 obj.pose.orientation.z = 0 obj.pose.orientation.w = 1 self.objects.append(obj) obj = ObjInstance() obj.object_id = "profile_21_60" obj.object_type = "profile_20_60" obj.pose = Pose() obj.pose.position.x = 0.75 obj.pose.position.y = 0.58 obj.pose.position.z = 0 obj.pose.orientation.x = 0 obj.pose.orientation.y = 0 obj.pose.orientation.z = 0 obj.pose.orientation.w = 1 self.objects.append(obj)
def main(): pub = rospy.Publisher("/art/object_detector/object_filtered", InstancesArray, queue_size=10) pub_point_left = rospy.Publisher("/art/user/pointing_left", PoseStamped, queue_size=10) pub_point_right = rospy.Publisher("/art/user/pointing_right", PoseStamped, queue_size=10) arr = InstancesArray() arr.header.frame_id = "marker" obj = ObjInstance() obj.object_id = "3" obj.object_type = "profile_20_60" obj.pose.position.x = 0.5 obj.pose.position.y = 0.5 obj.pose.position.z = 0.08 obj.pose.orientation = yaw2orientation(0) arr.instances.append(obj) obj2 = ObjInstance() obj2.object_id = "4" obj2.object_type = "profile_20_60" obj2.pose.position.x = 0.7 obj2.pose.position.y = 0.2 obj2.pose.position.z = 0.08 obj2.pose.orientation = yaw2orientation(45 / 2) arr.instances.append(obj2) obj3 = ObjInstance() obj3.object_id = "even_another_object" obj3.object_type = "profile_20_60" obj3.pose.position.x = 0.9 obj3.pose.position.y = 0.3 obj3.pose.position.z = 0.08 obj3.pose.orientation = yaw2orientation(45) arr.instances.append(obj3) arr.new_objects.append(obj.object_id) arr.new_objects.append(obj2.object_id) arr.new_objects.append(obj3.object_id) rospy.sleep(1.0) pub.publish(arr) rospy.sleep(1.0) arr.new_objects = [] while (not rospy.is_shutdown()): arr.header.stamp = rospy.Time.now() pub.publish(arr) rospy.sleep(1.0)
def __init__(self): self.object_publisher = rospy.Publisher( '/art/object_detector/object_filtered', InstancesArray, queue_size=10, latch=True) self.srv_set_flag = rospy.Service('/art/object_detector/flag/set', ObjectFlagSet, self.set_cb) self.srv_clear_flag = rospy.Service('/art/object_detector/flag/clear', ObjectFlagClear, self.clear_cb) self.srv_clear_all_flags = rospy.Service( '/art/object_detector/flag/clear_all', Empty, self.empty_cb) self.forearm_cams = ("/l_forearm_cam_optical_frame", "/r_forearm_cam_optical_frame") self.srv_enable_forearm = rospy.Service( '/art/object_detector/forearm/enable', Empty, self.empty_cb) self.srv_disable_forearm = rospy.Service( '/art/object_detector/forearm/disable', Empty, self.empty_cb) self.srv_enable_detection = rospy.Service( '/art/object_detector/all/enable', Empty, self.empty_cb) self.srv_disable_detection = rospy.Service( '/art/object_detector/all/disable', Empty, self.empty_cb) self.objects = [] obj = ObjInstance() obj.object_id = "profile_20_60" obj.object_type = "profile_20_60" obj.pose = Pose() obj.pose.position.x = 0.3 obj.pose.position.y = 0.3 obj.pose.position.z = 0 obj.pose.orientation.x = 0 obj.pose.orientation.y = 0 obj.pose.orientation.z = 0 obj.pose.orientation.w = 1 self.objects.append(obj) obj = ObjInstance() obj.object_id = "profile_21_60" obj.object_type = "profile_20_60" obj.pose = Pose() obj.pose.position.x = 0.75 obj.pose.position.y = 0.58 obj.pose.position.z = 0 obj.pose.orientation.x = 0 obj.pose.orientation.y = 0 obj.pose.orientation.z = 0 obj.pose.orientation.w = 1 self.objects.append(obj)
def get_pick_obj_from_feeder(obj_type): assert isinstance(obj_type, str) obj = ObjInstance() obj.object_id = None obj.object_type = obj_type obj.pose = Pose() return obj
def ar_code_cb(self, data): rospy.logdebug("New arcodes arrived:") instances = InstancesArray() id = 0 for arcode in data.markers: aid = int(arcode.id) # list of allowed object ids # TODO load from param if aid not in [4, 5, 3, 21, 26, 31]: continue if aid not in self.objects_cache: # TODO AR code ID / object type assignment should be done somewhere... # type "profile_20_60" is just for example (used in art_db/test_db.py) if aid == 21: object_type = self.art.get_object_type( "profile_20_60_longer") else: object_type = self.art.get_object_type("profile_20_60") if object_type is None: # error or unknown object - let's ignore it self.objects_cache[aid] = None continue self.objects_cache[aid] = { 'type': object_type.name, 'bb': object_type.bbox } # skip unknown objects if self.objects_cache[aid] is None: continue obj_in = ObjInstance() obj_in.object_id = str(aid) obj_in.object_type = self.objects_cache[aid]['type'] obj_in.pose = arcode.pose.pose self.show_rviz_bb(arcode.header.frame_id, obj_in, arcode.id, self.objects_cache[aid]['bb']) instances.header.stamp = arcode.header.stamp instances.header.frame_id = arcode.header.frame_id instances.instances.append(obj_in) ++id if len(data.markers) == 0: rospy.logdebug("Empty") else: # print instances self.detected_objects_pub.publish(instances)
def ar_code_cb(self, data): rospy.logdebug("New arcodes arrived:") instances = InstancesArray() id = 0 for arcode in data.markers: aid = int(arcode.id) if aid not in self.objects_cache: try: resp = self.get_object_srv(obj_id=aid) except rospy.ServiceException, e: # error or unknown object - let's ignore it self.objects_cache[aid] = None continue self.objects_cache[aid] = {'name': resp.name, 'model_url': resp.model_url, 'type': resp.type, 'bb': resp.bbox} if self.objects_cache[aid] is None: continue obj_in = ObjInstance() obj_in.object_id = self.objects_cache[aid]['name'] obj_in.object_type = self.objects_cache[aid]['type'] obj_in.pose = arcode.pose.pose angles = transformations.euler_from_quaternion([obj_in.pose.orientation.x, obj_in.pose.orientation.y, obj_in.pose.orientation.z, obj_in.pose.orientation.w]) q = transformations.quaternion_from_euler(0, 0, angles[2]) obj_in.pose.orientation.x = q[0] obj_in.pose.orientation.y = q[1] obj_in.pose.orientation.z = q[2] obj_in.pose.orientation.w = q[3] obj_in.bbox = self.objects_cache[aid]['bb'] obj_in.pose.position.z = float(obj_in.bbox.dimensions[2]/2) self.show_rviz_bb(obj_in, arcode.id) instances.header.stamp = arcode.header.stamp instances.header.frame_id = arcode.header.frame_id instances.instances.append(obj_in) ++id
def timer_cb(self, event): ia = InstancesArray() ia.header.frame_id = self.target_frame ia.header.stamp = rospy.Time.now() objects_to_prune = [] now = rospy.Time.now() for k, v in self.objects.iteritems(): if (now - v["pose"].header.stamp) > self.max_age: objects_to_prune.append(k) ia.lost_objects.append(k) continue if v["cnt"] < self.min_cnt: continue obj = ObjInstance() obj.pose = v["pose"].pose obj.pose.orientation = self.normalize(obj.pose.orientation) obj.object_id = k obj.object_type = v["object_type"] ia.instances.append(obj) if v["cnt"] == self.min_cnt: ia.new_objects.append(k) # TODO also publish TF for each object??? self.pub.publish(ia) for k in objects_to_prune: rospy.loginfo("Object " + k + " no longer visible") del self.objects[k] if len(objects_to_prune) > 0: rospy.loginfo("Pruned " + str(len(objects_to_prune)) + " objects")
def get_pick_obj_from_feeder(instruction): obj = ObjInstance() obj.object_id = None obj.object_type = instruction.object[0] obj.pose = Pose() return obj
def inst(self, table_size, ground_objects_on_table=False, ground_bb_axis=SolidPrimitive.BOX_Z, yaw_only_on_table=False): inst = ObjInstance() inst.object_id = self.object_id inst.object_type = self.object_type.name w = [] px = [] py = [] pz = [] r = [] p = [] y = [] for frame_id, val in self.meas.iteritems(): if len(val) < 2: continue for idx in range(0, len(val)): v = val[idx] # distance normalized to 0, 1 d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist) # weight based on distance from object to sensor w_dist = (1.0 - d) ** 2 # (0, 1) # newer detections are more interesting w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1) w.append(w_dist * w_age) px.append(v[1].pose.position.x) py.append(v[1].pose.position.y) pz.append(v[1].pose.position.z) rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation)) r.append([cos(rpy[0]), sin(rpy[0])]) p.append([cos(rpy[1]), sin(rpy[1])]) y.append([cos(rpy[2]), sin(rpy[2])]) if len(w) < self.min_meas_cnt: return None inst.pose.position.x = np.average(px, weights=w) inst.pose.position.y = np.average(py, weights=w) inst.on_table = 0 < inst.pose.position.x < table_size[0] and 0 < inst.pose.position.y < table_size[1] ar = np.average(r, axis=0, weights=w) ap = np.average(p, axis=0, weights=w) ay = np.average(y, axis=0, weights=w) fr = atan2(ar[1], ar[0]) fp = atan2(ap[1], ap[0]) fy = atan2(ay[1], ay[0]) cur_rpy = [fr, fp, fy] q_arr = transformations.quaternion_from_euler(*cur_rpy) # ground objects that are really sitting on the table (exclude those in the air) if inst.on_table and ground_objects_on_table and np.average( pz, weights=w) < self.object_type.bbox.dimensions[ground_bb_axis] / 2.0 + 0.1: # TODO consider orientation! inst.pose.position.z = self.object_type.bbox.dimensions[ground_bb_axis] / 2.0 if yaw_only_on_table: # TODO figure out which axis should be kept # ...like this it only works for some objects (containers) q_arr[0] = 0.0 q_arr[1] = 0.0 q_arr = transformations.unit_vector(q_arr) else: inst.pose.position.z = np.average(pz, weights=w) a2q(inst.pose.orientation, q_arr) for (key, value) in self.flags.iteritems(): kv = KeyValue() kv.key = key kv.value = value inst.flags.append(kv) return inst
def ar_code_cb(self, data): rospy.logdebug("New arcodes arrived:") instances = InstancesArray() id = 0 for arcode in data.markers: aid = int(arcode.id) # list of allowed object ids # TODO load from param if aid not in [ 50, 51, 52, 53, 54, 55, 56, 57, 60, 61, 62, 63, 1001, 1002, 1003, 1004, 1005, 1006, 1007, 1008, 1009, 1010, 1011, 1012, 1013, 1014, 1015, 1016, 1017, 1018, 1019, 1020, 1021, 1022, 1023, 1024, 1025, 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008, 3001, 3002, 3003, 3004 ]: continue if aid not in self.objects_cache: # TODO AR code ID / object type assignment should be done somewhere... # type "profile_20_60" is just for example (used in art_db/test_db.py) object_type = None if aid in [ 50, 51, 52, 53, 54, 55, 56, 57, 1001, 1002, 1003, 1004, 1005, 1006, 1007, 1008, 1009, 1010, 1011, 1012, 1013, 1014, 1015, 1016, 1017, 1018, 1019, 1020, 1021, 1022, 1023, 1024, 1025 ]: object_type = self.art.get_object_type("Spojka") elif aid in [ 60, 61, 62, 63, 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008 ]: object_type = self.art.get_object_type("Kratka_noha") elif aid in [60, 61, 62, 63, 3001, 3002, 3003, 3004]: object_type = self.art.get_object_type("Dlouha_noha") if object_type is None: # error or unknown object - let's ignore it self.objects_cache[aid] = None continue self.objects_cache[aid] = { 'type': object_type.name, 'bb': object_type.bbox } # skip unknown objects if self.objects_cache[aid] is None: continue obj_in = ObjInstance() obj_in.object_id = str(aid) obj_in.object_type = self.objects_cache[aid]['type'] obj_in.pose = arcode.pose.pose self.show_rviz_bb(arcode.header.frame_id, obj_in, arcode.id, self.objects_cache[aid]['bb']) instances.header.stamp = arcode.header.stamp instances.header.frame_id = arcode.header.frame_id instances.instances.append(obj_in) ++id if len(data.markers) == 0: rospy.logdebug("Empty") else: # print instances self.detected_objects_pub.publish(instances)
def inst(self): inst = ObjInstance() inst.object_id = self.object_id inst.object_type = self.object_type w = [] px = [] py = [] pz = [] r = [] p = [] y = [] for frame_id, val in self.meas.iteritems(): if len(val) < 2: continue for idx in range(0, len(val)): v = val[idx] # distance normalized to 0, 1 d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist) # weight based on distance from object to sensor w_dist = (1.0 - d) ** 2 # (0, 1) # newer detections are more interesting w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1) w.append(w_dist * w_age) px.append(v[1].pose.position.x) py.append(v[1].pose.position.y) pz.append(v[1].pose.position.z) rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation)) r.append([cos(rpy[0]), sin(rpy[0])]) p.append([cos(rpy[1]), sin(rpy[1])]) y.append([cos(rpy[2]), sin(rpy[2])]) if len(w) < self.min_meas_cnt: return None inst.pose.position.x = np.average(px, weights=w) inst.pose.position.y = np.average(py, weights=w) inst.pose.position.z = np.average(pz, weights=w) ar = np.average(r, axis=0, weights=w) ap = np.average(p, axis=0, weights=w) ay = np.average(y, axis=0, weights=w) fr = atan2(ar[1], ar[0]) fp = atan2(ap[1], ap[0]) fy = atan2(ay[1], ay[0]) cur_rpy = [fr, fp, fy] # TODO hysteresis! # TODO is there a smarter way how to do it? # "fix" roll and pitch so they are only 0, 90, 180 or 270 degrees # yaw (in table coordinates) may stay as it is for i in range(0, 2): if cur_rpy[i] < 0.0: cur_rpy[i] = 2 * pi + cur_rpy[i] if cur_rpy[i] >= 7 * pi / 4 or cur_rpy[i] < pi / 4: cur_rpy[i] = 0.0 elif cur_rpy[i] >= pi / 4 and cur_rpy[i] < 3 * pi / 4: cur_rpy[i] = pi / 2 elif cur_rpy[i] >= 3 * pi / 4 and cur_rpy[i] < 5 * pi / 4: cur_rpy[i] = pi elif cur_rpy[i] >= 5 * pi / 4 and cur_rpy[i] < 7 * pi / 4: cur_rpy[i] = 3 * pi / 2 a2q(inst.pose.orientation, transformations.quaternion_from_euler(*cur_rpy)) return inst
def main(): pub = rospy.Publisher("/art/object_detector/object_filtered", InstancesArray, queue_size=10) pub_point_left = rospy.Publisher("/art/user/pointing_left", PoseStamped, queue_size=10) pub_point_right = rospy.Publisher("/art/user/pointing_right", PoseStamped, queue_size=10) arr = InstancesArray() arr.header.frame_id = "marker" arr.header.stamp = rospy.Time.now() obj = ObjInstance() obj.object_id = "my_object" obj.object_type = "object" obj.pose.position.x = 0.5 obj.pose.position.y = 0.5 obj.pose.position.z = 0.0 obj.pose.orientation.x = 0.0 obj.pose.orientation.y = 0.0 obj.pose.orientation.z = 0.0 obj.pose.orientation.w = 1.0 obj.bbox = SolidPrimitive() obj.bbox.type = SolidPrimitive.BOX obj.bbox.dimensions.append(0.05) obj.bbox.dimensions.append(0.05) obj.bbox.dimensions.append(0.2) arr.instances.append(obj) obj2 = ObjInstance() obj2.object_id = "another_object" obj2.object_type = "object" obj2.pose.position.x = 0.7 obj2.pose.position.y = 0.3 obj2.pose.position.z = 0.0 obj2.pose.orientation.x = 0.0 obj2.pose.orientation.y = 0.0 obj2.pose.orientation.z = 0.0 obj2.pose.orientation.w = 1.0 obj2.bbox = SolidPrimitive() obj2.bbox.type = SolidPrimitive.BOX obj2.bbox.dimensions.append(0.05) obj2.bbox.dimensions.append(0.05) obj2.bbox.dimensions.append(0.2) arr.instances.append(obj2) arr.new_objects.append(obj.object_id) arr.new_objects.append(obj2.object_id) ps = PoseStamped() ps.header.stamp = rospy.Time.now() ps.header.frame_id = "marker" ps.pose.position.x = 0.0 ps.pose.position.y = 0.5 ps.pose.position.z = 0 ps.pose.orientation.x = 0.0 ps.pose.orientation.y = 0.0 ps.pose.orientation.z = 0.0 ps.pose.orientation.w = 1.0 psr = PoseStamped() psr.header.stamp = rospy.Time.now() psr.header.frame_id = "marker" psr.pose.position.x = 0.0 psr.pose.position.y = 0.3 psr.pose.position.z = 0 psr.pose.orientation.x = 0.0 psr.pose.orientation.y = 0.0 psr.pose.orientation.z = 0.0 psr.pose.orientation.w = 1.0 noise = 0.0001 rospy.sleep(2.0) pub.publish(arr) rospy.sleep(1.0) arr.new_objects = [] while(not rospy.is_shutdown()): pub.publish(arr) rospy.sleep(1.0) while(not rospy.is_shutdown()): if psr.pose.position.x < 0.8: psr.pose.position.x += 0.003 else: psr.pose.position.x = 0 psr.pose.position.y = 0.3 pub_point_right.publish(psr) if ps.pose.position.x < 0.8: ps.pose.position.x += 0.002 else: ps.pose.position.x = 0 ps.pose.position.y = 0.5 if isclose(ps.pose.position.x, 0.5): for i in range(0, 300): pub.publish(arr) pub_point_left.publish(ps) rospy.sleep(0.01) if isclose(ps.pose.position.x, 0.6): for i in range(0, 300): pub.publish(arr) pub_point_left.publish(ps) rospy.sleep(0.01) pub.publish(arr) pub_point_left.publish(ps) rospy.sleep(0.01)
def inst(self): inst = ObjInstance() inst.object_id = self.object_id inst.object_type = self.object_type w = [] px = [] py = [] pz = [] r = [] p = [] y = [] for frame_id, val in self.meas.iteritems(): if len(val) < 2: continue for idx in range(0, len(val)): v = val[idx] # distance normalized to 0, 1 d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist) # weight based on distance from object to sensor w_dist = (1.0 - d) ** 2 # (0, 1) # newer detections are more interesting w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1) w.append(w_dist * w_age) px.append(v[1].pose.position.x) py.append(v[1].pose.position.y) pz.append(v[1].pose.position.z) rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation)) r.append([cos(rpy[0]), sin(rpy[0])]) p.append([cos(rpy[1]), sin(rpy[1])]) y.append([cos(rpy[2]), sin(rpy[2])]) if len(w) < self.min_meas_cnt: return None inst.pose.position.x = np.average(px, weights=w) inst.pose.position.y = np.average(py, weights=w) inst.pose.position.z = np.average(pz, weights=w) # TODO read table size from param inst.on_table = 0 < inst.pose.position.x < 1.5 and 0 < inst.pose.position.x < 0.7 ar = np.average(r, axis=0, weights=w) ap = np.average(p, axis=0, weights=w) ay = np.average(y, axis=0, weights=w) fr = atan2(ar[1], ar[0]) fp = atan2(ap[1], ap[0]) fy = atan2(ay[1], ay[0]) cur_rpy = [fr, fp, fy] a2q(inst.pose.orientation, transformations.quaternion_from_euler(*cur_rpy)) # TODO "fix" roll and pitch so they are only 0, 90, 180 or 270 degrees # yaw (in table coordinates) may stay as it is for (key, value) in self.flags.iteritems(): kv = KeyValue() kv.key = key kv.value = value inst.flags.append(kv) return inst
def main(): pub = rospy.Publisher("/art/object_detector/object_filtered", InstancesArray, queue_size=10) pub_point_left = rospy.Publisher("/art/user/pointing_left", PoseStamped, queue_size=10) pub_point_right = rospy.Publisher("/art/user/pointing_right", PoseStamped, queue_size=10) br = tf.TransformBroadcaster() arr = InstancesArray() arr.header.frame_id = "marker" obj = ObjInstance() obj.object_id = "3" obj.object_type = "profile_20_60" obj.pose.position.x = 0.5 obj.pose.position.y = 0.5 obj.pose.position.z = 0.025 obj.pose.orientation = yaw2orientation(90, 0, 0) arr.instances.append(obj) obj2 = ObjInstance() obj2.object_id = "4" obj2.object_type = "profile_20_60" obj2.pose.position.x = 0.7 obj2.pose.position.y = 0.2 obj2.pose.position.z = 0.08 obj2.pose.orientation = yaw2orientation(0, 0, 45 / 2) arr.instances.append(obj2) obj3 = ObjInstance() obj3.object_id = "even_another_object" obj3.object_type = "profile_20_60" obj3.pose.position.x = 0.9 obj3.pose.position.y = 0.3 obj3.pose.position.z = 0.025 obj3.pose.orientation = yaw2orientation(90, 90, 45) arr.instances.append(obj3) arr.new_objects.append(obj.object_id) arr.new_objects.append(obj2.object_id) arr.new_objects.append(obj3.object_id) rospy.sleep(1.0) pub.publish(arr) rospy.sleep(1.0) arr.new_objects = [] while not rospy.is_shutdown(): arr.header.stamp = rospy.Time.now() pub.publish(arr) for obj in arr.instances: br.sendTransform((obj.pose.position.x, obj.pose.position.y, obj.pose.position.z), (obj.pose.orientation.x, obj.pose.orientation.y, obj.pose.orientation.z, obj.pose.orientation.w), arr.header.stamp, "object_id_" + obj.object_id, "marker") rospy.sleep(0.25)