def blob_to_msg(img, blob):
from viz_feature_sim.msg import Blob
b = Blob()
b.size = blob.size
b.bearing = pix_x_to_bearing(img, blob.pt[0])
pixel = img[int(blob.pt[1])][int(blob.pt[0])]
b.color.b = pixel[0]
b.color.g = pixel[1]
b.color.r = pixel[2]
return b
def core_to_rosmsg(reading_list):
temp = []
for reading in reading_list:
blob = Blob()
blob.bearing = reading.bearing
blob.size = reading.size
blob.color.r = reading.r
blob.color.g = reading.g
blob.color.b = reading.b
temp.append(blob)
return temp
def easy_observation(odom, feature):
'''
helper method to create Blobs to use in navigation calculations
'''
blob = Blob()
heading = math.atan2(feature.y-odom.pose.pose.position.y,
feature.x-odom.pose.pose.position.x)
blob.bearing = heading - quaternion_to_heading(odom.pose.pose.orientation)
rospy.loginfo('h | b || %f | %f' % (heading, quaternion_to_heading(odom.pose.pose.orientation),))
blob.size = feature.red
blob.color.r = feature.red
blob.color.g = feature.green
blob.color.b = feature.blue
return blob
