def get_robot_roi(self):
"""Find the roi of the robot at it's waypoint - in case no target ROI is passed
Make sure the randomly generated viewpoints are all in this roi also - i.e. this room
"""
self.robot_polygon = None
# for (roi, meta) in self.soma_roi_store.query(SOMAROIObject._type): # OLD SOMA
query = SOMAQueryROIsRequest(query_type=0,
roiconfigs=[self.soma_config],
returnmostrecent=True)
for roi in self.soma_query(query).rois:
if roi.map_name != self.soma_map: continue
if roi.config != self.soma_config: continue
#if roi.geotype != "Polygon": continue
polygon = Polygon([(p.position.x, p.position.y)
for p in roi.posearray.poses])
if polygon.contains(
Point([
self.robot_pose.position.x, self.robot_pose.position.y
])):
rospy.loginfo("Robot (%s,%s) in ROI: %s" %
(self.robot_pose.position.x,
self.robot_pose.position.y, roi.type))
self.robot_polygon = polygon
return True
rospy.logwarn("This waypoint is not defined in a ROI")
return False
def handle_current_area_service(self, req):
"""
Will handle the get_current_map_area service call. This function will get the XY coordinates of a point in parameter and determine in which room this point is located.
Returns a response containing the room name.
:param req: input parameters of the service
:type action: CurrentArea
"""
x_robot = req.coord_x
y_robot = req.coord_y
point_to_check = Point(x_robot, y_robot)
for room in self.coord_IMs.keys():
list_points = []
for i in range(0, len(self.coord_IMs[room].keys())):
list_points.append((self.coord_IMs[room][str(i)]['x'],
self.coord_IMs[room][str(i)]['y']))
self.polygon_room = Polygon(list_points)
validation = self.polygon_room.contains(point_to_check)
if validation == True:
return CurrentAreaResponse(room)
return CurrentAreaResponse('')
def map_callback(map):
global pub
print("map received, building contours...")
grid = np.array(map.data)
grid = np.resize(grid, (map.info.height, map.info.width))
contours = measure.find_contours(grid, 0.8)
print("done making contours!")
array = PolygonArray()
for n, contour in enumerate(contours):
new_s = contour.copy()
appr_s = approximate_polygon(new_s, tolerance=5)
#coords = [(c[1], c[0]) for c in contour]
#print(coords)
#poly = shapely.geometry.Polygon(coords)
#plt.plot(appr_s[:, 1], appr_s[:, 0], linewidth=1)
#plt.plot(contour[:, 1], contour[:, 0], linewidth=2)
poly = Polygon()
poly.points = [Point(a[1], a[0], 0) for a in appr_s]
if len(poly.points) > 3:
array.polygons.append(poly)
pub.publish(array)
def callback_cmt(self, data):
print "received data: ", data
if data.points[0].z > 20:
width = max(abs(data.points[1].x - data.points[0].x),
abs(data.points[2].x - data.points[0].x),
abs(data.points[3].x - data.points[0].x))
height = max(abs(data.points[1].y - data.points[0].y),
abs(data.points[2].y - data.points[0].y),
abs(data.points[3].y - data.points[0].y))
center_x = min(data.points[0].x, data.points[1].x,
data.points[2].x, data.points[3].x) + 0.5 * width
center_y = min(data.points[0].y, data.points[1].y,
data.points[2].y, data.points[3].y) + 0.5 * height
Z = np.array([[np.float32(center_x)], [np.float32(center_y)],
[np.float32(width)], [np.float32(height)]])
self.kalman_filter.update(Z)
print("kalman filter gain:")
print self.kalman_filter.kalman.gain
new_data = Polygon()
new_data.points = [
Point32(),
Point32(),
Point32(),
Point32(),
Point32(),
Point32()
]
new_data.points[0].x = self.kalman_filter.current_prediction[
0] # center_x
new_data.points[0].y = self.kalman_filter.current_prediction[
1] # center_y
new_data.points[1].x = self.kalman_filter.current_prediction[
2] # width
new_data.points[1].y = self.kalman_filter.current_prediction[
3] # height
new_data.points[
2].x = new_data.points[0].x + 0.5 * new_data.points[1].x
new_data.points[
2].y = new_data.points[0].y + 0.5 * new_data.points[1].y
new_data.points[
3].x = new_data.points[0].x + 0.5 * new_data.points[1].x
new_data.points[
3].y = new_data.points[0].y - 0.5 * new_data.points[1].y
new_data.points[
4].x = new_data.points[0].x - 0.5 * new_data.points[1].x
new_data.points[
4].y = new_data.points[0].y - 0.5 * new_data.points[1].y
new_data.points[
5].x = new_data.points[0].x - 0.5 * new_data.points[1].x
new_data.points[
5].y = new_data.points[0].y + 0.5 * new_data.points[1].y
self.pub_kalman.publish(new_data)
def read_field_file(self):
# Setup msgs
self.safety_msg = PolygonStamped()
self.boundry_msg = PolygonStamped()
self.cut_area_msg = PolygonStamped()
self.safety_msg.header.stamp = rospy.Time.now()
self.safety_msg.header.frame_id = self.field_frame_id
self.boundry_msg.header = self.safety_msg.header
self.cut_area_msg.header = self.safety_msg.header
# Parse out the points
polygon_points = []
polygon_points32 = []
point_count = 0
for point in self.field_polygon:
point_count += 1
if point['fix_type'] < 3:
rospy.logwarn('Point %i has a low quality fix type of %i'
% (point_count, point['fix_type']))
(easting, northing) = (point['easting'], point['northing'])
polygon_points.append((float(easting), float(northing)))
polygon_points32.append(Point32(float(easting), float(northing), 0))
# Put the points into the boundry_msg
self.boundry_msg.polygon = Polygon(polygon_points32)
# Expand and contract the field shape for safety buffer and cut area
safety_points = self.offset_polygon(polygon_points, 2)
cut_area_points = self.offset_polygon(polygon_points, -0.5)
self.safety_msg.polygon = Polygon(safety_points)
self.cut_area_msg.polygon = Polygon(cut_area_points)
return True
def publish_obstacle_msg():
rospy.init_node("test_obstacle_msg")
pub = rospy.Publisher('/transparent_obstacle', Polygon, queue_size=1)
obstacle_msg = Polygon()
# Add polygon obstacle
v1 = Point32()
v1.x = -3
v1.y = 3
v2 = Point32()
v2.x = -4
v2.y = 4
v3 = Point32()
v3.x = -6
v3.y = 3
v4 = Point32()
v4.x = -7
v4.y = 4
obstacle_msg.points = [v1, v2, v3, v4]
# obstacle_msg.points = [v1, v2]
pub.publish(obstacle_msg)
r = rospy.Rate(10) # 10hz
# t = 0.0
while not rospy.is_shutdown():
# # Vary y component of the point obstacle
# obstacle_msg.points[0].y = 1*math.sin(t)
# t = t + 0.1
pub.publish(obstacle_msg)
r.sleep()
def publish_filtered_white_points(self, white_points):
# publish the filtered white points:
filtered_points_msg = Polygon()
filtered_points_msg.points = [
Point32(p[0], p[1], 0.) for p in white_points
]
self.logdebug("Publishing {} new white points".format(
len(filtered_points_msg.points)))
self.pub_filtered_white_points.publish(filtered_points_msg)
def publish_filtered_yellow_points(self, yellow_points):
# Publish the filtered yellow points:
filtered_points_msg = Polygon()
filtered_points_msg.points = [
Point32(p[0], p[1], 0.) for p in yellow_points
]
self.logdebug("Publishing {} new yellow points".format(
len(filtered_points_msg.points)))
self.pub_filtered_yellow_points.publish(filtered_points_msg)
def publish_polygon_voronoi(data,scale):
global polygon_voronoi_publisher
polygon_msg = Polygon()
polygon_msg.points = []
for i in range(len(data)):
point_temp = Point32()
point_temp.x = data[i][0]*scale
point_temp.y = data[i][1]*scale
point_temp.z = 0
polygon_msg.points.append(point_temp)
polygon_voronoi_publisher.publish(polygon_msg)
def test_polygon(self):
"""Test passing and getting a Polygon message"""
interface_name = 'polygon_' + self.interface_type
getter_service = 'lama_interfaces/GetPolygon'
setter_service = 'lama_interfaces/SetPolygon'
# Set up node as well as getter and setter services.
iface = self.interface_factory(interface_name, getter_service,
setter_service)
get_srv = rospy.ServiceProxy(iface.getter_service_name,
iface.getter_service_class)
set_srv = rospy.ServiceProxy(iface.setter_service_name,
iface.setter_service_class)
polygon = Polygon()
descriptor_from_setter = set_srv(polygon)
# descriptor_from_setter cannot be passed to get_srv because of
# type incompatibility, "transform" it to a ..._getRequest()
descriptor_to_getter = GetPolygonRequest()
descriptor_to_getter.id = descriptor_from_setter.id
response = get_srv(descriptor_to_getter)
self.assertIsNot(polygon, response.descriptor)
for point_out, point_in in zip(polygon.points,
response.descriptor.points):
self.assertAlmostEqual(point_out.x, point_in.x, places=5)
self.assertAlmostEqual(point_out.y, point_in.y, places=5)
self.assertAlmostEqual(point_out.z, point_in.z, places=5)
polygon = Polygon()
polygon.points.append(Point32())
polygon.points[0].x = 45.6
polygon.points[0].y = -34.0
polygon.points[0].z = -26.4
polygon.points.append(Point32())
polygon.points[1].x = -5.6
polygon.points[1].y = -3.0
polygon.points[1].z = 6.4
descriptor_from_setter = set_srv(polygon)
# descriptor_from_setter cannot be passed to get_srv because of
# type incompatibility, "transform" it to a ..._getRequest()
descriptor_to_getter = GetPolygonRequest()
descriptor_to_getter.id = descriptor_from_setter.id
response = get_srv(descriptor_to_getter)
self.assertIsNot(polygon, response.descriptor)
for point_out, point_in in zip(polygon.points,
response.descriptor.points):
self.assertAlmostEqual(point_out.x, point_in.x, places=5)
self.assertAlmostEqual(point_out.y, point_in.y, places=5)
self.assertAlmostEqual(point_out.z, point_in.z, places=5)
def start_map():
starting_coords = (42.293173, -71.263540) # my apartment...
manager = MapManager(MAP_HEIGHT, ZOOM, starting_coords[0],
starting_coords[1])
# BGR colors
RED = cv2.cv.Scalar(0, 0, 255)
YELLOW = cv2.cv.Scalar(0, 180, 180)
cv2.namedWindow(WINDOW_NAME, cv2.CV_WINDOW_AUTOSIZE)
cv2.cv.SetMouseCallback(WINDOW_NAME, manager.mouse_callback)
rospy.init_node('map_manager')
pub = rospy.Publisher('google_waypoints', Polygon, queue_size=10)
r = rospy.Rate(10)
polygon = Polygon()
if rospy.is_shutdown():
print 'borked'
while not rospy.is_shutdown():
points = manager.get_plotted_points_as_x_y_list()
for i in range(len(points)):
cv2.circle(manager.img,
center=points[i],
radius=5,
color=RED,
thickness=-1)
if i > 0:
cv2.line(manager.img,
pt1=points[i - 1],
pt2=points[i],
color=YELLOW,
thickness=2)
cv2.imshow(WINDOW_NAME, manager.img)
r.sleep()
polygon.points = []
for location in manager.plotted_points:
point = Point32()
point.x = location[0]
point.y = location[1]
point.z = 0
polygon.points.append(point)
pub.publish(polygon)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows
cv2.waitKey(1)
print manager.plotted_points
def publish_triangle_polygon(self, pose):
pts = []
pts.append(tfx.pose((0,0.0079,0)))
pts.append(tfx.pose((0,-0.0079,0)))
pts.append(tfx.pose((0,0, -0.013)))
for i in range(len(pts)):
# IPython.embed()
pts[i] = (pose.as_tf()*pts[i]).position.msg.Point()
polygon = Polygon()
polygon_stamped = PolygonStamped()
polygon.points = pts
polygon_stamped.polygon = polygon
polygon_stamped.header = pose.msg.Header()
self.triangle_polygon_publisher.publish(polygon_stamped)
def to_poly(path):
poly = Polygon()
poly.points = []
if path is None:
return poly
try:
for pt in path:
point = Point()
point.x = pt[0]
point.y = pt[1]
point.z = 0.0
poly.points.append(point)
return poly
except TypeError:
return poly
def __init__(self):
# Get ROS parameters
self.robot_ = Polygon()
for vertex in rospy.get_param('~robot'):
p = Point32()
p.x = vertex[0]
p.y = vertex[1]
self.robot_.points.append(p)
self.allow_unknown_ = rospy.get_param('~allow_unknown', True)
res_deg = rospy.get_param('~theta_resolution_deg', 1.0)
self.theta_resolution_ = res_deg * math.pi / 180
self.occupancy_threshold_ = rospy.get_param('~occupancy_threshold', 10)
self.set_config()
self.service_ = rospy.ServiceProxy('collision_check', Check)
self.pub_footprint_ = rospy.Publisher('footprint',
OccupancyGrid,
queue_size=1)
self.pub_robot_ = rospy.Publisher('robot',
MarkerArray,
queue_size=1)
rospy.Subscriber('initialpose',
PoseWithCovarianceStamped,
self.cb_pose,
queue_size=1)
def to_obsarray():
array = ObstacleArrayMsg()
array.header.frame_id = 'field'
oid = [0]
def next_id():
oid[0] += 1
return oid[0] - 1
red_line = [to_point((0, 4.1)), to_point((1.9, 6))]
blue_line = [to_point((4.1, 0)), to_point((6, 1.9))]
for l in (blue_line, red_line):
obs = ObstacleMsg(polygon=Polygon(l))
obs.id = next_id()
array.obstacles.append(obs)
for stationary in ((2, 4), (4, 2)):
obs = ObstacleMsg(polygon=to_square(stationary, 0.127))
obs.id = next_id()
array.obstacles.append(obs)
for color, pos in world.goals:
obs = ObstacleMsg(polygon=to_square(pos, 0.127))
obs.id = next_id()
array.obstacles.append(obs)
for pos in world.cones:
obs = ObstacleMsg(polygon=to_square(pos, 0.076))
obs.id = next_id()
array.obstacles.append(obs)
return array
def format_msg(src, Obj, s=2):
assert type(Obj)==type and 'msg' in str(Obj)
from sensor_msgs.msg import RegionOfInterest
from geometry_msgs.msg import Point32, Polygon
hd = hasattr(src,'header')
if type(src)==str: im = cv2.imread(src)
elif type(src)==np.ndarray: im = src.copy()
elif hd: im = CVB.compressed_imgmsg_to_cv2(src)
obj = Obj(header=src.header) if hd else Obj()
im, res = get_instances(im, HSV_Set, {}, s)
if not res: return obj, im, res
if hasattr(Obj,'class_ids'): obj.class_ids = res['cid']
if hasattr(Obj,'class_names'): obj.class_names = res['cls']
if hasattr(Obj,'scores'): obj.scores = [1.0]*len(res['cls'])
if hasattr(Obj,'boxes'): obj.boxes = [RegionOfInterest(x_offset=x,
y_offset=y, width=w, height=h) for (x,y,w,h) in res['box']]
if hasattr(Obj,'contours'): obj.contours = [Polygon([Point32(
x=x, y=y) for (x,y) in c.transpose()]) for c in res['cnt']]
elif hasattr(Obj,'masks'): # for masks
for mk in res['mask']: # get masks
m = CVB.cv2_to_imgmsg(mk.astype('uint8')*255, 'mono8')
m.header = obj.header; obj.masks.append(m)
return obj, im, res
def main():
rospy.init_node('kobuki_exploration')
area_to_explore = PolygonStamped()
center_point = PointStamped()
now = rospy.Time.now()
area_to_explore.header.stamp = now
area_to_explore.header.frame_id = "map"
points = [
Point32(-2.65, -2.56, 0.0),
Point32(5.41, -2.7, 0.0),
Point32(5.57, 4.44, 0.0),
Point32(-2.75, 4.37, 0.0)
]
area_to_explore.polygon = Polygon(points)
center_point.header = area_to_explore.header
center_point.point.x = 1.83
center_point.point.y = 1.57
center_point.point.z = 0.0
brain = PR2RobotBrain(area_to_explore, center_point)
brain.getReady()
brain.loop()
def find_footprint(self, points):
"""This function will take in an numpy.array of points in 2D and create
a convex polygon that encapsilates every point provided.\
Arguments to pass in:
points = 2D array of points numpy.array([:,:])
Returns a Polygon"""
footprint = Polygon()
if self.halo_density:
buffed_points = self.halo_buffer(points, self.halo_radius,
self.halo_density)
else:
buffed_points = points
if not points.size:
pass
else:
hull = spatial.ConvexHull(buffed_points)
for vertex in hull.vertices:
point = Point32()
point.x = buffed_points[vertex, 0]
point.y = buffed_points[vertex, 1]
point.z = 0.0
footprint.points.append(point)
return footprint
def get_roi_to_observe(self, roi_id, roi_config):
"""Get the roi to observe.
Select objects to observe based upon this region - i.e. the recommended interesting roi
"""
observe_polygon = None
if roi_id != "":
# for (roi, meta) in self.soma_roi_store.query(SOMAROIObject._type):
query = SOMAQueryROIsRequest(query_type=0,
roiids=[roi_id],
roiconfigs=[roi_config],
returnmostrecent=True)
response = self.soma_query(query)
for roi in response.rois:
if roi.map_name != self.soma_map: continue
if roi.config != roi_config: continue
if roi.id != roi_id: continue
#if roi.geotype != "Polygon": continue
observe_polygon = Polygon([(p.position.x, p.position.y)
for p in roi.posearray.poses])
rospy.loginfo("Observe ROI: %s" % roi.type)
if observe_polygon == None:
rospy.logwarn("ROI given to observe not found")
else:
rospy.logwarn("No ROI given to observe - use robot ROI")
observe_polygon = self.robot_polygon
return observe_polygon
def update(self):
while not rospy.is_shutdown():
if self.state == 'init':
rospy.loginfo("state is {}".format(self.state))
if self.pose_goal_set == False or self.pose_start_set == False or self.node_start == None or self.node_goal == None:
rospy.logwarn("Start or goal point not set yet")
else:
self.nodes = [self.node_start]
self.state = 'build tree'
elif self.state == 'build tree':
rospy.loginfo("state is {}".format(self.state))
self.state, self.nodes = self.build_tree()
elif self.state == 'complete':
rospy.loginfo("state is {}".format(self.state))
node_current = self.nodes[-1]
path = []
while node_current.parent != None:
# pose_current = Point32(
# node_current.point[0], node_current.point[1], 0.0)
pose_parrent = Point32(node_current.parent.point[0],
node_current.parent.point[1], 0.0)
path.append(pose_parrent)
# path.append(pose_current)
self.pub_path.publish(Polygon(path))
node_current = node_current.parent
self.rate.sleep()
self.state = 'done'
elif self.state == 'incomplete':
rospy.loginfo("state is {}".format(self.state))
rospy.loginfo("Cannot find path!")
else:
break
self.rate.sleep()
if rospy.is_shutdown():
del self.nodes[:]
def process_image(self, data):
if self.no_one_listening():
return
begin_processing = rospy.Time.now()
if begin_processing - self.last_detection < self.detection_interval:
return
# Unpackage image from ros message
cv_im = self.unpack_image(data)
rectangles = self.detector.apply_frame(cv_im)
# Use Super Polygon
super_polygon = []
for rect in rectangles:
super_polygon.append(Point32(x=rect[0][0], y=rect[0][1], z=0))
super_polygon.append(Point32(x=rect[1][0], y=rect[1][1], z=0))
super_polygon.append(Point32(x=rect[2][0], y=rect[2][1], z=0))
super_polygon.append(Point32(x=rect[3][0], y=rect[3][1], z=0))
self.people_pub.publish(Polygon(super_polygon)) # Publish to teleop_husky node
self.publish_viz(rectangles, cv_im)
return
def callback_marker(self,marker):
if self.npoint:
self.npoint=False
point = Point32()
self.msg_poly.header.stamp = rospy.Time.now()
self.msg_poly.header.frame_id = "/mocap"
point.x = marker.pose.position.x
point.y = marker.pose.position.y
point.z = marker.pose.position.z
print('appending point')
self.polygon.points.append(point)
self.msg_poly.polygon = self.polygon
elif self.new:
self.new = False
self.polygon = Polygon()
self.msg_poly = PolygonStamped()
self.pc += 1
print('add new polygon')
elif self.save:
self.save = False
df = pu.ros_to_pd(self.polygon)
df = pu.interpolate(df, 0.1)
#self.old_poly = pu.pandas_to_ros(df)
pu.save(df, 'polygons/poly_' + str(self.sc) + '_' + str(self.pc) + '.csv')
print('ploygon saved')
pub.publish(self.msg_poly)
def mock_road_obstacle():
pub = rospy.Publisher('/obstacles', PolygonArray, queue_size=10)
pub_r = rospy.Publisher('/road_markings', RoadMarkings, queue_size=10)
sub = rospy.Subscriber('/setpoint', Float32, callback)
rospy.init_node('mock_road_obstacle', anonymous=True)
marking = RoadMarkings
marking.left_line = [2, 0, 0]
marking.center_line = [0.59, 0, 0]
marking.right_line = [-1.5, 0, 0]
print('RoadMarkings sent')
pub_r.publish(marking)
point = Point32(x=0.4, y=-0.15, z=0)
polygon = Polygon()
polygon.points.append(point)
point.y = 0.15
polygon.points.append(point)
point.x = 0.45
polygon.points.append(point)
point.y = -0.15
polygon.points.append(point)
polygons = PolygonArray()
polygons.polygons.append(polygon)
pub.publish(polygons)
print('polygon sent')
rospy.spin()
def createArea(frame):
poly = PolygonStamped()
poly.header.frame_id = frame
poly.header.stamp = rospy.Time.now()
poly.polygon = Polygon()
poly.polygon.points = exploration_area_vertices
return poly
def handle_image(self, data):
if self.no_one_listening():
return
begin_processing = rospy.Time.now()
if begin_processing - self.last_detection < self.detection_interval:
return # don't do anything unless enough time has elapsed
# convert from ros message to openCV image
cv2_image = self.ros_msg_to_cv2(data)
rectangles = self.detector.apply_frame(cv2_image)
# publish super_polygon
super_polygon = []
for rect in rectangles:
super_polygon.append(Point32(x=rect[0][0], y=rect[0][1], z=0))
super_polygon.append(Point32(x=rect[1][0], y=rect[1][1], z=0))
super_polygon.append(Point32(x=rect[2][0], y=rect[2][1], z=0))
super_polygon.append(Point32(x=rect[3][0], y=rect[3][1], z=0))
self.people_publisher.publish(Polygon(super_polygon))
self.publish_viz(rectangles, cv2_image)
elapsed = rospy.Time.now() - begin_processing
# adjust frame processing rate to match detector rate,
# plus a small margin
self.detection_interval = rospy.Duration(elapsed.to_sec() + 0.1)
def generate_road_poly_msg(self): # update road poly
# generate road poly list
road_poly_list = []
for pose in self.env.env.road_poly:
p1_l = [pose[0][0][0], pose[0][0][1]]
p1_r = [pose[0][1][0], pose[0][1][1]]
p2_r = [pose[0][2][0], pose[0][2][1]]
p2_l = [pose[0][3][0], pose[0][3][1]]
if pose[1] != (1, 1, 1) and pose[1] != (
1, 0, 0): # get rid of corner markers
road_poly_list.append([p1_l, p1_r, p2_r, p2_l])
# inflate road poly
road_poly_inflated = inflate(road_poly_list, self.inflation_radius)
# create road poly message
road_poly_array = []
for poly in road_poly_inflated:
p1_l = Point32(poly[0][0], poly[0][1], 0) # z is always 0
p1_r = Point32(poly[1][0], poly[1][1], 0)
p2_r = Point32(poly[2][0], poly[2][1], 0)
p2_l = Point32(poly[3][0], poly[3][1], 0)
road_poly = Polygon([
p1_l, p1_r, p2_r, p2_l, p1_l
]) # add first again so that it works with point in poly check
road_poly_array.append(road_poly)
road_poly_msg = RoadPoly(road_poly_array)
# send road poly message
time.sleep(1.0)
self.road_poly_pub.publish(road_poly_msg)
time.sleep(1.0)
def build_polygon_msg(hull):
msg = Polygon()
for vertex_idx in hull.vertices:
msg.points.append(
Point32(hull.points[vertex_idx, 0], hull.points[vertex_idx, 1],
0))
return msg
def __init__(self, **kwargs):
assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
"Invalid arguments passed to constructor: %r" % kwargs.keys()
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
from geometry_msgs.msg import Polygon
self.polygon = kwargs.get('polygon', Polygon())
def setup(self):
self.inflation_radius = float(
rospy.get_param('inflation_radius', '0.0'))
#points = rospy.get_param('roi', '[]')
roi = rospy.get_param('roi', [])
points = []
for point in roi:
rospy.loginfo('Point: %s', point)
points.append(Point32(float(point[0]), float(point[1]), 0))
polygon = Polygon(points)
self.roi = polygon
self.views_at_pose = int(rospy.get_param('views_at_pose', '8'))
self.min_pan = float(rospy.get_param('min_pan', '-2.09'))
self.max_pan = float(rospy.get_param('max_pan', '2.09'))
self.min_tilt = float(
rospy.get_param('min_tilt', '-0.22')
) # self.min_tilt = float(rospy.get_param('min_tilt', '-0.22'))
self.max_tilt = float(rospy.get_param('max_tilt', '0.52'))
self.velocity = float(rospy.get_param('velocity', '1.0'))
rospy.loginfo("Wait for nav_goals")
rospy.wait_for_service('nav_goals')
rospy.loginfo("Done (nav_goals)")
try:
self.nav_goals = rospy.ServiceProxy('nav_goals', NavGoals)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def pose_callback(self, pose_msg):
'''
Uses a pose message to generate an odometry and state message.
:param pose_msg: (geometry_msgs/PoseStamped) pose message
'''
new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map",
pose_msg.header)
if not self.use_amcl:
pose_msg = new_pose_msg
self.new_pose_pub.publish(
pose_msg
) # if using AMCL, this will be the same as the original pose message
if trans and rot: # if not getting tf, trans and rot will be None
footprint = PolygonStamped()
footprint.header = pose_msg.header # has same frame_id (map) and time stamp
loomo_points = np.array([[0.16, -0.31], [0.16, 0.31],
[-0.16, 0.31], [-0.16, -0.31]])
roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot)
rot = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array
rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]])
polygon = Polygon(
points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T])
footprint.polygon = polygon
self.footprint_pub.publish(footprint)
odom_msg = Odometry()
odom_msg.pose.pose = pose_msg.pose
odom_msg.header = pose_msg.header
self.odom_pub.publish(odom_msg)
state_msg = State()
state_msg.header = pose_msg.header
state_msg.state_stamp = pose_msg.header.stamp
state_msg.pos.x = pose_msg.pose.position.x
state_msg.pos.y = pose_msg.pose.position.y
state_msg.pos.z = pose_msg.pose.position.z
state_msg.quat.x = pose_msg.pose.orientation.x
state_msg.quat.y = pose_msg.pose.orientation.y
state_msg.quat.z = pose_msg.pose.orientation.z
state_msg.quat.w = pose_msg.pose.orientation.w
if self.last_state_time and self.last_state:
dt = pose_msg.header.stamp.nsecs - self.last_state_time
state_msg.vel.x = (state_msg.pos.x -
self.last_state.pos.x) / (float(dt) / 10**9)
state_msg.vel.y = (state_msg.pos.y -
self.last_state.pos.y) / (float(dt) / 10**9)
state_msg.vel.z = 0 # ground robot not traveling in z direction
self.last_state_time = pose_msg.header.stamp.nsecs
self.last_state = state_msg
self.state_pub.publish(state_msg)
def __init__(self):
print 'status init'
self.depth = PointCloud()
self.image = Image()
self.roi = Polygon()
self.humans = Humans()
self.objects = Objects()
self.robots = Robots()
def get_polygon(self, roi_id):
objs = self._msg_store.query(SOMAROIObject._type, message_query={"map": self.soma_map,
"config": self.soma_conf,
"roi_id": roi_id})
ids = []
poses = []
for o,om in objs:
ids.append(o.id)
poses.append(o.pose)
sorted_poses = [_pose for (_id,_pose) in sorted(zip(ids, poses))]
poly = Polygon()
poly.points = []
for p in sorted_poses:
point = Point()
point.x = p.position.x
point.y = p.position.y
poly.points.append(point)
return poly
def start_map():
starting_coords = (42.293173, -71.263540)
manager = MapManager(MAP_HEIGHT, ZOOM, starting_coords[0], starting_coords[1])
# BGR colors
RED = cv2.cv.Scalar(0, 0, 255)
YELLOW = cv2.cv.Scalar(0, 180, 180)
cv2.namedWindow(WINDOW_NAME, cv2.CV_WINDOW_AUTOSIZE)
cv2.cv.SetMouseCallback(WINDOW_NAME, manager.mouse_callback)
rospy.init_node('map_manager')
pub = rospy.Publisher('google_waypoints', Polygon, queue_size=10)
r = rospy.Rate(10)
polygon = Polygon()
if rospy.is_shutdown():
print 'borked'
while not rospy.is_shutdown():
points = manager.get_plotted_points_as_x_y_list()
for i in range(len(points)):
cv2.circle(manager.img, center=points[i], radius=5, color=RED, thickness=-1)
if i > 0:
cv2.line(manager.img, pt1=points[i-1], pt2=points[i], color=YELLOW, thickness = 2)
cv2.imshow(WINDOW_NAME, manager.img)
r.sleep()
polygon.points = []
for location in manager.plotted_points:
point = Point32()
point.x = location[0]
point.y = location[1]
point.z = 0
polygon.points.append(point)
pub.publish(polygon)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows
cv2.waitKey(1)
print manager.plotted_points
def poly_to_ros(p):
newpoly = RosPolygon()
newpoly.points = [Point32(x,y,0) for x,y in p]
return newpoly
if __name__ == '__main__':
rospy.init_node('test_tables', anonymous = False)
nx = Table()
msg_store=MessageStoreProxy()
my_table = Table()
my_table.table_id = "SouthWestTable"
my_table.header.frame_id = "/map" # The parent frame that the table is in
table_pose = PoseWithCovariance() # The transformation to the table frame
# Fill in the table position...
my_table.pose = table_pose
polygon = Polygon() # The table top surrounding polygon in the table frame
# Fill in the points in the polygon...
polygon.points = [
Point32(4,9.7,0.95),
Point32(4,8.8,0.95),
Point32(2.1,8.8,0.95),
Point32(2.1,9.7,0.95),
]
my_table.tabletop = polygon
rospy.loginfo(polygon)
# Store the table
msg_store.insert(my_table)
