def init_pcn( self, shape ): # create the pose cell network pcn = PoseCellNetwork( shape ) # start off will all of the energy is the central cell midpoint = (math.floor(shape[0]/2),math.floor(shape[1]/2),math.floor(shape[2]/2)) pcn.inject( 1, midpoint ) self.current_pose_cell = midpoint return pcn
def __init__( self ): self.im_count = 0 # Initialize the Pose Cell Network self.pcn = PoseCellNetwork(shape=POSE_SIZE) self.pc_count = 0 self.twist_data = deque() self.odom_freq = ODOM_FREQ midpoint = (math.floor(POSE_SIZE[0]/2),math.floor(POSE_SIZE[1]/2),math.floor(POSE_SIZE[2]/2)) self.pcn.inject( 1, midpoint ) # Set up layout for ROS message dim = [ MultiArrayDimension( label="x", size=POSE_SIZE[0], stride=POSE_SIZE[0] * POSE_SIZE[1] * POSE_SIZE[2] ), MultiArrayDimension( label="y", size=POSE_SIZE[1], stride=POSE_SIZE[1] * POSE_SIZE[2] ), MultiArrayDimension( label="th", size=POSE_SIZE[2], stride=POSE_SIZE[2] ) ] self.pc_layout = MultiArrayLayout(dim=dim) # Initialize the View Templates # TODO: put reasonable values here self.vts = ViewTemplates( x_range=X_RANGE, y_range=Y_RANGE, x_step=X_STEP, y_step=Y_STEP, im_x=IM_SIZE[0], im_y=IM_SIZE[1], match_threshold=MATCH_THRESHOLD) self.vt_count = 0 self.bridge = CvBridge() # Initialize the Experience Map self.em = ExperienceMap() self.em_count = 0 self.vis_odom_data = deque() rospy.init_node( 'posecells', anonymous=True ) if not USE_VISUAL_ODOMETRY: self.sub_odom = rospy.Subscriber( 'navbot/odom',Odometry,self.odom_callback ) self.sub_vis = rospy.Subscriber( 'navbot/camera/image',Image,self.vis_callback ) self.pub_pc = rospy.Publisher( 'navbot/posecells', numpy_msg(Float64MultiArray) ) self.pub_em = rospy.Publisher( 'navbot/experiencemap', Pose2D ) # Set up publisher for the template matches (sends just the index value of a match) self.pub_tm = rospy.Publisher( 'navbot/templatematches', Int32 ) # Set up a visual odometer self.vis_odom = SimpleVisualOdometer() if RECORD_ROSBAG: date = datetime.datetime.now() self.bag = rosbag.Bag( '../testdata/Output-{0}-{1}-{2}-{3}.bag'.format( date.month, date.day, date.hour, date.minute ),'w' )