def _send_request(self): request = RecognizedObjectArray() # Dummy obj 1 rec_object_1 = RecognizedObject() rec_object_1.type.key = "box" rec_object_1.pose.pose.pose.position.x = 0.1 rec_object_1.pose.pose.pose.position.y = 0.7 rec_object_1.pose.pose.pose.position.z = 0 rec_object_1.pose.pose.pose.orientation.x = 0.707 rec_object_1.pose.pose.pose.orientation.y = -0.707 rec_object_1.pose.pose.pose.orientation.z = 0 rec_object_1.pose.pose.pose.orientation.w = 0 # Dummy obj 2 rec_object_2 = RecognizedObject() rec_object_2.type.key = "box" rec_object_2.pose.pose.pose.position.x = 0.3 rec_object_2.pose.pose.pose.position.y = 1 rec_object_2.pose.pose.pose.position.z = 0 rec_object_2.pose.pose.pose.orientation.x = 0.707 rec_object_2.pose.pose.pose.orientation.y = 0 rec_object_2.pose.pose.pose.orientation.z = 0 rec_object_2.pose.pose.pose.orientation.w = 0.707 request.objects = [rec_object_1, rec_object_2] self._request_sim(request)
def ar_marker_callback(self, msg): objects_msg = RecognizedObjectArray() objects_msg.header = copy.deepcopy(msg.header) for marker in msg.markers: if marker.id < len( defined_objects) and marker.pose.pose.position.z > 0.3: object = RecognizedObject() object.pose = self.get_pose_from_pose(marker.pose) #object.pose.pose.pose.orientation.w = 1.0 object.pose.header = copy.deepcopy(marker.header) #object.pose.header.frame_id = "ar_marker_" + str(marker.id) object.type.key = defined_objects[marker.id]["name"] objects_msg.objects.append(object) self.ar_object_publisher.publish(objects_msg)
def execute(self, userdata): rospy.loginfo("DETECT: Wait for spotting object..") recognized_object_array = RecognizedObjectArray() while 1: try: recognized_object_array = rospy.wait_for_message('/recognized_object_array', RecognizedObjectArray, timeout=2) recognized_object = str(recognized_object_array.objects) except rospy.ROSException as e: if 'timeout exceeded' in e.message: rospy.loginfo('no object detected! ') continue # no object detected, continue with trying else: raise e index_pos = 0 index_pos = recognized_object.find("position", index_pos) if index_pos > 0: rospy.loginfo("DETECT: Spot an Object") break; rospy.loginfo("DETECT: SUCCEEDED") return 'succeeded'
def __init__(self): self.publisher = rospy.Publisher("recognized_objects", RecognizedObjectArray, queue_size=1, latch=True) self.regex_to_type = { "^utl5_small_": "utl5_small", "^utl5_medium_": "utl5_medium", "^utl5_large_": "utl5_large", "^duplo_2x4x1_": "duplo_2x4x1" } self.recognized_object_array = RecognizedObjectArray() self.update_recognized_objects_array()
def run(self): ''' Main run function that constructs the recognised object array message from the matched object list and noise params. :return: ''' rate = rospy.Rate(30) while not rospy.is_shutdown(): try: self.matched_object_lock.acquire() object_array = RecognizedObjectArray() object_array.header.stamp = rospy.Time.now() object_array.header.frame_id = self.camera_link_name for object in self.matched_object_list: object_msg = RecognizedObject() object_pose = PoseWithCovarianceStamped() object_pose.header.stamp = rospy.Time.now() object_pose.header.frame_id = self.camera_link_name object_pose.pose.pose = posemath.toMsg(self.cam_trans * posemath.fromMsg(object[0]) * \ self.generate_noise_trans()) object_msg.pose = object_pose object_msg.type.key = object[1] object_array.objects.append(object_msg) # publish tf trans = TransformStamped() trans.header.stamp = rospy.Time.now() trans.header.frame_id = self.camera_link_name trans.child_frame_id = object[1] trans.transform.translation = object_pose.pose.pose.position trans.transform.rotation = object_pose.pose.pose.orientation self.tf_pub.sendTransform(trans) self.object_publisher.publish(object_array) except rospy.exceptions.ROSInterruptException: break finally: self.matched_object_lock.release() rate.sleep()
def execute(self, goal): """Action server callback. Callback gets called in a separate thread whenever a new goal is received. Args: goal (ObjectRecognitionGoal): action goal """ if goal.use_roi: rospy.logwarn('ROI parameter is not supported.') try: image = self._camera.image(desired_encoding='rgb8', timeout=1.0) # TODO: enable tracking mode (reset=False) objects = self._predictor.predict(image, reset=True) rospy.logdebug('Recognized: %s', ','.join([label for label, _ in objects])) # publish debug info self._publish_markers(objects) self._publish_overlay(image, objects) header = Header(stamp=rospy.Time.now(), frame_id=self._frame_id) roa = RecognizedObjectArray(header=header) for i, (label, pose) in enumerate(objects): ro = RecognizedObject(header=header) ro.type.db = self._predictor.dataset_name ro.type.key = label ro.confidence = self._predictor.get_score(i) ro.pose.header = header ro.pose.pose.pose = _pose_to_msg(pose) roa.objects.append(ro) self._array_publisher.publish(roa) result = ObjectRecognitionResult(recognized_objects=roa) self._server.set_succeeded(result) except Exception as ex: rospy.logerr('Recognition failed: %s', ex) rospy.logdebug(traceback.format_exc()) self._server.set_aborted(text=str(ex))
def execute_callback(self, goal): now = rospy.Time.now() r_array = RecognizedObjectArray() r_array.header.stamp = now r_array.header.frame_id = self.frame_id if goal.use_roi: raise Warning("use_roi in ObjectRecognitionRequest ignored") for o in self.object_names: try: object_tf = self.listener.lookupTransform( "/world", o, rospy.Time(0)) print o print object_tf print "" p = PoseWithCovarianceStamped() p.pose.pose = rox_msg.transform2pose_msg(object_tf) p.header.stamp = now p.header.frame_id = self.frame_id r = RecognizedObject() r.header.stamp = now r.header.frame_id = self.frame_id r.type.key = o r.confidence = 1 r.pose = p r_array.objects.append(r) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue result = ObjectRecognitionResult() result.recognized_objects = r_array self.server.set_succeeded(result=result)
def execute(self, goal): rospy.loginfo("Executing the server -- Goal Detected") result = RecognizeResult() possibleObjects = RecognizedObject() object_goal = goal.objects.pop() rospy.loginfo("Goal Received:: " + str(goal)) rospy.loginfo("One of the Goals are:: " + str(object_goal)) possibleObjects.type = object_goal rospy.loginfo("PossibleObject:: " + str(possibleObjects)) objectArray = RecognizedObjectArray() objectArray.objects.append(possibleObjects) result.recognized_objects = objectArray rospy.loginfo("Results that are sending to Object Detection SM :: " + str(result)) rospy.sleep(goal.refine_pose_time) self.server.set_succeeded(result)
def calback_gazebo_state(self, msg): ''' Callback function for getting the model states from gazebo. Function gets all the model states then matches the names to the object list. :param msg: gazebo model states :return: ''' try: self.matched_object_lock.acquire() self.matched_object_list = [] object_array = RecognizedObjectArray() object_array.header.stamp = rospy.Time.now() object_array.header.frame_id = self.camera_link_name for object_name in self.object_names: try: index = msg.name.index(object_name) self.matched_object_list.append( [msg.pose[index], msg.name[index]]) except ValueError: continue finally: self.matched_object_lock.release()
def record_frame_list(self, frame_list): recognized_object_array = RecognizedObjectArray() start_time = rospy.get_time() while (rospy.get_time() - start_time) < self.time_to_measure and len(frame_list) < self.max_objects: try: recognized_object_array = rospy.wait_for_message('/recognized_object_array', RecognizedObjectArray, timeout=2) recognized_object = str(recognized_object_array.objects) except rospy.ROSException as e: if 'timeout exceeded' in e.message: rospy.loginfo('no object detected! ') continue # no object detected, continue with trying else: raise e index = 0 index_pos = 1 while index_pos > 0: index_pos = recognized_object.find("position", index) index_ori = recognized_object.find("orientation", index) index_cov = recognized_object.find("covariance", index) index = index + index_pos # rospy.loginfo('index: \n{}' .format(index)) # rospy.loginfo('index_pos: \n{}' .format(index_pos)) object_pose = PoseStamped() object_pose.header.frame_id = "camera_rgb_optical_frame" if index_pos > 0: obj_pos = str(recognized_object[index_pos:index_ori]) obj_ori = str(recognized_object[index_ori:index_cov]) match = re.findall(r'-?\d+\.?\d*', str(obj_pos)) object_pose.pose.position.x = float(match[0]) object_pose.pose.position.y = float(match[1]) object_pose.pose.position.z = float(match[2]) match = re.findall(r'-?\d+\.?\d*', str(obj_ori)) object_pose.pose.orientation.x = float(match[0]) object_pose.pose.orientation.y = float(match[1]) object_pose.pose.orientation.z = float(match[2]) object_pose.pose.orientation.w = float(match[3]) frame = Frame() frame.set_pose(object_pose) # rospy.loginfo('obj: \n{}' .format(obj)) # append only if object not exist for frame_tar in frame_list: if compare_objects(frame_tar, frame, self.max_distance_between_frames): break else: frame_list.append(frame) rospy.loginfo("append object to list") return frame_list
# drop location pose in odom frame sm.userdata.drop_target_pose = PoseStamped() sm.userdata.drop_target_pose.header.frame_id = 'odom' # move_base goal pose in odom frame sm.userdata.move_base_odom = PoseStamped() # move arm trajectory sm.userdata.arm_plan = RobotTrajectory() # joints' retracted position in joint space (arm joints only) sm.userdata.retract_pos = [0.0, 0.0, 0.80, -1.80, 0.0, 0.0] # array of RecognizedObject sm.userdata.object_array = RecognizedObjectArray() # define a object_recognition goal sm.userdata.ork_goal = ObjectRecognitionGoal() sm.userdata.ork_goal.use_roi = False sm.userdata.ork_frame = '' sm.userdata.picked_objects = ['apple', 'big coffee', 'monster'] sm.userdata.target_object = '' with sm: def ork_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: if len(result.recognized_objects.objects) > 0: userdata.ork_object_array = result.recognized_objects.objects
def execute_overhead_callback(self, goal): now = rospy.Time.now() if self.ros_overhead_cam_info is None: raise Exception("Camera Info not received") self.last_ros_image_stamp = self.ros_image_stamp try: self.ros_overhead_trigger.wait_for_service(timeout=0.1) self.ros_overhead_trigger() except: pass wait_count = 0 while self.ros_overhead_cam_info.ros_image is None or self.ros_image_stamp == self.last_ros_image_stamp: if wait_count > 250: raise Exception("Image receive timeout") time.sleep(0.25) wait_count += 1 print "Past timeout" #stored data in local function variable, so changed all references off of self.ros_image img = self.ros_overhead_cam_info.ros_image if img is None: raise Exception("Camera image data not received") print "image received" r_array = RecognizedObjectArray() r_array.header.stamp = now r_array.header.frame_id = self.frame_id if goal.use_roi: raise Warning("use_roi in ObjectRecognitionRequest ignored") search_objects = dict() with self.payloads_lock: for _, p in self.payloads.items(): search_objects[p.name] = p.markers for _, l in self.link_markers.items(): search_objects[l.header.frame_id] = l.markers #TODO: handle multiple aruco dictionaries, currently only use one print "past payload lock" aruco_dicts = set() for m in search_objects.itervalues(): for m2 in m: aruco_dicts.add(m2.marker.dictionary) print len(aruco_dicts) aruco_dicts.add("DICT_ARUCO_ORIGINAL") assert len( aruco_dicts ) == 1, "Currently all tags must be from the same dictionary" if not hasattr(cv2.aruco, next(iter(aruco_dicts))): raise ValueError("Invalid aruco-dict value") aruco_dict_id = getattr(cv2.aruco, next(iter(aruco_dicts))) aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id) c_pose = self.listener.lookupTransform( "/world", self.ros_overhead_cam_info.ros_data.header.frame_id, rospy.Time(0)) print "c_pose" print c_pose.p print c_pose.R parameters = cv2.aruco.DetectorParameters_create() parameters.cornerRefinementWinSize = 32 parameters.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers( img, aruco_dict, parameters=parameters) print ids for object_name, payload_markers in search_objects.items(): tag_object_poses = dict() for m in payload_markers: board = get_aruco_gridboard(m.marker, aruco_dict) #board = cv2.aruco.GridBoard_create(4, 4, .04, .0075, aruco_dict, 16) retval, rvec, tvec = cv2.aruco.estimatePoseBoard( corners, ids, board, self.ros_overhead_cam_info.camMatrix, self.ros_overhead_cam_info.distCoeffs) print retval print rvec print tvec if (retval > 0): if (m.name == "leeward_mid_panel_marker_1" or m.name == "leeward_tip_panel_marker_1"): print "Found tag: " + m.name try: #o_pose = self.listener.lookupTransform(m.name, object_name, rospy.Time(0)) o_pose = rox_msg.msg2transform(m.pose).inv() print object_name print o_pose print "" print "o_pose" print o_pose.p print o_pose.R Ra, b = cv2.Rodrigues(rvec) a_pose = rox.Transform(Ra, tvec) print "a_pose" print a_pose.p print a_pose.R tag_object_poses[m.name] = c_pose * (a_pose * o_pose) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue #TODO: merge tag data if multiple tags detected if len(tag_object_poses) > 0: p = PoseWithCovarianceStamped() p.pose.pose = rox_msg.transform2pose_msg( tag_object_poses.itervalues().next()) p.header.stamp = now p.header.frame_id = self.frame_id r = RecognizedObject() r.header.stamp = now r.header.frame_id = self.frame_id r.type.key = object_name r.confidence = 1 r.pose = p r_array.objects.append(r) """for o in self.object_names: try: (trans,rot) = self.listener.lookupTransform("/world", o, rospy.Time(0)) print o print trans print rot print "" p=PoseWithCovarianceStamped() p.pose.pose=Pose(Point(trans[0], trans[1], trans[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) p.header.stamp=now p.header.frame_id=self.frame_id r=RecognizedObject() r.header.stamp=now r.header.frame_id=self.frame_id r.type.key=o r.confidence=1 r.pose=p r_array.objects.append(r) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue""" print "Succeeded in finding tags" result = ObjectRecognitionResult() result.recognized_objects = r_array self.overhead_server.set_succeeded(result=result)