def pub_transformed():
pub = rospy.Publisher('pub_transformed', PoseStampedArray, queue_size=10)
rospy.init_node('pub_transformed', anonymous=True)
rate = rospy.Rate(1) # 10hz
b = Bagger(filename=constants.DEMO_FILE)
posarr = PoseStampedArray()
posarr.header.frame_id = 'r_wrist_roll_link'
posarr.header.stamp = rospy.get_rostime()
posarr.poses = b.getTransformedPosesStamped()
# print posarr
while not rospy.is_shutdown():
pub.publish(posarr)
rate.sleep()
def plot_transformed():
topic = 'visualization_transformed_marker_array'
publisher = rospy.Publisher(topic, MarkerArray, queue_size=1000)
rospy.init_node('plot_transformed',anonymous=True)
r = rospy.Rate(1)
b = Bagger(filename=constants.DEMO_FILE)
alvar_markers = b.getTransformedMarkers()
markerArray = MarkerArray()
i = 0
l = len(alvar_markers)
for alvar_marker in alvar_markers:
# if not i:
# print alvar_marker
marker = Marker()
marker.header.frame_id = alvar_marker.header.frame_id
marker.header.stamp = rospy.get_rostime()
marker.ns = "visualization_markers"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.03
marker.scale.y = 0.03
marker.scale.z = 0.03
marker.color.a = 1.0
# marker.color.r = 1.0
# marker.color.g = 1.0
marker.color.r = i / l
marker.color.g = (l-i)/l
marker.color.b = 0.0
marker.pose = alvar_marker.pose.pose
# We add the new marker to the MarkerArray, removing the oldest
# marker from it when necessary
# if(count > MARKERS_MAX):
# markerArray.markers.pop(0)
marker.lifetime = rospy.Duration();
markerArray.markers.append(marker)
i+=1
while not rospy.is_shutdown():
publisher.publish(markerArray)
# print "publishing transf marker arry"
r.sleep()
def pub_teststates():
pub = rospy.Publisher('pub_teststates', PoseArray, queue_size=10)
rospy.init_node('pub_teststates', anonymous=True)
rate = rospy.Rate(1) # 10hz
a = Bagger(filename=constants.TEST_FILE)
markers = a.getMarkers()
start_m = markers[0]
goal_m = markers[-1]
sg_markers = [start_m, goal_m]
b = Bagger(filename=constants.DATA_FILE)
# tranformed_sg_markers = b.transformStartGoal(sg_markers)
posarr = PoseArray()
posarr.header.frame_id = 'r_wrist_roll_link'
posarr.header.stamp = rospy.get_rostime()
posarr.poses = b.transformStartGoal(sg_markers)
while not rospy.is_shutdown():
pub.publish(posarr)
rate.sleep()
print "publishing transformed_start_goal"
def experiment3(trees, train, test):
# Bag'em
learner = Bagger()
learner.add_classifiers(trees)
# Test
print 'Testing ensemble against \'train\''
results = learner.test(train[0], train[1])
accuracy = sum(r[0] == r[1] for r in results)
print '* Train Accuracy: {0}/{1} ({2:.4f})'.format(
accuracy, len(results),
float(accuracy) / len(results))
print 'Testing ensemble against \'test\''
results = learner.test(test[0], test[1])
accuracy = sum(r[0] == r[1] for r in results)
print '* Test Accuracy: {0}/{1} ({2:.4f})'.format(
accuracy, len(results),
float(accuracy) / len(results))
