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])
