def get_place_to_search(self): focused_field = self._get_focused_field() x_s = focused_field[0] y_s = focused_field[1] lowest_value = 100 best_field = None for i in range(1, len(self._grid.field)): for x_diff in range(-i, i): y_diff = i - abs(x_diff) x1 = x_s + x_diff y1 = y_s + y_diff x2 = x_s - x_diff y2 = y_s - y_diff if x1 < len(self._grid.field) and y1 < len(self._grid.field[0]) and \ self._grid.field[x1][y1] < lowest_value: best_field = [x1, y1] lowest_value = self._grid.field[x1][y1] if x2 < len(self._grid.field) and y2 < len(self._grid.field[0]) and \ self._grid.field[x2][y2] < lowest_value: best_field = [x2, y2] lowest_value = self._grid.field[x2][y2] if lowest_value == 0: break # ends both loops if inner loop breaks, kinda hacky but should work else: continue break rospy.logdebug('Focused field: %s Field to search: %s' % (str(focused_field), str(best_field))) place_to_search = PointStamped() place_to_search.point = Point(best_field[0], best_field[1], 0) place_to_search.header.frame_id = '/odom_combined' rospy.logdebug('Place to search %s' % str(place_to_search)) visualization.publish_vector( self._grid.coordinates[best_field[0], best_field[1]], 9999) return place_to_search
def get_place_to_search(self): focused_field = self._get_focused_field() x_s = focused_field[0] y_s = focused_field[1] lowest_value = 100 best_field = None for i in range(1, len(self._grid.field)): for x_diff in range(-i, i): y_diff = i - abs(x_diff) x1 = x_s + x_diff y1 = y_s + y_diff x2 = x_s - x_diff y2 = y_s - y_diff if x1 < len(self._grid.field) and y1 < len(self._grid.field[0]) and \ self._grid.field[x1][y1] < lowest_value: best_field = [x1, y1] lowest_value = self._grid.field[x1][y1] if x2 < len(self._grid.field) and y2 < len(self._grid.field[0]) and \ self._grid.field[x2][y2] < lowest_value: best_field = [x2, y2] lowest_value = self._grid.field[x2][y2] if lowest_value == 0: break # ends both loops if inner loop breaks, kinda hacky but should work else: continue break rospy.logdebug('Focused field: %s Field to search: %s' % (str(focused_field), str(best_field))) place_to_search = PointStamped() place_to_search.point = Point(best_field[0], best_field[1], 0) place_to_search.header.frame_id = '/odom_combined' rospy.logdebug('Place to search %s' % str(place_to_search)) visualization.publish_vector(self._grid.coordinates[best_field[0], best_field[1]], 9999) return place_to_search
def update_search_grid(self): # TODO Get planning scene from moveit service # TODO Get gripper pose and calculate visible fields # Create a PoseStamped for the camera camera_pose = self._get_camera_pose() rospy.logdebug('Camera pose: %s' % str(camera_pose)) # Get the points for the fov fov_points = self._get_fov_points() rospy.logdebug('FOV points: %s' % str(fov_points)) # Get the intersection points of the fov lines with the table table_points = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]] point_to_vector = lambda point: scipy.array( [point.x, point.y, point.z]) intersection_points = map( lambda fov_point: intersection_line_plane( point_to_vector(camera_pose.pose.position), point_to_vector(fov_point.point), *table_points), fov_points) rospy.logdebug('Intersection points fov lines with table: %s' % str(intersection_points)) # Update visible fields visible_fields = self._get_visible_fields(intersection_points) for pos in visible_fields: self._grid.field[pos[0], pos[1]] += 1 rospy.loginfo('Visible fields: %s' % str(visible_fields)) # Visualize search grid visualization.publish_marker_array(self._grid.to_marker_array()) # Visualize fov visualization.publish_lines( camera_pose.pose.position, map(lambda point_stamped: point_stamped.point, fov_points)) # Visualize intersection fov lines with table map(lambda vec, name: visualization.publish_vector(vec, name), intersection_points, [5000, 6000, 7000, 8000])
def update_search_grid(self): # TODO Get planning scene from moveit service # TODO Get gripper pose and calculate visible fields # Create a PoseStamped for the camera camera_pose = self._get_camera_pose() rospy.logdebug('Camera pose: %s' % str(camera_pose)) # Get the points for the fov fov_points = self._get_fov_points() rospy.logdebug('FOV points: %s' % str(fov_points)) # Get the intersection points of the fov lines with the table table_points = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]] point_to_vector = lambda point: scipy.array([point.x, point.y, point.z]) intersection_points = map(lambda fov_point: intersection_line_plane(point_to_vector(camera_pose.pose.position), point_to_vector(fov_point.point), *table_points), fov_points) rospy.logdebug('Intersection points fov lines with table: %s' % str(intersection_points)) # Update visible fields visible_fields = self._get_visible_fields(intersection_points) for pos in visible_fields: self._grid.field[pos[0], pos[1]] += 1 rospy.loginfo('Visible fields: %s' % str(visible_fields)) # Visualize search grid visualization.publish_marker_array(self._grid.to_marker_array()) # Visualize fov visualization.publish_lines(camera_pose.pose.position, map(lambda point_stamped: point_stamped.point, fov_points)) # Visualize intersection fov lines with table map(lambda vec, name: visualization.publish_vector(vec, name), intersection_points, [5000, 6000, 7000, 8000])