class InfoGatherer(object):
'''
classdocs
'''
def __init__(self):
# Listen to lines for affordances
sub = message_filters.Subscriber("/lines", Lines)
self.linesCache = message_filters.Cache(sub, 200)
# Listen to visualization_markers for markers
sub = message_filters.Subscriber("/visualization_marker", Marker)
self.markersCache = message_filters.Cache(sub, 200)
self.mover = JointMover()
# Fix pitch
self.mover.move("HeadPitch", [.7], .5)
rospy.loginfo( "Gatherer initiated")
def gather(self):
movements = [[.5],[-.5],[0]]
lines = []
markers = []
for m in movements:
self.mover.move("HeadYaw", m, .5)
rospy.sleep(.5)
# Collect messages away from movement
start = rospy.Time.now()
for i in range(5):
self.linesCache.waitForTimedMessage(start)
# Cant wait for marker Messages as are published only on discovery
#self.markersCache.waitForTimedMessage(start)
lines += self.linesCache.getInterval(start, rospy.Time.now())
markers += self.markersCache.getInterval(start, rospy.Time.now())
# print lines
# print markers
# Filter out the lines from the markerArray
onlyLines = []
for d in lines:
onlyLines += d.lines
aff = AffordanceCalc().calcAffordances(onlyLines)
print "Markers", markers
marks = MarkerCalc().calcMarkers(markers)
return aff, marks
class Affordances(object):
floorFrame = "/l_sole"
affCalc = AffordanceCalc()
def __init__(self):
sub = message_filters.Subscriber("/lines", Lines)
self.cache = message_filters.Cache(sub, 200)
self.mover = JointMover()
# Fix pitch
self.mover.move("HeadPitch", [.7], .5)
rospy.loginfo( "Affordances initiated")
self.pubMarker = rospy.Publisher("/triangles",MarkerArray)
def getAffordances(self):
movements = [[.5],[-.5],[0]]
data = []
# movements = [[0]]
for m in movements:
self.mover.move("HeadYaw", m, .5)
rospy.sleep(.5)
# Collect messages away from movement
start = rospy.Time.now()
for i in range(5):
self.cache.waitForTimedMessage(start)
data += self.cache.getInterval(start, rospy.Time.now())
lines = []
for d in data:
lines += d.lines
# for m in d.markers:
# if len(m.points) > 0:
# # Get all lines from points - usually should be just one line per data item
# p = m.points[0]
# for i in range(1,len(m.points)):
# s = m.points[i]
# lines.append((Point(p.x,p.y,0),Point(s.x,s.y,0)))
# p = s
return self.affCalc.calcAffordances(lines)
import roslib; roslib.load_manifest('morris_simulation')
import rospy
from moveJoint import JointMover
from naoqi import ALProxy
if __name__ == '__main__':
rospy.init_node("move ankle")
ankle = "LAnklePitch"
time = 1
# Enable stiffness
try:
motionProxy = ALProxy("ALMotion", "127.0.0.1", 9559)
motionProxy.wakeUp()
jm = JointMover()
rospy.loginfo("Going to starting position")
jm.move(ankle, [0.922747], [10])
motionProxy.rest()
rospy.sleep(time)
motionProxy.wakeUp()
rospy.loginfo("At Starting position")
for i in range(3):
rospy.loginfo("Going to other end")
jm.move(ankle, [-1.189516], [time])
rospy.loginfo("At the other end")
motionProxy.rest()
rospy.sleep(time)
motionProxy.wakeUp()
rospy.loginfo("Going to starting position again")
jm.move(ankle, [0.922747], [time])
rospy.loginfo("At the starting position again")
motionProxy.rest()