class DroneCommunication: # Initialization Function def __init__(self): # Parameters from YAML File self.drone_base_footprint_frame = rospy.get_param('drone_base_footprint') self.drone_base_link_frame = rospy.get_param('drone_base_link') self.map_frame = rospy.get_param('map_frame') self.control_drone_srv_name = rospy.get_param('drone_control_srv') # TF Listener for Getting Drone Position self.tf = Buffer() self.tf_listener = TransformListener(self.tf) # String with Num. of Command that will be sent to Drone Service self.drone_command = '' # Control Drone Service self.control_drone = rospy.ServiceProxy(self.control_drone_srv_name, ControlDrone) # Get the Yaw Between Map and Drone Function def get_drone_yaw(self): # TODO: Read TF and Compute Yaw try: tf_ = self.tf.lookup_transform(self.drone_base_link_frame, self.map_frame, rospy.Time(0), rospy.Duration(1.0)) (_, _, yaw) = euler_from_quaternion([tf_.transform.rotation.x, tf_.transform.rotation.y, tf_.transform.rotation.z, tf_.transform.rotation.w]) return 180-np.rad2deg(yaw) except tf.LookupException or tf.ConnectivityException or tf.ExtrapolationException: return -1 # Get the Position in Map of Drone Function def get_drone_pose(self): # TODO: Read TF and Compute Yaw try: tf_ = self.tf.lookup_transform(self.map_frame, self.drone_base_link_frame, rospy.Time(0), rospy.Duration(1.0)) (r1, r2, r3, _) = quaternion_matrix([tf_.transform.rotation.x, tf_.transform.rotation.y, tf_.transform.rotation.z, tf_.transform.rotation.w]) pose = np.asmatrix([r1[0:3], r2[0:3], r3[0:3]]) * \ np.asmatrix([[tf_.transform.translation.x],[tf_.transform.translation.y],[tf_.transform.translation.z]]) (x, y, z) = (pose.item(0), pose.item(1), pose.item(2)) except tf.LookupException or tf.ConnectivityException or tf.ExtrapolationException: return -1 return x, y, z # Send SIGNAL to Control Drone Service to Execute Command def sent_control_command(self, command): rospy.wait_for_service(self.control_drone_srv_name) try: drone_execute_result = self.control_drone(command) except rospy.ServiceException, e: print "Service call failed: %s" % e return drone_execute_result.status
class SetObjectTarget(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['target_set', 'no_new_object'], input_keys=['sot_object_array', 'sot_ork_frame', 'sot_picked_objects', 'sot_target_object', 'sot_grasp_target_pose'], output_keys=['sot_target_object', 'sot_grasp_target_pose']) self.ork_srv = rospy.ServiceProxy('get_object_info', GetObjectInformation) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) def execute(self, ud): rospy.logdebug("Entered 'SET_OBJECT_TARGET' state.") new_target = '' is_new_obj = False # loop through all found objects for i in range(len(ud.sot_object_array)): # get object info, mainly its name try: req = GetObjectInformationRequest() req.type = ud.sot_object_array[i].type res = self.ork_srv(req) # if the object hasn't been grasped yet, make the object the current grasp target is_new_obj = True if any(res.information.name in s for s in ud.sot_picked_objects): is_new_obj = False if is_new_obj: new_target = res.information.name except rospy.ServiceException: rospy.logerr("GetObjectInformation service request failed.") # if a new object has been detected if new_target and is_new_obj: # get the transform from ork's frame to odom transform = self.tf_buffer.lookup_transform('odom', ud.sot_ork_frame, rospy.Time(0)) # get the object's pose in odom frame ud.sot_grasp_target_pose = do_transform_pose(ud.sot_object_array[i].pose.pose, transform) ud.sot_target_object = new_target ud.sot_grasp_target_pose.pose.position.z += 0.08 ud.sot_grasp_target_pose.pose.position.x -= 0.15 print "OBJECT NAME : " + new_target print "POSE : " print ud.sot_grasp_target_pose check_call([PATH_TO_PDF_CREATOR], shell=True) check_call(['~/sara_ws/src/PDF_creator/create_pdf.sh'], shell=True) return 'target_set' # rospy.logerr("Could not get transform from " + ud.sot_ork_frame + " to 'odom'.") return 'no_new_object'
class TransformHandler(): def __init__(self, gt_frame, est_frame, max_time_between=0.01): self.gt_frame = gt_frame self.est_frame = est_frame self.frames = [gt_frame, est_frame] self.tf_buffer = Buffer(cache_time=rospy.Duration(max_time_between)) self.__tf_listener = TransformListener(self.tf_buffer) #self.warn_timer = rospy.Timer(rospy.Duration(5), self.__warn_timer_cb) def __warn_timer_cb(self, evt): available_frames = self.tf_buffer.all_frames_as_string() avail = True for frame in self.frames: if frame not in available_frames: rospy.logwarn('Frame {} has not been seen yet'.format(frame)) avail = False if avail: self.warn_timer.shutdown() def get_transform(self, fixed_frame, target_frame): # caller should handle the exceptions return self.tf_buffer.lookup_transform(target_frame, fixed_frame, rospy.Time(0))
class SimpleMapper(Node): def __init__(self, name): super().__init__(name) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.create_timer(SAMPLE_PERIOD, self.log_pose) self.time_since_start = 0 self.file = open('odom_log.csv', 'w') def log_pose(self): rotation = None translation = None try: tf = self.tf_buffer.lookup_transform('odom', 'base_link', Time(sec=0, nanosec=0)) rotation = quaternion_to_euler(tf.transform.rotation)[0] translation = [ tf.transform.translation.x, tf.transform.translation.y ] except (LookupException, ConnectivityException, ExtrapolationException) as e: print('No required transformation found: `{}`'.format(str(e))) return if translation[0] or translation[ 1] or rotation or self.time_since_start: self.time_since_start += SAMPLE_PERIOD output = f'{self.time_since_start:.1f},{translation[0]:.3f},{translation[1]:.3f},{rotation:.2f}\n' print(output) self.file.write(output)
class OfflineTfBuffer(): def __init__(self, transforms): self.t_front = transforms[0].header.stamp self.t_back = transforms[-1].header.stamp self.buff = Buffer(cache_time=(self.t_back - self.t_front), debug=False) for transform in transforms: self.buff.set_transform(transform, '/auth') def LookupTransform(self, target_frame, source_frame, times): return [ self.buff.lookup_transform(target_frame, source_frame, t) for t in times ] def AvailableTimeRange(self, target_frame, source_frame, times): if times[0] > self.t_back or times[-1] < self.t_front: return [] idx_front = 0 idx_back = len(times) - 1 while times[idx_front] < self.t_front: idx_front += 1 while times[idx_back] > self.t_back: idx_back -= 1 return (idx_front, idx_back + 1) def CanTransform(self, target_frame, source_frame, times): return self.buff.can_transform( target_frame, source_frame, times[0]) and self.buff.can_transform( target_frame, source_frame, times[-1])
def align_cb(userdata, default_goal): rospy.logdebug("Entered align base callback.") align_goal = MoveBaseGoal() tf_buffer = Buffer() tf_listener = TransformListener(tf_buffer) tf_stamped = tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0), rospy.Duration(10)) # we don't want the robot to translate, only to rotate align_goal.target_pose.pose.position.x = tf_stamped.transform.translation.x align_goal.target_pose.pose.position.y = tf_stamped.transform.translation.y # get orientation to face waypoint 2 desired_yaw = atan2(userdata.al_cb_waypoints[1].pose.position.y - align_goal.target_pose.pose.position.y, userdata.al_cb_waypoints[1].pose.position.x - align_goal.target_pose.pose.position.x) q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, desired_yaw) align_goal.target_pose.pose.orientation.x = q[0] align_goal.target_pose.pose.orientation.y = q[1] align_goal.target_pose.pose.orientation.z = q[2] align_goal.target_pose.pose.orientation.w = q[3] align_goal.target_pose.header.frame_id = 'map' align_goal.target_pose.header.stamp = rospy.Time.now() return align_goal
class AttemptMonitor(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['monitoring_done', 'wp2_case', 'monitor_failed'], input_keys=['ma_current_attempt', 'ma_attempt_limit', 'ma_target_wp', 'ma_waypoints', 'ma_wp_str'], output_keys=['ma_current_attempt', 'ma_target_wp']) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True) self.face_cmd = rospy.Publisher('/face_mode', UInt8, queue_size=1, latch=True) self.grasp_distance = 1.6 def execute(self, ud): rospy.logdebug("Entered 'MONITOR_ATTEMPTS' state.") # attempt limit not reached, try to reach target again if ud.ma_current_attempt < ud.ma_attempt_limit: if ud.ma_target_wp == 2: """ # wp2 is a special case we know that wp2 is initially blocked by an obstacle and cannot be reached until the obstacle move or is moved we consider the robot has reached the target if it is within grasp distance """ tf_stamped = self.tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0)) if sqrt((ud.ma_waypoints[1].pose.position.x - tf_stamped.transform.translation.x) ** 2 + (ud.ma_waypoints[1].pose.position.y - tf_stamped.transform.translation.y) ** 2) < self.grasp_distance: return 'wp2_case' else: if ud.ma_current_attempt < ud.ma_attempt_limit: ud.ma_current_attempt += 1 else: ud.ma_current_attempt = 1 tts_msg = String() tts_msg.data = "I can not reach" + ud.ma_wp_str[ud.ma_target_wp - 1] + "." + \ "I am moving toward the next waypoint." self.tts_pub.publish(tts_msg) ud.ma_target_wp += 1 return 'monitoring_done' else: ud.ma_current_attempt += 1 return 'monitoring_done' else: # attempt limit reached, skip to the next goal ud.ma_current_attempt = 1 self.face_cmd.publish(YELLOW_FACE) tts_msg = String() tts_msg.data = "I can not reach" + ud.ma_wp_str[ud.ma_target_wp - 1] + "." + "I am moving toward the next waypoint." self.tts_pub.publish(tts_msg) ud.ma_target_wp += 1 return 'monitoring_done'
class GetDropPose(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['drop_pose_acquired'], output_keys=['drop_pose']) self.tabletop_sub = rospy.Subscriber('/object_recognition/table_array', TableArray, self.tabletop_cb, queue_size=10) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.msg_received = False self.mutex = threading.Lock() self.tables = TableArray() def tabletop_cb(self, table_array): self.mutex.acquire() self.msg_received = True self.tables = table_array self.mutex.release() def execute(self, ud): loop_again = True while loop_again: self.mutex.acquire() if self.msg_received: loop_again = False self.mutex.release() rospy.sleep(rospy.Duration(1)) transform = self.tf_buffer.lookup_transform('odom', self.tables.header.frame_id, rospy.Time(0)) drop_pose_z = 1.5 tmp = PoseStamped() lst = [] for t in range(len(self.tables.tables)): tmp.pose = self.tables.tables[t].pose tmp.header.frame_id = self.tables.header.frame_id tmp.header.stamp = rospy.Time.now() eval_pose = do_transform_pose(tmp, transform) rpy = tf_conversions.transformations.euler_from_quaternion(eval_pose.pose.orientation) if rpy[2]**2 > 0.90: lst.append([(eval_pose.pose.position.z - drop_pose_z)**2, eval_pose]) lst.sort() ud.drop_pose = lst[0][1] print ud.drop_pose return 'drop_pose_acquired'
class TfBridge(object): """ Utility class to interface with /tf2 """ def __init__(self): """ """ self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.tf_broadcaster = TransformBroadcaster() def publish_pose_to_tf(self, pose, source_frame, target_frame, time=None): """ Publish the given pose to /tf2 """ msg = pose.to_msg() transform = TransformStamped() transform.child_frame_id = target_frame transform.header.frame_id = source_frame if time is not None: transform.header.stamp = time else: transform.header.stamp = rospy.Time().now() transform.transform.translation = msg.position transform.transform.rotation = msg.orientation self.tf_broadcaster.sendTransform(transform) def get_pose_from_tf(self, source_frame, target_frame, time=None): """ Get the pose from /tf2 """ try: if time is not None: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, time) else: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, rospy.Time(0)) x = trans.transform.translation.x y = trans.transform.translation.y z = trans.transform.translation.z rx = trans.transform.rotation.x ry = trans.transform.rotation.y rz = trans.transform.rotation.z rw = trans.transform.rotation.w pose = Vector6D(x=x, y=y, z=z).from_quaternion(rx, ry, rz, rw) return True, pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logwarn("[perception] Exception occured: {}".format(e)) return False, None
class TfWrapper(object): def __init__(self, buffer_size=2): self.tfBuffer = Buffer(rospy.Duration(buffer_size)) self.tf_listener = TransformListener(self.tfBuffer) self.tf_static = StaticTransformBroadcaster() self.tf_frequency = rospy.Duration(1.0) self.broadcasting_frames = [] self.broadcasting_frames_lock = Lock() rospy.sleep(0.1) def transform_pose(self, target_frame, pose): try: transform = self.tfBuffer.lookup_transform( target_frame, pose.header.frame_id, # source frame pose.header.stamp, rospy.Duration(2.0)) new_pose = do_transform_pose(pose, transform) return new_pose except ExtrapolationException as e: rospy.logwarn(e) def lookup_transform(self, target_frame, source_frame): p = PoseStamped() p.header.frame_id = source_frame p.pose.orientation.w = 1.0 return self.transform_pose(target_frame, p) def add_frame_from_pose(self, name, pose_stamped): with self.broadcasting_frames_lock: frame = TransformStamped() frame.header = pose_stamped.header frame.child_frame_id = name frame.transform.translation = pose_stamped.pose.position frame.transform.rotation = pose_stamped.pose.orientation self.broadcasting_frames.append(frame) def start_frame_broadcasting(self): self.tf_broadcaster = tf.TransformBroadcaster() self.tf_timer = rospy.Timer(self.tf_frequency, self.broadcasting_cb) def broadcasting_cb(self, data): with self.broadcasting_frames_lock: for frame in self.broadcasting_frames: frame.header.stamp = rospy.get_rostime() self.tf_broadcaster.sendTransformMessage(frame) def broadcast_static_frame(self, name, pose_stamped): frame = TransformStamped() frame.header = pose_stamped.header frame.child_frame_id = name frame.transform.translation = pose_stamped.pose.position frame.transform.rotation = pose_stamped.pose.orientation self.tf_static.sendTransform(frame)
class ProductDistanceServer(Node): def __init__(self): super().__init__('product_distance_server') self.srv = self.create_service(ProductDistance, 'product_distance', self.product_distance_callback) self.tfBuffer = Buffer() self.listener = TransformListener(self.tfBuffer, self) self.get_logger().info("Product Distance Server is ready") def distance(self, P1, P2): dist = math.sqrt( math.pow(P2.x - P1.x, 2) + math.pow(P2.y - P1.y, 2) + math.pow(P2.z - P1.z, 2) * 1.0) return dist def product_distance_callback(self, request, response): try: trans = self.tfBuffer.lookup_transform('base_link', request.source_frame, Time()) #Doing this because TransformStamped return Vector3 instead of Point frame_position = Point() frame_position.x = trans.transform.translation.x frame_position.y = trans.transform.translation.y frame_position.z = trans.transform.translation.z distance = self.distance(request.product_position, frame_position) #Find 3D distance -> https://stackoverflow.com/questions/20184992/finding-3d-distances-using-an-inbuilt-function-in-python # p0 = np.array([request.product_position.x,request.product_position.y,0.0]) # p1 = np.array([trans.transform.translation.x,trans.transform.translation.y,0.0]) # distance = np.linalg.norm(p0 - p1) # distance=math.dist([request.product_position.x,request.product_position.y],[trans.transform.translation.x,trans.transform.translation]) #distance=math.dist([1,2],[2,3]) #self.get_logger().info("Distance: %s -> product=(%s,%s,%s) target=(%s,%s,%s)"%(distance,p0[0],p0[1],p0[2],p1[0],p1[1],p1[2])) response = ProductDistance.Response() response.transform = trans response.distance = distance response.frame_position = frame_position return response except Exception as ex: template = "Error while computing transform. An exception of type {0} occurred: {1!r}" message = template.format(type(ex).__name__, ex.args) self.get_logger().error(message) response = ProductDistance.Response() return response
def __init__(self): _node = Node("tf2twist") qos_profile = QoSProfile(depth=10) # loop_rate = _node.create_rate(100) tfBuffer = Buffer() listener = TransformListener(tfBuffer, _node, qos=qos_profile) twist_data = geometry_msgs.msg.Twist() pub_cmd_vel = _node.create_publisher(geometry_msgs.msg.Twist, "cmd_vel", qos_profile) # self.transfromstamped = try: while rclpy.ok(): rclpy.spin_once(_node) now = _node.get_clock().now() - rclpy.duration.Duration( seconds=0, nanoseconds=1000000) try: trans = tfBuffer.lookup_transform( 'odom_frame', 'base_link', now, rclpy.duration.Duration(seconds=0, nanoseconds=0)) # print(trans) # print(trans.transform.rotation.x) roll, pitch, yaw = self.quaternion_to_euler( trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w) # print(yaw) if (yaw > 0.3): twist_data.angular.z = 70.0 #yaw*125*1.3 elif (yaw < -0.3): twist_data.angular.z = -70.0 #yaw*125*1.3 else: twist_data.angular.z = 0.0 pub_cmd_vel.publish(twist_data) except (LookupException, LookupError, ConnectionAbortedError, ConnectionError, ConnectionRefusedError, ConnectionResetError, ExtrapolationException, ConnectivityException) as e: # print(e) pass except (KeyboardInterrupt): pass
class StartGuiding(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['go_back'], input_keys=['sg_waypoints'], output_keys=['sg_target_wp']) self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.face_cmd = rospy.Publisher('/face_mode', UInt8, queue_size=1, latch=True) def execute(self, ud): tts_msg = String() rospy.sleep(rospy.Duration(5)) self.face_cmd.publish(YELLOW_FACE) tts_msg.data = "I can not guide you back safely. I will go back to the stating location on my own." self.tts_pub.publish(tts_msg) tf_stamped = self.tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0)) lst = [] for i in range(len(ud.sg_waypoints)): distance = sqrt((tf_stamped.transform.translation.x - ud.sg_waypoints[i].pose.position.x)**2 + (tf_stamped.transform.translation.y - ud.sg_waypoints[i].pose.position.y)**2) lst.append([distance, i]) lst.sort() if lst[0][1] == 0: ud.sg_target_wp = 0 else: ud.sg_target_wp = lst[0][1] - 1 return 'go_back'
class TfWrapper(object): def __init__(self, buffer_size=2): self.tfBuffer = Buffer(rospy.Duration(buffer_size)) self.tf_listener = TransformListener(self.tfBuffer) rospy.sleep(0.1) def transformPose(self, target_frame, pose): try: transform = self.tfBuffer.lookup_transform( target_frame, pose.header.frame_id, # source frame pose.header.stamp, rospy.Duration(1.0)) new_pose = do_transform_pose(pose, transform) return new_pose except ExtrapolationException as e: rospy.logwarn(e)
class Environment: def __init__(self): self.buffer = Buffer() self.listener = TransformListener(self.buffer) def get_transform(self, source_frame, target_frame, timeout=3): frames = '"{}" w.r.t. "{}"'.format(target_frame, source_frame) if rospy.get_param('verbose'): rospy.loginfo('Acquiring transform: ' + frames) start = rospy.get_time() while not rospy.is_shutdown() and rospy.get_time() - start < timeout: try: return self.buffer.lookup_transform(source_frame, target_frame, rospy.Time()) except TransformException: pass if rospy.get_param('verbose'): rospy.logwarn('Failed to find transform: ' + frames)
class WaitForObstacle(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['wait_over'], input_keys=['wait_target_wp', 'wait_waypoints'], output_keys=['wait_target_wp']) self.move_base_srv = rospy.ServiceProxy('/move_base/make_plan', GetPlan) self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) def execute(self, ud): rospy.logdebug("Entered 'WAIT_FOR_OBSTACLE' state.") nb_loop = 0 max_nb_loop = 5 start_pose = PoseStamped() tf_stamped = self.tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0)) start_pose = do_transform_pose(start_pose, tf_stamped) goal_pose = ud.wait_waypoints[1] # wp2 goal_tolerance = 0.10 while nb_loop < max_nb_loop: try: res = self.move_base_srv(start=start_pose, goal=goal_pose, tolerance=goal_tolerance) if res.plan.poses: break except rospy.ServiceException: pass nb_loop += 1 rospy.sleep(rospy.Duration(5)) if nb_loop > max_nb_loop: ud.wait_target_wp += 1 tts_msg = String() tts_msg.data = "I detect that the path to the waypoint is still obstructed." self.tts_pub.publish(tts_msg) tts_msg.data = "I have waited long enough. I am moving toward the next waypoint." self.tts_pub.publish(tts_msg) return 'wait_over'
def odom_callback(data): global camera_odom, camera_pose camera_odom = data camera_odom.pose.pose.position = camera_pose.pose.position camera_odom.twist.twist.linear.x = -camera_odom.twist.twist.linear.x camera_odom.twist.twist.linear.y = -camera_odom.twist.twist.linear.y rospy.init_node(vehicle_type+"_"+vehicle_id+'_ego_transfer') rospy.Subscriber("/t265/odom/sample", Odometry, odom_callback, queue_size=1) pose_pub = rospy.Publisher("/camera_pose", PoseStamped, queue_size=1) odom_pub = rospy.Publisher("/camera_odom", Odometry, queue_size=1) rate = rospy.Rate(20) tfBuffer = Buffer() tflistener = TransformListener(tfBuffer) while not rospy.is_shutdown(): try: tfstamped = tfBuffer.lookup_transform('map', 'camera', rospy.Time(0)) camera_pose.header.stamp = rospy.Time.now() camera_pose.pose.position.x = tfstamped.transform.translation.x camera_pose.pose.position.y = tfstamped.transform.translation.y camera_pose.pose.position.z = tfstamped.transform.translation.z camera_pose.pose.orientation = tfstamped.transform.rotation pose_pub.publish(camera_pose) odom_pub.publish(camera_odom) rate.sleep() except: rate.sleep()
class UnderworldsSimulation(object): def __init__(self): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.tf_broadcaster = TransformBroadcaster() self.base_frame_id = rospy.get_param("~base_frame_id", "base_footprint") self.global_frame_id = rospy.get_param("~global_frame_id", "odom") self.env_urdf_file_path = rospy.get_param("~env_urdf_file_path", "") self.cad_models_additional_search_path = rospy.get_param( "~cad_models_additional_search_path", "") self.static_entities_config_filename = rospy.get_param( "~static_entities_config_filename", "") self.bridge = CvBridge() self.entity_id_map = {} self.reverse_entity_id_map = {} self.joint_id_map = {} self.reverse_joint_id_map = {} self.constraint_id_map = {} self.markers_id_map = {} self.use_gui = rospy.get_param("~use_gui", True) if self.use_gui is True: self.client_simulator_id = p.connect(p.GUI) else: self.client_simulator_id = p.connect(p.DIRECT) if self.cad_models_additional_search_path != "": p.setAdditionalSearchPath(self.cad_models_additional_search_path) if self.env_urdf_file_path != "": self.load_urdf(self.global_frame_id, self.env_urdf_file_path, [0, 0, 0], [0, 0, 0, 1]) if self.static_entities_config_filename != "": with open(self.static_entities_config_filename, 'r') as stream: static_entities = yaml.safe_load(stream) for entity in static_entities: start_position = [ entity["position"]["x"], entity["position"]["y"], entity["position"]["z"] ] start_orientation = [ entity["orientation"]["x"], entity["orientation"]["y"], entity["orientation"]["z"] ] start_orientation = quaternion_from_euler( entity["orientation"]["x"], entity["orientation"]["y"], entity["orientation"]["z"], 'rxyz') self.load_urdf(entity["id"], entity["file"], start_position, start_orientation) p.setGravity(0, 0, -10) p.setRealTimeSimulation(0) self.rgb_image_topic = rospy.get_param("~rgb_image_topic", "/camera/rgb/image_raw") self.rgb_camera_info_topic = rospy.get_param( "~rgb_camera_info_topic", "/camera/rgb/camera_info") self.depth_image_topic = rospy.get_param("~depth_image_topic", "/camera/depth/image_raw") self.depth_camera_info_topic = rospy.get_param( "~depth_camera_info_topic", "/camera/depth/camera_info") self.tracks_topic = rospy.get_param("~tracks_topic", "tracks") self.position_tolerance = rospy.get_param("~position_tolerance", 0.001) self.orientation_tolerance = rospy.get_param("~orientation_tolerance", 0.001) self.robot_urdf_file_path = rospy.get_param("~robot_urdf_file_path", "r2d2.urdf") self.joint_states_listener = JointStatesListener(self) self.perspective_estimator = PerspectiveEstimator(self) self.use_depth = rospy.get_param("~use_depth", False) self.publish_markers = rospy.get_param("publish_markers", True) self.publish_perspectives = rospy.get_param("publish_perspectives", True) if self.publish_perspectives is True: self.perspective_publisher = rospy.Publisher("perspective_viz", Image, queue_size=1) if self.publish_markers is True: self.marker_publisher = rospy.Publisher("internal_simulation_viz", MarkerArray, queue_size=1) self.track_sub = rospy.Subscriber(self.tracks_topic, SceneChangesStamped, self.observation_callback) if self.publish_markers is True: self.simulation_timer = rospy.Timer(rospy.Duration(1.0 / 24.0), self.visualization_callback) def load_urdf(self, id, filename, t, q, remove_friction=False): try: base_link_sim_id = p.loadURDF( filename, t, q, flags=p.URDF_ENABLE_SLEEPING or p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES) except Exception as e: rospy.logwarn("[simulation] Error loading '{}': {}".format( filename, e)) return # If file successfully loaded if base_link_sim_id >= 0: self.entity_id_map[id] = base_link_sim_id # Create a joint map to ease exploration self.reverse_entity_id_map[base_link_sim_id] = id self.joint_id_map[base_link_sim_id] = {} self.reverse_joint_id_map[base_link_sim_id] = {} for i in range( 0, p.getNumJoints(base_link_sim_id, physicsClientId=self.client_simulator_id)): info = p.getJointInfo(base_link_sim_id, i, physicsClientId=self.client_simulator_id) self.joint_id_map[base_link_sim_id][info[1]] = info[0] self.reverse_joint_id_map[base_link_sim_id][info[0]] = info[1] rospy.loginfo( "[simulation] '{}' File successfully loaded".format(filename)) else: raise ValueError( "Invalid URDF file provided: '{}' ".format(filename)) def get_transform_from_tf2(self, source_frame, target_frame, time=None): try: if time is not None: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, time) else: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, rospy.Time(0)) x = trans.transform.translation.x y = trans.transform.translation.y z = trans.transform.translation.z rx = trans.transform.rotation.x ry = trans.transform.rotation.y rz = trans.transform.rotation.z rw = trans.transform.rotation.w return True, [x, y, z], [rx, ry, rz, rw] except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logwarn("[simulation] Exception occured: {}".format(e)) return False, [0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0] def update_constraint(self, id, t, q): if id not in self.entity_id_map: raise ValueError( "Entity <{}> is not loaded into the simulator".format(id)) if id in self.constraint_id[id]: p.changeConstraint(self.constraint_id[id], t, jointChildFrameOrientation=q, maxForce=50) else: self.constraint_id_map[id] = p.createConstraint( self.entity_id_map[id], -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], t, childFrameOrientation=q) def remove_constraint(self, id): if id not in self.constraint_id_map: raise ValueError( "Constraint for entity <{}> not created".format(id)) p.removeConstraint(self.constraint_id[id]) def add_shape(self, track): if track.has_shape is True: if track.shape.type == PrimitiveShape.CYLINDER: position = [] orientation = [] visual_shape_id = p.createVisualShape( p.GEOM_CYLINDER, radius=track.shape.dimensions[0], length=track.shape.dimensions[1]) collision_shape_id = p.createCollisionShape( p.GEOM_CYLINDER, radius=track.shape.dimensions[0], length=track.shape.dimensions[1]) entity_id = p.createMultiBody( baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, basePosition=position, baseOrientation=orientation, flags=p.URDF_ENABLE_SLEEPING or p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES) if entity_id >= 0: self.entity_id_map[track.id] = entity_id self.reverse_entity_id_map[entity_id] = track.id elif track.shape.type == PrimitiveShape.MESH: pass else: raise NotImplementedError( "Only cylinder shapes for unknown objects supported at the moment." ) def remove_shape(self, track): pass def update_entity(self, id, t, q): if id not in self.entity_id_map: raise ValueError( "Entity <{}> is not loaded into the simulator".format(id)) base_link_sim_id = self.entity_id_map[id] t_current, q_current = p.getBasePositionAndOrientation( base_link_sim_id) update_position = not np.allclose( np.array(t_current), t, atol=self.position_tolerance) update_orientation = not np.allclose( np.array(q_current), q, atol=self.position_tolerance) if update_position is True or update_orientation is True: p.resetBasePositionAndOrientation( base_link_sim_id, t, q, physicsClientId=self.client_simulator_id) def publish_marker_array(self): marker_array = MarkerArray() marker_array.markers = [] now = rospy.Time() for sim_id in range(0, p.getNumBodies()): visual_shapes = p.getVisualShapeData(sim_id) for shape in visual_shapes: marker = Marker() entity_id = shape[0] link_id = shape[1] type = shape[2] dimensions = shape[3] mesh_file_path = shape[4] position = shape[5] orientation = shape[6] rgba_color = shape[7] if link_id != -1: link_state = p.getLinkState(sim_id, link_id) t_link = link_state[0] q_link = link_state[1] t_inertial = link_state[2] q_inertial = link_state[3] tf_world_link = np.dot(translation_matrix(t_link), quaternion_matrix(q_link)) tf_inertial_link = np.dot(translation_matrix(t_inertial), quaternion_matrix(q_inertial)) world_transform = np.dot(tf_world_link, np.linalg.inv(tf_inertial_link)) else: t_link, q_link = p.getBasePositionAndOrientation(sim_id) world_transform = np.dot(translation_matrix(t_link), quaternion_matrix(q_link)) marker.header.frame_id = self.global_frame_id marker.header.stamp = now marker.id = entity_id + ( link_id + 1) << 24 # same id convention than bullet marker.action = Marker.MODIFY marker.ns = self.reverse_entity_id_map[sim_id] trans_shape = np.dot(translation_matrix(position), quaternion_matrix(orientation)) shape_transform = np.dot(world_transform, trans_shape) position = translation_from_matrix(shape_transform) orientation = quaternion_from_matrix(shape_transform) marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] if len(rgba_color) > 0: marker.color.r = rgba_color[0] marker.color.g = rgba_color[1] marker.color.b = rgba_color[2] marker.color.a = rgba_color[3] if type == p.GEOM_SPHERE: marker.type = Marker.SPHERE marker.scale.x = dimensions[0] * 2.0 marker.scale.y = dimensions[0] * 2.0 marker.scale.z = dimensions[0] * 2.0 elif type == p.GEOM_BOX: marker.type = Marker.CUBE marker.scale.x = dimensions[0] marker.scale.y = dimensions[1] marker.scale.z = dimensions[2] elif type == p.GEOM_CAPSULE: marker.type = Marker.SPHERE elif type == p.GEOM_CYLINDER: marker.type = Marker.CYLINDER marker.scale.x = dimensions[1] * 2.0 marker.scale.y = dimensions[1] * 2.0 marker.scale.z = dimensions[0] elif type == p.GEOM_PLANE: marker.type = Marker.CUBE marker.scale.x = dimensions[0] marker.scale.y = dimensions[1] marker.scale.z = 0.0001 elif type == p.GEOM_MESH: marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "file://" + mesh_file_path marker.scale.x = dimensions[0] marker.scale.y = dimensions[1] marker.scale.z = dimensions[2] marker.mesh_use_embedded_materials = True else: raise NotImplementedError marker.lifetime = rospy.Duration(1.0) marker_array.markers.append(marker) self.marker_publisher.publish(marker_array) def observation_callback(self, tracks_msg): p.stepSimulation() for track in tracks_msg.changes.nodes: if track.has_camera is True and track.is_located is True: position = track.pose_stamped.pose.pose.position orientation = track.pose_stamped.pose.pose.orientation t = [position.x, position.y, position.z] q = [ orientation.x, orientation.y, orientation.z, orientation.w ] rgb_image, depth_image, detections, viz_frame = self.perspective_estimator.estimate( t, q, track.camera) viz_img_msg = self.bridge.cv2_to_imgmsg(viz_frame) self.perspective_publisher.publish(viz_img_msg) def visualization_callback(self, event): self.publish_marker_array()
rospy.init_node('turtle1_listener') tfBuffer = Buffer() listener = TransformListener(tfBuffer) rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) turtle_name = rospy.get_param('turtle', 'turtle2') spawner(4, 2, 0, turtle_name) turtle_vel = rospy.Publisher('%s/cmd_vel' % turtle_name, Twist, queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: trans = tfBuffer.lookup_transform(turtle_name, 'turtle1', rospy.Time()) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rate.sleep() continue msg = Twist() msg.linear.x = 0.5 * math.sqrt(trans.transform.translation.x**2 + trans.transform.translation.y**2) msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x) turtle_vel.publish(msg) rate.sleep()
class SimpleMapper(Node): def __init__(self, name): super().__init__(name) fill_map_param = self.declare_parameter('fill_map', True) # Init map related elements self.map = [-1] * MAP_WIDTH * MAP_HEIGHT self.map_publisher = self.create_publisher( OccupancyGrid, '/map', qos_profile=QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, )) self.tf_publisher = StaticTransformBroadcaster(self) tf = TransformStamped() tf.header.stamp = self.get_clock().now().to_msg() tf.header.frame_id = 'map' tf.child_frame_id = 'odom' tf.transform.translation.x = 0.0 tf.transform.translation.y = 0.0 tf.transform.translation.z = 0.0 self.tf_publisher.sendTransform(tf) # Init laser related elements if fill_map_param.value: self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.scanner_subscriber = self.create_subscription( LaserScan, '/scan', self.update_map, 1) # Start publishing the map self.publish_map() self.create_timer(1, self.publish_map) def publish_map(self): now = self.get_clock().now() msg = OccupancyGrid() msg.header.stamp = now.to_msg() msg.header.frame_id = 'map' msg.info.resolution = RESOLUTION msg.info.width = MAP_WIDTH msg.info.height = MAP_HEIGHT msg.info.origin.position.x = WORLD_ORIGIN_X msg.info.origin.position.y = WORLD_ORIGIN_Y msg.data = self.map self.map_publisher.publish(msg) def update_map(self, msg): # Determine transformation of laser and robot in respect to odometry laser_rotation = None laser_translation = None try: tf = self.tf_buffer.lookup_transform('odom', msg.header.frame_id, Time(sec=0, nanosec=0)) laser_rotation = quaternion_to_euler(tf.transform.rotation)[0] laser_translation = tf.transform.translation except (LookupException, ConnectivityException, ExtrapolationException) as e: print('No required transformation found: `{}`'.format(str(e))) return # Determine position of robot and laser world_robot_x = laser_translation.x + WORLD_ORIGIN_X world_robot_y = laser_translation.y + WORLD_ORIGIN_Y world_laser_xs = [] world_laser_ys = [] laser_range_angle = msg.angle_min + laser_rotation for laser_range in msg.ranges: if laser_range < msg.range_max and laser_range > msg.range_min: laser_x = world_robot_x + laser_range * cos(laser_range_angle) laser_y = world_robot_y + laser_range * sin(laser_range_angle) world_laser_xs.append(laser_x) world_laser_ys.append(laser_y) laser_range_angle += msg.angle_increment # Determine position on map (from world coordinates) robot_x = int(world_robot_x / RESOLUTION) robot_y = int(world_robot_y / RESOLUTION) laser_xs = [] laser_ys = [] for world_laser_x, world_laser_y in zip(world_laser_xs, world_laser_ys): laser_x = int(world_laser_x / RESOLUTION) laser_y = int(world_laser_y / RESOLUTION) laser_xs.append(laser_x) laser_ys.append(laser_y) # Fill the map based on known readings for laser_x, laser_y in zip(laser_xs, laser_ys): self.plot_bresenham_line(robot_x, laser_x, robot_y, laser_y) self.map[laser_y * MAP_WIDTH + laser_x] = 100 def plot_bresenham_line(self, x0, x1, y0, y1): # Bresenham's line algorithm (https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm) dx = abs(x1 - x0) sx = 1 if x0 < x1 else -1 dy = -abs(y1 - y0) sy = 1 if y0 < y1 else -1 err = dx + dy while True: self.map[y0 * MAP_WIDTH + x0] = 0 if x0 == x1 and y0 == y1: break e2 = 2 * err if e2 >= dy: err += dy x0 += sx if e2 <= dx: err += dx y0 += sy
class HandeyeSampler(object): """ Manages the samples acquired from tf. """ def __init__(self, handeye_parameters): self.handeye_parameters = handeye_parameters # tf structures self.tfBuffer = Buffer() """ used to get transforms to build each sample :type: tf2_ros.Buffer """ self.tfListener = TransformListener(self.tfBuffer) """ used to get transforms to build each sample :type: tf2_ros.TransformListener """ self.tfBroadcaster = TransformBroadcaster() """ used to publish the calibration after saving it :type: tf.TransformBroadcaster """ # internal input data self.samples = [] """ list of acquired samples Each sample is a dictionary going from 'rob' and 'opt' to the relative sampled transform in tf tuple format. :type: list[dict[str, ((float, float, float), (float, float, float, float))]] """ def _wait_for_tf_init(self): """ Waits until all needed frames are present in tf. :rtype: None """ self.tfBuffer.lookup_transform( self.handeye_parameters.robot_base_frame, self.handeye_parameters.robot_effector_frame, Time(0), Duration(20)) self.tfBuffer.lookup_transform( self.handeye_parameters.tracking_base_frame, self.handeye_parameters.tracking_marker_frame, Time(0), Duration(60)) def _get_transforms(self, time=None): """ Samples the transforms at the given time. :param time: sampling time (now if None) :type time: None|Time :rtype: dict[str, ((float, float, float), (float, float, float, float))] """ if time is None: time = Time.now() # here we trick the library (it is actually made for eye_on_hand only). Trust me, I'm an engineer if self.handeye_parameters.eye_on_hand: rob = self.tfBuffer.lookup_transform( self.handeye_parameters.robot_base_frame, self.handeye_parameters.robot_effector_frame, time, Duration(10)) else: rob = self.tfBuffer.lookup_transform( self.handeye_parameters.robot_effector_frame, self.handeye_parameters.robot_base_frame, time, Duration(10)) opt = self.tfBuffer.lookup_transform( self.handeye_parameters.tracking_base_frame, self.handeye_parameters.tracking_marker_frame, time, Duration(10)) return {'robot': rob, 'optical': opt} def take_sample(self): """ Samples the transformations and appends the sample to the list. :rtype: None """ loginfo("Taking a sample...") transforms = self._get_transforms() loginfo("Got a sample") self.samples.append(transforms) def remove_sample(self, index): """ Removes a sample from the list. :type index: int :rtype: None """ if 0 <= index < len(self.samples): del self.samples[index] def get_samples(self): """ Returns the samples accumulated so far. :rtype: [dict[str, ((float, float, float), (float, float, float, float))]] :return: A list of tuples containing the tracking and the robot transform pairs """ return self.samples
class RobotListener(object): def __init__(self, robot_urdf): self.global_frame_id = 'map' self.bullet_node_id_map = {} # Initialize tf2 buffer self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.robot_urdf = robot_urdf self.base_frame_id = "base_footprint" sleep(1.) _, t, q = self.get_transform_from_tf2(self.global_frame_id, self.base_frame_id) self.bullet_node_id_map[self.base_frame_id] = p.loadURDF( self.robot_urdf, t, q) self.init_robot_joint_map() self.joint_state_subscriber = rospy.Subscriber( "/joint_states", JointState, self.update_robot_joints) def init_robot_joint_map(self): self.simulator_joint_id_map = {} for i in range( 0, p.getNumJoints(self.bullet_node_id_map[self.base_frame_id])): info = p.getJointInfo(self.bullet_node_id_map[self.base_frame_id], i) joint_name = info[1] self.simulator_joint_id_map[joint_name] = i def get_transform_from_tf2(self, source_frame, target_frame): try: trans = self.tf_buffer.lookup_transform(source_frame, target_frame, rospy.Time(0)) x = trans.transform.translation.x y = trans.transform.translation.y z = trans.transform.translation.z rx = trans.transform.rotation.x ry = trans.transform.rotation.y rz = trans.transform.rotation.z rw = trans.transform.rotation.w return True, [x, y, z], [rx, ry, rz, rw] except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): return False, [0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0] def update_robot_base(self): base_link_sim_id = self.bullet_node_id_map[self.base_frame_id] success, pos, rot = self.get_transform_from_tf2( self.global_frame_id, self.base_frame_id) if success: p.resetBasePositionAndOrientation(base_link_sim_id, pos, rot) def update_robot_joints(self, joint_states_msg): self.update_robot_base() base_link_sim_id = self.bullet_node_id_map[self.base_frame_id] for joint_index in range(0, len(joint_states_msg.name)): join_sim_id = self.simulator_joint_id_map[ joint_states_msg.name[joint_index]] p.resetJointState(base_link_sim_id, join_sim_id, joint_states_msg.position[joint_index], 0.0)
class LandmarkMethodROS(LandmarkMethodBase): def __init__(self, img_proc=None): super(LandmarkMethodROS, self).__init__(device_id_facedetection=rospy.get_param( "~device_id_facedetection", default="cuda:0")) self.subject_tracker = FaceEncodingTracker() if rospy.get_param( "~use_face_encoding_tracker", default=True) else SequentialTracker() self.bridge = CvBridge() self.__subject_bridge = SubjectListBridge() self.camera_frame = rospy.get_param("~camera_frame", "kinect2_link") self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "kinect2_ros_frame") self.tf2_broadcaster = TransformBroadcaster() self.tf2_buffer = Buffer() self.tf2_listener = TransformListener(self.tf2_buffer) self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze") self.visualise_headpose = rospy.get_param("~visualise_headpose", default=True) self.pose_stabilizers = {} # Introduce scalar stabilizers for pose. try: if img_proc is None: tqdm.write("Wait for camera message") cam_info = rospy.wait_for_message("/camera_info", CameraInfo, timeout=None) self.img_proc = PinholeCameraModel() # noinspection PyTypeChecker self.img_proc.fromCameraInfo(cam_info) else: self.img_proc = img_proc if np.array_equal( self.img_proc.intrinsicMatrix().A, np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])): raise Exception( 'Camera matrix is zero-matrix. Did you calibrate' 'the camera and linked to the yaml file in the launch file?' ) tqdm.write("Camera message received") except rospy.ROSException: raise Exception("Could not get camera info") # multiple person images publication self.subject_pub = rospy.Publisher("/subjects/images", MSG_SubjectImagesList, queue_size=3) # multiple person faces publication for visualisation self.subject_faces_pub = rospy.Publisher("/subjects/faces", Image, queue_size=3) Server(ModelSizeConfig, self._dyn_reconfig_callback) # have the subscriber the last thing that's run to avoid conflicts self.color_sub = rospy.Subscriber("/image", Image, self.process_image, buff_size=2**24, queue_size=3) def _dyn_reconfig_callback(self, config, _): self.model_points /= (self.model_size_rescale * self.interpupillary_distance) self.model_size_rescale = config["model_size"] self.interpupillary_distance = config["interpupillary_distance"] self.model_points *= (self.model_size_rescale * self.interpupillary_distance) self.head_pitch = config["head_pitch"] return config def process_image(self, color_msg): color_img = gaze_tools.convert_image(color_msg, "bgr8") timestamp = color_msg.header.stamp self.update_subject_tracker(color_img) if not self.subject_tracker.get_tracked_elements(): tqdm.write("\033[2K\033[1;31mNo face found\033[0m", end="\r") return self.subject_tracker.update_eye_images(self.eye_image_size) final_head_pose_images = [] for subject_id, subject in self.subject_tracker.get_tracked_elements( ).items(): if subject.left_eye_color is None or subject.right_eye_color is None: continue if subject_id not in self.pose_stabilizers: self.pose_stabilizers[subject_id] = [ Stabilizer(state_num=2, measure_num=1, cov_process=0.1, cov_measure=0.1) for _ in range(6) ] success, head_rpy, translation_vector = self.get_head_pose( subject.marks, subject_id) if success: # Publish all the data self.publish_pose(timestamp, translation_vector, head_rpy, subject_id) if self.visualise_headpose: # pitch roll yaw trans_msg = self.tf2_buffer.lookup_transform( self.ros_tf_frame, self.tf_prefix + "/head_pose_estimated" + str(subject_id), timestamp) rotation = trans_msg.transform.rotation euler_angles_head = list( transformations.euler_from_quaternion( [rotation.x, rotation.y, rotation.z, rotation.w])) euler_angles_head = gaze_tools.limit_yaw(euler_angles_head) face_image_resized = cv2.resize( subject.face_color, dsize=(224, 224), interpolation=cv2.INTER_CUBIC) final_head_pose_images.append( LandmarkMethodROS.visualize_headpose_result( face_image_resized, gaze_tools.get_phi_theta_from_euler( euler_angles_head))) if len(self.subject_tracker.get_tracked_elements().items()) > 0: self.publish_subject_list( timestamp, self.subject_tracker.get_tracked_elements()) if len(final_head_pose_images) > 0: headpose_image_ros = self.bridge.cv2_to_imgmsg( np.hstack(final_head_pose_images), "bgr8") headpose_image_ros.header.stamp = timestamp self.subject_faces_pub.publish(headpose_image_ros) def get_head_pose(self, landmarks, subject_id): """ We are given a set of 2D points in the form of landmarks. The basic idea is that we assume a generic 3D head model, and try to fit the 2D points to the 3D model based on the Levenberg-Marquardt optimization. We can use OpenCV's implementation of SolvePnP for this. This method is inspired by http://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/ We are planning to replace this with our own head pose estimator. :param landmarks: Landmark positions to be used to determine head pose :param subject_id: ID of the subject that corresponds to the given landmarks :return: success - Whether the pose was successfully extracted rotation_vector - rotation vector that along with translation vector brings points from the model coordinate system to the camera coordinate system translation_vector - see rotation_vector """ camera_matrix = self.img_proc.intrinsicMatrix() dist_coeffs = self.img_proc.distortionCoeffs() try: success, rodrigues_rotation, translation_vector, _ = cv2.solvePnPRansac( self.model_points, landmarks.reshape(len(self.model_points), 1, 2), cameraMatrix=camera_matrix, distCoeffs=dist_coeffs, flags=cv2.SOLVEPNP_DLS) except cv2.error as e: tqdm.write( '\033[2K\033[1;31mCould not estimate head pose: {}\033[0m'. format(e), end="\r") return False, None, None if not success: tqdm.write( '\033[2K\033[1;31mUnsuccessful in solvingPnPRanscan\033[0m', end="\r") return False, None, None # this is generic point stabiliser, the underlying representation doesn't matter rotation_vector, translation_vector = self.apply_kalman_filter_head_pose( subject_id, rodrigues_rotation, translation_vector / 1000.0) rotation_vector[0] += self.head_pitch _rotation_matrix, _ = cv2.Rodrigues(rotation_vector) _rotation_matrix = np.matmul( _rotation_matrix, np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])) _m = np.zeros((4, 4)) _m[:3, :3] = _rotation_matrix _m[3, 3] = 1 _rpy_rotation = np.array(transformations.euler_from_matrix(_m)) return success, _rpy_rotation, translation_vector def apply_kalman_filter_head_pose(self, subject_id, rotation_vector_unstable, translation_vector_unstable): stable_pose = [] pose_np = np.array( (rotation_vector_unstable, translation_vector_unstable)).flatten() for value, ps_stb in zip(pose_np, self.pose_stabilizers[subject_id]): ps_stb.update([value]) stable_pose.append(ps_stb.state[0]) stable_pose = np.reshape(stable_pose, (-1, 3)) rotation_vector = stable_pose[0] translation_vector = stable_pose[1] return rotation_vector, translation_vector def publish_subject_list(self, timestamp, subjects): assert (subjects is not None) subject_list_message = self.__subject_bridge.images_to_msg( subjects, timestamp) self.subject_pub.publish(subject_list_message) def publish_pose(self, timestamp, nose_center_3d_tf, head_rpy, subject_id): t = TransformStamped() t.header.frame_id = self.camera_frame t.header.stamp = timestamp t.child_frame_id = self.tf_prefix + "/head_pose_estimated" + str( subject_id) t.transform.translation.x = nose_center_3d_tf[0] t.transform.translation.y = nose_center_3d_tf[1] t.transform.translation.z = nose_center_3d_tf[2] rotation = transformations.quaternion_from_euler(*head_rpy) t.transform.rotation.x = rotation[0] t.transform.rotation.y = rotation[1] t.transform.rotation.z = rotation[2] t.transform.rotation.w = rotation[3] try: self.tf2_broadcaster.sendTransform([t]) except rospy.ROSException as exc: if str(exc) == "publish() to a closed topic": pass else: raise exc self.tf2_buffer.set_transform(t, 'extract_landmarks') def update_subject_tracker(self, color_img): faceboxes = self.get_face_bb(color_img) if len(faceboxes) == 0: self.subject_tracker.clear_elements() return tracked_subjects = self.get_subjects_from_faceboxes( color_img, faceboxes) # track the new faceboxes according to the previous ones self.subject_tracker.track(tracked_subjects)
class ArmTCPTransformHandler: """ This class uses a TransformListener to handle transforms related to the TCP. """ def __init__(self): self.__tf_buffer = Buffer() self.__tf_listener = TransformListener(self.__tf_buffer) self.__static_broadcaster = StaticTransformBroadcaster() def ee_link_to_tcp_pose_target(self, pose, ee_link): try: transform_tcp_to_ee_link = self.__tf_buffer.lookup_transform( ee_link, "TCP", rospy.Time(0)) transform_tcp_to_ee_link.header.frame_id = "ee_link_target" transform_tcp_to_ee_link.child_frame_id = "tcp_target" stamp = transform_tcp_to_ee_link.header.stamp transform_world_to_tcp_target = self.transform_from_pose( pose, "base_link", "ee_link_target", stamp) self.__tf_buffer.set_transform(transform_world_to_tcp_target, "default_authority") self.__tf_buffer.set_transform(transform_tcp_to_ee_link, "default_authority") ee_link_target_transform = self.__tf_buffer.lookup_transform( "base_link", "tcp_target", stamp) return self.pose_from_transform(ee_link_target_transform.transform) except ArmTCPTransformHandlerException: self.set_empty_tcp_to_ee_link_transform(ee_link) return pose def tcp_to_ee_link_pose_target(self, pose, ee_link): try: transform_tcp_to_ee_link = self.__tf_buffer.lookup_transform( "TCP", ee_link, rospy.Time(0)) transform_tcp_to_ee_link.header.frame_id = "tcp_target" transform_tcp_to_ee_link.child_frame_id = "ee_link_target" stamp = transform_tcp_to_ee_link.header.stamp transform_world_to_tcp_target = self.transform_from_pose( pose, "base_link", "tcp_target", stamp) self.__tf_buffer.set_transform(transform_world_to_tcp_target, "default_authority") self.__tf_buffer.set_transform(transform_tcp_to_ee_link, "default_authority") ee_link_target_transform = self.__tf_buffer.lookup_transform( "base_link", "ee_link_target", stamp) return self.pose_from_transform(ee_link_target_transform.transform) except ArmTCPTransformHandlerException: self.set_empty_tcp_to_ee_link_transform(ee_link) return pose def tcp_to_ee_link_position_target(self, position, ee_link): pose = Pose(position, Quaternion(0, 0, 0, 1)) return self.tcp_to_ee_link_pose_target(pose, ee_link).position def tcp_to_ee_link_quaternion_target(self, quaternion, ee_link): pose = Pose(Point(0, 0, 0), quaternion) return self.tcp_to_ee_link_pose_target(pose, ee_link).orientation def tcp_to_ee_link_rpy_target(self, roll, pitch, yaw, ee_link): qx, qy, qz, qw = quaternion_from_euler(roll, pitch, yaw) pose = Pose(Point(0, 0, 0), Quaternion(qx, qy, qz, qw)) pose = self.tcp_to_ee_link_pose_target(pose, ee_link) new_roll, new_pitch, new_yaw = euler_from_quaternion(*pose.orientation) return new_roll, new_pitch, new_yaw def set_empty_tcp_to_ee_link_transform(self, ee_link): ee_link_to_tcp_transform = self.transform_from_pose( Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)), ee_link, "TCP") self.__static_broadcaster.sendTransform(ee_link_to_tcp_transform) return ee_link_to_tcp_transform def lookup_transform(self, target_frame, source_frame, stamp=None): if stamp: return self.__tf_buffer.lookup_transform(target_frame, source_frame, stamp) return self.__tf_buffer.lookup_transform(target_frame, source_frame, rospy.Time(0)) def display_target_pose(self, pose, name="target_pose"): t = self.transform_from_pose(pose, "base_link", name) self.__static_broadcaster.sendTransform(t) @staticmethod def transform_from_pose(pose, parent_frame, child_frame, stamp=None): t = TransformStamped() t.transform.translation.x = pose.position.x t.transform.translation.y = pose.position.y t.transform.translation.z = pose.position.z t.transform.rotation = pose.orientation t.header.frame_id = parent_frame t.child_frame_id = child_frame if stamp: t.header.stamp = stamp return t @staticmethod def pose_from_transform(transform): pose = Pose() pose.position.x = transform.translation.x pose.position.y = transform.translation.y pose.position.z = transform.translation.z pose.orientation = transform.rotation return pose
class HealthMonitor(): # main health monitor class heartbeats = [ 'lidar', 'seek', 'realsense_rgb', 'realsense_depth', 'transformed_imu', 'localize', 'detection', 'detection_filtering', 'slam' ] totalBeats = len(heartbeats) def __init__(self): # status array self.healthLastTime = np.ones(self.totalBeats, dtype=int) self.healthPrevTime = np.zeros(self.totalBeats, dtype=int) self.healthTimeDiff = np.zeros(self.totalBeats, dtype=int) # fault dictionary self.healthDict = {} # how long before error is raised in seconds self.errorThreshold = float(rospy.get_param('~error_thres', '2.5')) # how long before node is restarted self.restartThreshold = float(rospy.get_param('~restart_thres', '5')) # whether or not to restart self.restartNodesFlag = bool(rospy.get_param('~restart_flag', 'False')) # health monitor rate self.timerFreq_ = float(rospy.get_param('~monitor_rate', '20')) # run the node self.rosInterface() # initialize the last recieved health time at the current time curTime = rospy.Time.now() self.healthLastTime *= curTime.to_nsec() while not rospy.is_shutdown(): self.run() rospy.sleep(1.0 / self.timerFreq_) def run(self): # subtract current times from last times curTime = rospy.Time.now() self.healthTimeDiff = curTime.to_nsec() - self.healthLastTime self.slamCheck() # rospy.loginfo(self.healthTimeDiff) # convert thresholds to nanoseconds errorThres = int(self.errorThreshold * 1e9) # # see if an error has occured self.checkError = self.healthTimeDiff < errorThres # publish health status self.outputMsg.status_time = rospy.Time.now() self.outputMsg.status_array = self.checkError self.healthPub_.publish(self.outputMsg) def rosInterface(self): # global sensor hearbeat topics self.lidarTopic = str(rospy.get_param('lidar_beat', '/scan')) self.seekTopic = str( rospy.get_param('seek_beat', '/seek_camera/filteredImage')) self.realSenseRGBTopic = str( rospy.get_param('realsense_rgb_beat', '/camera/color/image_raw')) self.realSenseDepthTopic = str( rospy.get_param('realsense_depth_beat', '/camera/depth/image_rect_raw')) self.transformedIMUTopic = str( rospy.get_param('transform_imu_beat', '/imu')) # global real time status update topics self.humanLocalizeTopic = str( rospy.get_param('human_beat', '/RelLocHeartbeat')) self.humanDetectionTopic = str( rospy.get_param('detection_beat', '/darknet_ros/detection_image')) self.humanFilterTopic = str( rospy.get_param('filter_beat', '/world_state')) # the heartbeats to listen to subscribers self.lidarSub_ = rospy.Subscriber(self.lidarTopic, LaserScan, self.lidarBeatCallback) self.seekSub_ = rospy.Subscriber(self.seekTopic, Image, self.seekCallback) self.realSenseRGBSub_ = rospy.Subscriber(self.realSenseRGBTopic, Image, self.realSenseRGBCallback) self.realSenseDepthSub_ = rospy.Subscriber(self.realSenseDepthTopic, Image, self.realSenseDepthCallback) self.transformedIMUSub_ = rospy.Subscriber(self.transformedIMUTopic, Imu, self.imuCallback) self.humanLocalizeSub_ = rospy.Subscriber(self.humanLocalizeTopic, Empty, self.humanLocalizeCallback) self.humanDetectionSub_ = rospy.Subscriber(self.humanDetectionTopic, Image, self.humanDetectionCallback) self.humanFilterSub_ = rospy.Subscriber(self.humanFilterTopic, WorldState, self.humanFilterCallback) # status publisher self.statusTopic_ = str( rospy.get_param("health_status", "/health_status")) self.outputMsg = watchStatus() self.healthPub_ = rospy.Publisher(self.statusTopic_, watchStatus, queue_size=10) # tf listener self.tfBuffer = Buffer() self.slamListener = TransformListener(self.tfBuffer) def slamCheck(self): try: transform = self.tfBuffer.lookup_transform("map", "base_link", rospy.Time()) self.callbackHandle(transform, 'slam') except (LookupException, ConnectivityException, ExtrapolationException): pass def callbackHandle(self, msg, system): # udpates the system descriptor's array entry with last message receieved time sys_index = self.heartbeats.index(system) self.healthLastTime[sys_index] = rospy.Time.now().to_nsec() # if want to ignore the time # self.checkError[sys_index] = True # TODO: see if I can use just one callback... def lidarBeatCallback(self, msg): self.callbackHandle(msg, 'lidar') def seekCallback(self, msg): self.callbackHandle(msg, 'seek') def realSenseRGBCallback(self, msg): self.callbackHandle(msg, 'realsense_rgb') def realSenseDepthCallback(self, msg): self.callbackHandle(msg, 'realsense_depth') def imuCallback(self, msg): self.callbackHandle(msg, 'transformed_imu') def humanLocalizeCallback(self, msg): self.callbackHandle(msg, 'localize') def humanDetectionCallback(self, msg): self.callbackHandle(msg, 'detection') def humanFilterCallback(self, msg): self.callbackHandle(msg, 'detection_filtering')
class Eval(object): def __init__(self): self.tfBuffer = Buffer(rospy.Duration(10)) self.tf_listener = TransformListener(self.tfBuffer) self.barcode_pose_sub = rospy.Subscriber('barcode/pose', Barcode, self.barcode_sub, queue_size=100) self.done = rospy.Service('barcode_evaluation/done', Trigger, self.done_cb) self.marker_pub = rospy.Publisher('wrong_codes', Marker, queue_size=10) self.barcodes = {} self.tp = {} self.fp = {} self.data = Data() self.load_ground_truth() rospy.loginfo('rdy to evaluate barcode detection') rospy.sleep(1) def make_marker(self, barcode_msg): ean = barcode_msg.barcode m = Marker() m.header = barcode_msg.barcode_pose.header m.pose = barcode_msg.barcode_pose.pose m.action = Marker.ADD m.type = Marker.CUBE m.scale = Vector3(.06, .06, .06) m.color = ColorRGBA(1, 0, 0, 1.0) text = Marker() text.header = barcode_msg.barcode_pose.header text.action = Marker.ADD text.type = Marker.TEXT_VIEW_FACING text.text = ean text.scale = Vector3(0, 0, .06) text.color = ColorRGBA(1, 0, 0, 1) text.pose.position.x = barcode_msg.barcode_pose.pose.position.x text.pose.position.y = barcode_msg.barcode_pose.pose.position.y text.pose.position.z = barcode_msg.barcode_pose.pose.position.z + 0.05 m.text = ean m.ns = ean text.ns = ean m.id = 2 self.marker_pub.publish(m) self.marker_pub.publish(text) def transformPose(self, target_frame, pose): transform = self.tfBuffer.lookup_transform( target_frame, pose.header.frame_id, # source frame pose.header.stamp, # get the tf at first available time rospy.Duration(1.0)) new_pose = do_transform_pose(pose, transform) return new_pose def done_cb(self, req): self.data.save_to_file() resp = TriggerResponse() resp.success = True self.final_results() return resp def load_ground_truth(self): # data = json.load(open('../data/iai_shelfs_barcodes.json')) data = json.load(open('../data/ground_truth.json')) for shelf in data.values(): for row in shelf: for barcode in row['barcode']: self.barcodes[str(barcode['code'])[:-1]] = np.array([ -barcode['pose']['position']['x'], 0.515 - barcode['pose']['position']['y'], 0.16 + barcode['pose']['position']['z'] ]) def final_results(self): tp = len(self.tp) fp = len(self.fp) fn = len(self.barcodes) - tp rospy.loginfo('tp: {}, fp: {}, fn: {}'.format(tp, fp, fn)) self.tp = {} self.fp = {} def barcode_sub(self, data): inner_barcode = data.barcode[1:-1] transformed_pose = self.tfBuffer.lookup_transform( 'map', data.barcode, rospy.Time(), timeout=rospy.Duration(4)) data.barcode_pose = self.transformPose('map', data.barcode_pose) p_map = transformed_pose.transform.translation p = np.array([p_map.x, p_map.y, p_map.z]) self.data.add_barcode(data) try: p2 = self.barcodes[inner_barcode] if inner_barcode not in self.tp: self.tp[inner_barcode] = p print('ground truth {}; predicted {}'.format(p2, p)) rospy.loginfo('detected {} position diff: {}'.format( inner_barcode, np.linalg.norm(p - p2))) print('-----') except KeyError as e: if inner_barcode not in self.fp: rospy.logwarn('unknown barcode {} at {} {}'.format( inner_barcode, p, data)) self.fp[inner_barcode] = p self.make_marker(data)
class ARObjectsPerception(object): def __init__(self): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.tf_broadcaster = TransformBroadcaster() self.camera_info_topic = rospy.get_param("~camera_info_topic", "") self.global_frame_id = rospy.get_param("~global_frame_id", "odom") self.resource_directory = rospy.get_param("~resource_directory", "") if self.resource_directory == "": raise ValueError( "Need to specify the '~resource_directory' parameter") self.ar_tags_config = rospy.get_param("~ar_tags_config", "") if self.ar_tags_config == "": raise ValueError("Need to specify the '~ar_tags_config' parameter") self.load_config(self.ar_tags_config) self.tracks = {} self.camera_info = None for entity_id, entity in self.config.items(): track = uwds3_msgs.msg.SceneNode() track.label = self.config[entity_id]["label"] track.description = self.config[entity_id]["description"] track.is_located = True track.has_shape = True shape = uwds3_msgs.msg.PrimitiveShape() shape.type = uwds3_msgs.msg.PrimitiveShape.MESH position = self.config[entity_id]["position"] orientation = self.config[entity_id]["orientation"] q = quaternion_from_euler(orientation["x"], orientation["y"], orientation["z"], "rxyz") shape.pose.position.x = position["x"] shape.pose.position.y = position["y"] shape.pose.position.z = position["z"] shape.pose.orientation.x = q[0] shape.pose.orientation.y = q[1] shape.pose.orientation.z = q[2] shape.pose.orientation.w = q[3] shape.mesh_resource = "file://" + self.resource_directory + self.config[ entity_id]["mesh_resource"] track.shapes = [shape] self.tracks[entity_id] = track self.camera_info_subscriber = rospy.Subscriber( self.camera_info_topic, sensor_msgs.msg.CameraInfo, self.camera_info_callback) self.ar_track_subscriber = rospy.Subscriber( "ar_pose_marker", ar_track_alvar_msgs.msg.AlvarMarkers, self.observation_callback) self.tracks_publisher = rospy.Publisher( "tracks", uwds3_msgs.msg.SceneChangesStamped, queue_size=1) def load_config(self, config_file_path): with open(config_file_path, "r") as file: self.config = yaml.load(file) def camera_info_callback(self, msg): if self.camera_info is None: rospy.loginfo("[ar_perception] Camera info received !") self.camera_info = msg self.camera_frame_id = msg.header.frame_id self.camera_matrix = np.array(msg.K).reshape((3, 3)) self.dist_coeffs = np.array(msg.D) def observation_callback(self, ar_track_msg): scene_changes = uwds3_msgs.msg.SceneChangesStamped() scene_changes.world = "tracks" scene_changes.header = ar_track_msg.header scene_changes.header.stamp = rospy.Time().now() scene_changes.header.frame_id = self.global_frame_id if self.camera_info is not None: if len(ar_track_msg.markers) > 0: scene_changes.header.stamp = ar_track_msg.header.stamp for object in ar_track_msg.markers: success, view_pose = self.get_pose_from_tf2( self.global_frame_id, object.header.frame_id) #, ar_track_msg.header.stamp) if success is True: position = object.pose.pose.position orientation = object.pose.pose.orientation obj_sensor_pose = Vector6D(x=position.x, y=position.y, z=position.z).from_quaternion( orientation.x, orientation.y, orientation.z, orientation.w) world_pose = view_pose + obj_sensor_pose self.tracks[ object.id].pose_stamped.header = scene_changes.header self.tracks[ object. id].pose_stamped.pose.pose.position.x = world_pose.position( ).x self.tracks[ object. id].pose_stamped.pose.pose.position.y = world_pose.position( ).y self.tracks[ object. id].pose_stamped.pose.pose.position.z = world_pose.position( ).z q = world_pose.quaternion() self.tracks[ object.id].pose_stamped.pose.pose.orientation.x = q[0] self.tracks[ object.id].pose_stamped.pose.pose.orientation.y = q[1] self.tracks[ object.id].pose_stamped.pose.pose.orientation.z = q[2] self.tracks[ object.id].pose_stamped.pose.pose.orientation.w = q[3] scene_changes.changes.nodes.append(self.tracks[object.id]) self.tracks_publisher.publish(scene_changes) def get_pose_from_tf2(self, source_frame, target_frame, time=None): try: if time is not None: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, time) else: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, rospy.Time(0)) x = trans.transform.translation.x y = trans.transform.translation.y z = trans.transform.translation.z rx = trans.transform.rotation.x ry = trans.transform.rotation.y rz = trans.transform.rotation.z rw = trans.transform.rotation.w pose = Vector6D(x=x, y=y, z=z).from_quaternion(rx, ry, rz, rw) return True, pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logwarn("[perception] Exception occured: {}".format(e)) return False, None
class TfBridge(object): """ Utility class to interface with tf2 """ def __init__(self, prefix=""): """ """ self.tf_buffer = Buffer() self.prefix = prefix self.tf_listener = TransformListener(self.tf_buffer) self.tf_broadcaster = TransformBroadcaster() def publish_pose_to_tf(self, pose, source_frame, target_frame, header=None): """ Publish the given pose to /tf2 """ msg = pose.to_msg() transform = TransformStamped() transform.child_frame_id = target_frame transform.header.frame_id = source_frame if header is not None: transform.header.stamp = header.stamp else: transform.header.stamp = rospy.Time().now() transform.transform.translation = msg.position transform.transform.rotation = msg.orientation self.tf_broadcaster.sendTransform(transform) def get_pose_from_tf(self, source_frame, target_frame, time=None): """ Get the pose from /tf2 """ try: if time is not None: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, time) else: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, rospy.Time(0)) x = trans.transform.translation.x y = trans.transform.translation.y z = trans.transform.translation.z rx = trans.transform.rotation.x ry = trans.transform.rotation.y rz = trans.transform.rotation.z rw = trans.transform.rotation.w pose = Vector6D(x=x, y=y, z=z).from_quaternion(rx, ry, rz, rw) return True, pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logwarn("[tf_bridge] Exception occured: {}".format(e)) return False, None def publish_tf_frames(self, tracks, events, header): for track in tracks: if track.is_located() is True and track.is_confirmed() is True: self.publish_pose_to_tf(track.pose, header.frame_id, self.prefix + track.id, header=header) for event in events: if event.is_located() is True: frame = event.subject + event.description + event.object pose = Vector6D(x=event.point.x, y=event.point.y, z=event.point.z) self.publish_pose_to_tf(pose, header.frame_id, self.prefix + frame, header=header)
'/land_param', Float32MultiArray, land_param_callback, queue_size=1) cmd_vel_pub = rospy.Publisher('/xtdrone/' + vehicle_type + '_' + vehicle_id + '/cmd_vel_enu', Twist, queue_size=1) cmd_pub = rospy.Publisher('/xtdrone/' + vehicle_type + '_' + vehicle_id + '/cmd', String, queue_size=1) rate = rospy.Rate(20) while not rospy.is_shutdown(): try: tfstamped = tfBuffer.lookup_transform('map', 'tag_' + vehicle_id, rospy.Time(0)) get_time = False cmd_vel_enu.linear.x = Kp_xy * (tfstamped.transform.translation.x + x_bias - local_pose.pose.position.x) cmd_vel_enu.linear.y = Kp_xy * (tfstamped.transform.translation.y + y_bias - local_pose.pose.position.y) if land_flag: cmd_vel_enu.linear.z = -land_vel else: cmd_vel_enu.linear.z = Kp_z * ( tfstamped.transform.translation.z + z_bias - local_pose.pose.position.z) print(cmd_vel_enu) except:
class RqtCalibrationEvaluator(Plugin): def __init__(self, context): super(RqtCalibrationEvaluator, self).__init__(context) # Give QObjects reasonable names self.setObjectName('CalibrationEvaluator') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) if not args.quiet: print('arguments: ', args) print('unknowns: ', unknowns) # Create QWidget self._widget = QWidget() self._infoWidget = QWidget() # Get path to UI file which should be in the "resource" folder of this package ui_file = os.path.join(rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource', 'rqt_handeye_evaluator.ui') ui_info_file = os.path.join( rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource', 'rqt_handeye_info.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) loadUi(ui_info_file, self._infoWidget) self._widget.layout().insertWidget(0, self._infoWidget) # Give QObjects reasonable names self._widget.setObjectName('RqtHandeyeCalibrationUI') # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) self._infoWidget.calibNameLineEdit.setText(rospy.get_namespace()) if rospy.get_param('eye_on_hand', False): self._infoWidget.calibTypeLineEdit.setText("eye on hand") else: self._infoWidget.calibTypeLineEdit.setText("eye on base") self._infoWidget.trackingBaseFrameLineEdit.setText( rospy.get_param('tracking_base_frame', 'optical_origin')) self._infoWidget.trackingMarkerFrameLineEdit.setText( rospy.get_param('tracking_marker_frame', 'optical_target')) self._infoWidget.robotBaseFrameLineEdit.setText( rospy.get_param('robot_base_frame', 'base_link')) self._infoWidget.robotEffectorFrameLineEdit.setText( rospy.get_param('robot_effector_frame', 'tool0')) self.output_label = self._widget.label_message self.output_label.setText('Waiting for samples...') self._widget.pushButton_reset.clicked.connect(self.reset) self.is_eye_on_hand = rospy.get_param('~eye_on_hand') self.robot_base_frame = rospy.get_param('~robot_base_frame') self.robot_effector_frame = rospy.get_param('~robot_effector_frame') self.tracking_measurement_frame = rospy.get_param( '~tracking_marker_frame') if self.is_eye_on_hand: self.robot_measurement_frame = self.robot_base_frame else: self.robot_measurement_frame = self.robot_effector_frame self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.update_timer = QTimer(self) self.update_timer.timeout.connect(self.tick) self.update_timer.start(500) self.last_robot_transform = None # used to see if we are in a steady state self.measurement_transforms = [ ] # used to measure the quality of the calibration: should have the same value self.robot_transforms = [ ] # used to determine when to sample: we wait for a steady state that has not been sampled yet self._widget.show() def tick(self): # wait for steady state to avoid problems with lag # if next to a copy already in the dataset, tell to move # if could not sample transform, tell how old the last one is # show average error; show dataset in 3D? try: new_robot_transform = self.tf_buffer.lookup_transform( self.robot_base_frame, self.robot_effector_frame, rospy.Time.now(), rospy.Duration.from_sec(0.2)) new_measurement_transform = self.tf_buffer.lookup_transform( self.robot_measurement_frame, self.tracking_measurement_frame, rospy.Time.now(), rospy.Duration.from_sec(0.2)) if new_robot_transform is None: rospy.logwarn( 'Could not sample transform between {} and {}'.format( self.robot_base_frame, self.robot_effector_frame)) if new_measurement_transform is None: rospy.logwarn( 'Could not sample transform between {} and {}'.format( self.robot_measurement_frame, self.tracking_measurement_frame)) if self.last_robot_transform is None: self.last_robot_transform = new_robot_transform self.updateUI() rospy.loginfo('Sampled first transform') return if RqtCalibrationEvaluator.transform_too_far( new_robot_transform, self.last_robot_transform, absolute_tolerance=0.001): self.last_robot_transform = new_robot_transform msg = 'Waiting for steady state' rospy.loginfo(msg) self.output_label.setText(msg) return if self.robot_transform_is_too_close_to_previous_sample( new_robot_transform, absolute_tolerance=0.003): self.updateUI() rospy.loginfo( 'Now we have {} samples\ntoo close to an old pose, move around!' .format(len(self.measurement_transforms))) self.output_label.setText( 'Too close to an old pose, move around!') return self.robot_transforms.append(new_robot_transform) self.measurement_transforms.append(new_measurement_transform) rospy.loginfo('Appending transform; we got {} now'.format( len(self.measurement_transforms))) self.updateUI() # TODO: provide feedback if the data is sufficient except (LookupException, ExtrapolationException, ConnectivityException) as e: msg = 'Could not sample pose!' rospy.loginfo(msg) rospy.logerr(e) self.output_label.setText(msg) def reset(self): self.robot_transforms = [] self.measurement_transforms = [] self.updateUI() def updateUI(self): self._widget.spinBox_samples.setValue(len(self.measurement_transforms)) if len(self.measurement_transforms) > 2: def translation_from_msg(msg): t = msg.transform.translation return t.x, t.y, t.z translations = [ translation_from_msg(t) for t in self.measurement_transforms ] translations_np = np.array(translations) translations_avg = translations_np.mean(axis=0) translations_from_avg = translations_np - translations_avg translations_max_divergence = np.max(translations_from_avg) rospy.loginfo( "Maximum divergence: {}".format(translations_max_divergence)) self._widget.doubleSpinBox_error.setEnabled(True) self._widget.doubleSpinBox_error.setValue( translations_max_divergence.max()) else: self._widget.doubleSpinBox_error.setValue(0) self._widget.doubleSpinBox_error.setEnabled(False) @staticmethod def transform_to_concatenated_translation_quaternion(transform): tr = transform.transform.translation quat = transform.transform.rotation return np.array([tr.x, tr.y, tr.z, quat.x, quat.y, quat.z, quat.w]) def robot_transform_is_too_close_to_previous_sample( self, new_robot_transform, absolute_tolerance): # TODO: use a meaningful metric posevec = RqtCalibrationEvaluator.transform_to_concatenated_translation_quaternion( new_robot_transform) for t in reversed(self.robot_transforms): old_posevec = RqtCalibrationEvaluator.transform_to_concatenated_translation_quaternion( t) if np.allclose(posevec, old_posevec, atol=absolute_tolerance): return True return False @staticmethod def transform_too_far(t1, t2, absolute_tolerance): # TODO: use a meaningful metric return not np.allclose( RqtCalibrationEvaluator. transform_to_concatenated_translation_quaternion(t1), RqtCalibrationEvaluator. transform_to_concatenated_translation_quaternion(t2), atol=absolute_tolerance) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class ImcInterface(object): def __init__(self): self._estimated_state_pub = rospy.Publisher("imc/estimated_state", EstimatedState, queue_size=5) self._estimated_state_msg = EstimatedState() self._geo_converter = rospy.ServiceProxy("convert_points", ConvertGeoPoints) self._plan_db = dict() self._current_plan = PlanDB() self._tf_buffer = Buffer() TransformListener(self._tf_buffer) rospy.Subscriber("gps", NavSatFix, self._handle_gps) rospy.Subscriber("odometry", Odometry, self._handle_pose) rospy.Subscriber("imc/goto_waypoint", Pose, self._handle_goto) rospy.Subscriber("imc/plan_control", PlanControl, self._handle_plan_control) rospy.Subscriber("imc/abort", Empty, self._handle_abort) rospy.Subscriber("imc/imc_heartbeat", Empty, self._handle_imc_heartbeat) rospy.Subscriber("imc/plan_db", PlanDB, self._handle_plan_db) rospy.Timer(rospy.Duration(1, 0), self._send_estimated_state) self._waypoint_serv = rospy.Service("load_waypoints", InitWaypointSet, self._send_goal) self._action_client = actionlib.SimpleActionClient( "follow_waypoints", FollowWaypointsAction) self._mode_client = rospy.ServiceProxy("controller/set_control_mode", SetControlMode) self._plan_db_pub = rospy.Publisher("imc/plan_db", PlanDB, queue_size=10) self._marker_pub = rospy.Publisher("current_waypoints", WaypointSet, queue_size=1) def _send_goal(self, req): res = InitWaypointSetResponse() res.success = False try: self._mode_client.wait_for_service(rospy.Duration(5)) mode_req = SetControlModeRequest() mode_req.mode.mode = mode_req.mode.LOSGUIDANCE self._mode_client(mode_req) self._action_client.wait_for_server(rospy.Duration(5)) goal = FollowWaypointsGoal() goal.waypoints.header = Header(0, rospy.Time.now(), "utm") goal.waypoints.start_time = Time(goal.waypoints.header.stamp) goal.waypoints.waypoints = req.waypoints self._marker_pub.publish(goal.waypoints) self._action_client.send_goal(goal, self._handle_done, self._handle_active, self._handle_feedback) res.success = True return res except Exception as e: rospy.logerr("{} | {}".format(rospy.get_name(), e.message)) return res def _handle_done(self, status, result): rospy.loginfo( "{} | Action completed, status: {}\n Waypoints completed: {}". format(rospy.get_name(), status, result)) def _handle_active(self): rospy.loginfo("{} | Waypoints set, Front Seat Driver active.".format( rospy.get_name())) def _handle_feedback(self, feedback): rospy.loginfo_throttle(10, "{} | {}".format(rospy.get_name(), feedback)) def _handle_goto(self, msg): """Parse if a goto message is received""" # TODO Not implemented in imc_ros_bridge yet rospy.loginfo(msg) pass def _parse_plan(self): # TODO this parser only accepts a set of goto commands """A plan has been requested by neptus to start. Parse the plan here and send to controller.""" geopoints = [] speeds = [] is_depths = [] for maneuver in self._current_plan.plan_spec.maneuvers: geopoints.append( GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi, maneuver.maneuver.lon * 180.0 / np.pi, maneuver.maneuver.z)) speeds.append(maneuver.maneuver.speed) is_depths.append(maneuver.maneuver.z_units == 1) req = ConvertGeoPointsRequest() req.geopoints = geopoints points = self._geo_converter.call(req).utmpoints wps = [] # If datum is available, then reference frame is ENU World for point, speed, is_depth in zip(points, speeds, is_depths): wp = Waypoint() wp.point.x = point.x wp.point.y = point.y wp.point.z = -abs(point.z) if is_depth else abs(point.z) wp.header.stamp = rospy.Time.now() wp.header.frame_id = "utm" wp.radius_of_acceptance = 1.0 wp.max_forward_speed = speed wp.use_fixed_heading = False wp.heading_offset = 0.0 wps.append(wp) req = InitWaypointSetRequest() req.start_time = Time(rospy.Time.from_sec(0)) req.start_now = True req.waypoints = wps req.max_forward_speed = max(speeds) req.interpolator.data = "lipb" req.heading_offset = 0 self._send_goal(req) def _handle_plan_control(self, msg): """Parse requests to start or stop a plan.""" typee = msg.type op = msg.op # request to start a mission! if typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_SET: self._current_plan = self._plan_db[msg.plan_id] self._parse_plan() req = SetControlModeRequest() req.mode.mode = req.mode.LOSGUIDANCE self._mode_client(req) # request to stop mission elif typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_DEL: self._current_plan = PlanDB() req = SetControlModeRequest() req.mode.mode = req.mode.HOLDPOSITION self._mode_client(req) def _handle_plan_db(self, msg): """Parse requests for Plan Database messages""" typee = msg.type op = msg.op # request for plan info if typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_GET_INFO: # we need to respond to this with some info... but what? rospy.loginfo_throttle_identical( 30, "Got REQUEST GET_INFO planDB msg from Neptus for plan: {}". format(str(msg.plan_id))) response = PlanDB() response.plan_id = self._plan_db[msg.plan_id].plan_id response.type = imc_enums.PLANDB_TYPE_SUCCESS response.op = imc_enums.PLANDB_OP_GET_INFO response.plandb_information = self._plan_db[ msg.plan_id].plandb_information self._plan_db_pub.publish(response) rospy.loginfo_throttle_identical( 30, "Answered GET_INFO for plan:" + str(response.plan_id)) # request for plan state elif typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_GET_STATE: rospy.loginfo_throttle_identical( 30, "Got REQUEST GET_STATE planDB msg from Neptus") response = PlanDB() response.plan_id = self._current_plan.plan_id response.type = imc_enums.PLANDB_TYPE_SUCCESS response.op = imc_enums.PLANDB_OP_GET_STATE response.plandb_state = PlanDBState() response.plandb_state.plan_count = len(self._plan_db) response.plandb_state.plans_info = [] totalSize = 0 latest = None # TODO Neptus needs an md5 calculation on the plans_info message to be stored in plandb_state.md5 in order # to sync, but we cannot seem to get that right for now. # https://github.com/LSTS/imcjava/blob/d95fddeab4c439e603cf5e30a32979ad7ace5fbc/src/java/pt/lsts/imc/adapter/PlanDbManager.java#L160 # See above for an example # It seems like we need to keep a planDB ourselves on this side, collect all the plans we # received and answer this get_state with data from them all to get a properly synchronized plan. buffer = StringIO( ) # This buffer method for calculating md5 sum of a ros message is not ideal. md = hashlib.md5() for p in self._plan_db.values(): pdi = p.plandb_information totalSize += pdi.plan_size response.plandb_state.plans_info.append(pdi) latest = latest if latest is None else max( latest, pdi.change_time) pdi.serialize(buffer) [md.update(i) for i in buffer.buflist] response.plandb_state.plan_count = len( response.plandb_state.plans_info) response.plandb_state.plan_size = totalSize response.plandb_state.md5 = md.digest() self._plan_db_pub.publish(response) rospy.loginfo_throttle_identical(30, "Answered GET_STATE for plans.") # ack for plan set success elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_SET: rospy.loginfo_throttle_identical( 20, "Received SUCCESS for plandb set") # ack for plan get info elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_GET_INFO: rospy.loginfo_throttle_identical( 20, "Received SUCCESS for plandb get info") # ack for plan get state elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_GET_STATE: rospy.loginfo_throttle_identical( 20, "Received SUCCESS for plandb get state") # request to set plan elif op == imc_enums.PLANDB_OP_SET: # update the plan_info and plan_state attributes msg.plandb_information.plan_id = msg.plan_id msg.plandb_information.plan_size = len(msg.plan_spec.maneuvers) msg.plandb_information.change_time = time.time() msg.plandb_information.md5 = msg.plan_spec_md5 self._plan_db.update({msg.plan_spec.plan_id: msg }) # place the plan in a dictionary of plans # send a success response response = PlanDB() response.type = imc_enums.PLANDB_TYPE_SUCCESS response.op = imc_enums.PLANDB_OP_SET response.plan_id = msg.plan_spec.plan_id self._plan_db_pub.publish(response) # handle other requests else: rospy.loginfo_throttle_identical( 5, "Received some unhandled planDB message:\n" + str(msg)) def _handle_abort(self, msg): """Listen for the abort message, do something if it comes :)""" self._action_client.wait_for_server() if self._action_client.get_state() == GoalStatus.ACTIVE: self._action_client.cancel_goal() self._mode_client.wait_for_service() req = SetControlModeRequest() req.mode.mode = req.mode.ABORT self._mode_client(req) def _handle_imc_heartbeat(self, msg): """Here in case you want to do something on the heartbeat of the IMC""" pass def _handle_gps(self, msg): # neptus expects latitude and longitude to be in radians self._estimated_state_msg.lat = msg.latitude * np.pi / 180.0 self._estimated_state_msg.lon = msg.longitude * np.pi / 180.0 self._estimated_state_msg.alt = msg.altitude def _handle_pose(self, msg): if self._tf_buffer.can_transform("bluerov2/base_link", "map_ned", rospy.Time.now(), rospy.Duration.from_sec(0.5)): tf = self._tf_buffer.lookup_transform("bluerov2/base_link", "map_ned", rospy.Time.now(), rospy.Duration.from_sec(0.5)) else: return # TODO neptus gets confused if you try to give all available estimates, there is probably a transformation problem # self._estimated_state_msg.x = tf.transform.translation.x # self._estimated_state_msg.y = tf.transform.translation.y # self._estimated_state_msg.z = tf.transform.translation.z # self._estimated_state_msg.depth = tf.transform.translation.z # self._estimated_state_msg.height = msg.pose.pose.position.z R = Rotation([ tf.transform.rotation.x, tf.transform.rotation.y, tf.transform.rotation.z, tf.transform.rotation.w ]) self._estimated_state_msg.phi, self._estimated_state_msg.theta, self._estimated_state_msg.psi = R.as_euler( "xyz", False).squeeze() # self._estimated_state_msg.u = msg.twist.twist.linear.x # self._estimated_state_msg.v = -msg.twist.twist.linear.y # self._estimated_state_msg.w = -msg.twist.twist.linear.z # self._estimated_state_msg.p = msg.twist.twist.angular.x # self._estimated_state_msg.q = -msg.twist.twist.angular.y # self._estimated_state_msg.r = -msg.twist.twist.angular.z # u = np.array([msg.twist.twist.linear.x,msg.twist.twist.linear.y,msg.twist.twist.linear.z]) # self._estimated_state_msg.vx, self._estimated_state_msg.vy, self._estimated_state_msg.vz = R.apply(u, True) def _send_estimated_state(self, event): self._estimated_state_pub.publish(self._estimated_state_msg)
class TabletopPerception(object): def __init__(self): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.tf_broadcaster = TransformBroadcaster() self.rgb_image_topic = rospy.get_param("~rgb_image_topic", "/camera/rgb/image_raw") self.rgb_camera_info_topic = rospy.get_param( "~rgb_camera_info_topic", "/camera/rgb/camera_info") self.depth_image_topic = rospy.get_param("~depth_image_topic", "/camera/depth/image_raw") self.depth_camera_info_topic = rospy.get_param( "~depth_camera_info_topic", "/camera/depth/camera_info") self.base_frame_id = rospy.get_param("~base_frame_id", "base_link") self.global_frame_id = rospy.get_param("~global_frame_id", "odom") self.bridge = CvBridge() rospy.loginfo( "[tabletop_perception] Subscribing to '/{}' topic...".format( self.depth_camera_info_topic)) self.camera_info = None self.camera_frame_id = None self.camera_info_subscriber = rospy.Subscriber( self.rgb_camera_info_topic, CameraInfo, self.camera_info_callback) detector_model_filename = rospy.get_param("~detector_model_filename", "") detector_weights_filename = rospy.get_param( "~detector_weights_filename", "") detector_config_filename = rospy.get_param("~detector_config_filename", "") face_detector_model_filename = rospy.get_param( "~face_detector_model_filename", "") face_detector_weights_filename = rospy.get_param( "~face_detector_weights_filename", "") face_detector_config_filename = rospy.get_param( "~face_detector_config_filename", "") self.detector = SSDDetector(detector_model_filename, detector_weights_filename, detector_config_filename, 300) self.face_detector = SSDDetector(face_detector_model_filename, face_detector_weights_filename, face_detector_config_filename, 300) self.foreground_detector = ForegroundDetector(interactive_mode=False) shape_predictor_config_filename = rospy.get_param( "~shape_predictor_config_filename", "") self.facial_landmarks_estimator = FacialLandmarksEstimator( shape_predictor_config_filename) face_3d_model_filename = rospy.get_param("~face_3d_model_filename", "") self.head_pose_estimator = HeadPoseEstimator(face_3d_model_filename) facial_features_model_filename = rospy.get_param( "~facial_features_model_filename", "") self.facial_features_estimator = FacialFeaturesEstimator( face_3d_model_filename, facial_features_model_filename) self.color_features_estimator = ColorFeaturesEstimator() self.n_frame = rospy.get_param("~n_frame", 4) self.frame_count = 0 self.publish_tf = rospy.get_param("~publish_tf", True) self.publish_visualization_image = rospy.get_param( "~publish_visualization_image", True) self.n_init = rospy.get_param("~n_init", 1) self.max_iou_distance = rospy.get_param("~max_iou_distance", 0.8) self.max_color_distance = rospy.get_param("~max_color_distance", 0.8) self.max_face_distance = rospy.get_param("~max_face_distance", 0.8) self.max_centroid_distance = rospy.get_param("~max_centroid_distance", 0.8) self.max_disappeared = rospy.get_param("~max_disappeared", 7) self.max_age = rospy.get_param("~max_age", 10) self.object_tracker = MultiObjectTracker(iou_cost, color_cost, 0.7, self.max_color_distance, 10, 5, self.max_age, use_appearance_tracker=True) self.face_tracker = MultiObjectTracker(iou_cost, centroid_cost, self.max_iou_distance, None, self.n_init, self.max_disappeared, self.max_age, use_appearance_tracker=False) self.person_tracker = MultiObjectTracker(iou_cost, color_cost, self.max_iou_distance, self.max_color_distance, self.n_init, self.max_disappeared, self.max_age, use_appearance_tracker=False) self.shape_estimator = ShapeEstimator() self.object_pose_estimator = ObjectPoseEstimator() self.tracks_publisher = rospy.Publisher("tracks", SceneChangesStamped, queue_size=1) self.visualization_publisher = rospy.Publisher("tracks_image", Image, queue_size=1) self.use_depth = rospy.get_param("~use_depth", False) if self.use_depth is True: rospy.loginfo( "[tabletop_perception] Subscribing to '/{}' topic...".format( self.rgb_image_topic)) self.rgb_image_sub = message_filters.Subscriber( self.rgb_image_topic, Image) rospy.loginfo( "[tabletop_perception] Subscribing to '/{}' topic...".format( self.depth_image_topic)) self.depth_image_sub = message_filters.Subscriber( self.depth_image_topic, Image) self.sync = message_filters.ApproximateTimeSynchronizer( [self.rgb_image_sub, self.depth_image_sub], 10, 0.2, allow_headerless=True) self.sync.registerCallback(self.observation_callback) else: rospy.loginfo( "[tabletop_perception] Subscribing to '/{}' topic...".format( self.rgb_image_topic)) self.rgb_image_sub = rospy.Subscriber(self.rgb_image_topic, Image, self.observation_callback, queue_size=1) def camera_info_callback(self, msg): if self.camera_info is None: rospy.loginfo("[tabletop_perception] Camera info received !") self.camera_info = msg self.camera_frame_id = msg.header.frame_id self.camera_matrix = np.array(msg.K).reshape((3, 3)) self.dist_coeffs = np.array(msg.D) def observation_callback(self, bgr_image_msg, depth_image_msg=None): if self.camera_info is not None: perception_timer = cv2.getTickCount() bgr_image = self.bridge.imgmsg_to_cv2(bgr_image_msg, "bgr8") rgb_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB) if depth_image_msg is not None: depth_image = self.bridge.imgmsg_to_cv2(depth_image_msg) else: depth_image = None if self.publish_visualization_image is True: viz_frame = bgr_image _, image_height, image_width = bgr_image.shape success, view_pose = self.get_pose_from_tf2( self.global_frame_id, self.camera_frame_id) if success is not True: rospy.logwarn( "[tabletop_perception] The camera sensor is not localized in world space (frame '{}'), please check if the sensor frame is published in /tf" .format(self.global_frame_id)) else: view_matrix = view_pose.transform() self.frame_count %= self.n_frame ###################################################### # Detection ###################################################### detections = [] if self.frame_count == 0: detections = self.detector.detect(rgb_image, depth_image=depth_image) object_detections = self.foreground_detector.detect( rgb_image, depth_image=depth_image, prior_detections=detections) detections += object_detections elif self.frame_count == 1: detections = self.face_detector.detect( rgb_image, depth_image=depth_image) else: detections = [] #################################################################### # Features estimation #################################################################### self.color_features_estimator.estimate(rgb_image, detections) ###################################################### # Tracking ###################################################### if self.frame_count == 0: object_detections = [ d for d in detections if d.label != "person" ] person_detections = [ d for d in detections if d.label == "person" ] face_tracks = self.face_tracker.update(rgb_image, []) object_tracks = self.object_tracker.update( rgb_image, object_detections) person_tracks = self.person_tracker.update( rgb_image, person_detections) elif self.frame_count == 1: face_tracks = self.face_tracker.update( rgb_image, detections) object_tracks = self.object_tracker.update(rgb_image, []) person_tracks = self.person_tracker.update(rgb_image, []) else: face_tracks = self.face_tracker.update(rgb_image, []) object_tracks = self.object_tracker.update(rgb_image, []) person_tracks = self.person_tracker.update(rgb_image, []) tracks = face_tracks + object_tracks + person_tracks ######################################################## # Pose & Shape estimation ######################################################## self.facial_landmarks_estimator.estimate( rgb_image, face_tracks) self.head_pose_estimator.estimate(face_tracks, view_matrix, self.camera_matrix, self.dist_coeffs) self.object_pose_estimator.estimate( object_tracks + person_tracks, view_matrix, self.camera_matrix, self.dist_coeffs) self.shape_estimator.estimate(rgb_image, tracks, self.camera_matrix, self.dist_coeffs) ######################################################## # Recognition ######################################################## self.facial_features_estimator.estimate(rgb_image, face_tracks) self.frame_count += 1 ###################################################### # Visualization of debug image ###################################################### perception_fps = cv2.getTickFrequency() / (cv2.getTickCount() - perception_timer) cv2.rectangle(viz_frame, (0, 0), (250, 40), (200, 200, 200), -1) perception_fps_str = "Perception fps : {:0.1f}hz".format( perception_fps) cv2.putText( viz_frame, "Nb detections/tracks : {}/{}".format( len(detections), len(tracks)), (5, 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) cv2.putText(viz_frame, perception_fps_str, (5, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) scene_changes = SceneChangesStamped() scene_changes.world = "tracks" header = bgr_image_msg.header header.frame_id = self.global_frame_id scene_changes.header = header for track in tracks: if self.publish_visualization_image is True: track.draw(viz_frame, (230, 0, 120, 125), 1, view_matrix, self.camera_matrix, self.dist_coeffs) if track.is_confirmed(): scene_node = track.to_msg(header) if self.publish_tf is True: if track.is_located() is True: self.publish_tf_pose(scene_node.pose_stamped, self.global_frame_id, scene_node.id) scene_changes.changes.nodes.append(scene_node) if self.publish_visualization_image is True: viz_img_msg = self.bridge.cv2_to_imgmsg(viz_frame) self.visualization_publisher.publish(viz_img_msg) self.tracks_publisher.publish(scene_changes) def get_pose_from_tf2(self, source_frame, target_frame, time=None): try: if time is not None: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, time) else: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, rospy.Time(0)) x = trans.transform.translation.x y = trans.transform.translation.y z = trans.transform.translation.z rx = trans.transform.rotation.x ry = trans.transform.rotation.y rz = trans.transform.rotation.z rw = trans.transform.rotation.w pose = Vector6D(x=x, y=y, z=z).from_quaternion(rx, ry, rz, rw) return True, pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logwarn("[perception] Exception occured: {}".format(e)) return False, None def publish_tf_pose(self, pose_stamped, source_frame, target_frame): transform = TransformStamped() transform.child_frame_id = target_frame transform.header.frame_id = source_frame transform.header.stamp = pose_stamped.header.stamp transform.transform.translation = pose_stamped.pose.pose.position transform.transform.rotation = pose_stamped.pose.pose.orientation self.tf_broadcaster.sendTransform(transform)