#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys, os, traceback

import roslib
import rospy
import tf
from tf import transformations

#foreach( $t in ${node.importTopic} )
from ${t.type.packageName}.msg import ${t.type.name}
#end

class ${node.name}_node():
    """
    """
    def on_execute(self):
#foreach( $t in ${node.outTopics} )
        """ 
        Periodically Called Routine
        """
        # self._${t.name} = ${t.type.name}
        # self._${t.name}Out.publish(self._${t.name})
#end
        pass
        
    def __init__(self):
        """
        Constructor
        """
        rospy.init_node('${node.name}')
        
        # Periodic Execution Rate (astah add this parameter by default.)
        self._param_exec_rate = rospy.get_param("~exec_rate", 20.0)
        
        # Parameters
#foreach( $p in ${node.params} )
        self._param_${p.name} = rospy.get_param("~${p.name}", ${p.defaultValue})
#end

        # Publishing Topics
#foreach( $t in ${node.outTopics} )
        self._${t.name}Out = rospy.Publisher('${t.name}', ${t.type.name}, queue_size=10)
        self._${t.name} = ${t.type.name}
#end
        # Subscribing Topics
#foreach( $t in ${node.inTopics} )
        self._${t.name}In = rospy.Subscriber('${t.name}', ${t.type.name}, self.on_${t.name})
        self._${t.name} = ${t.type.name}
#end
        #main loop
        rate = rospy.Rate(self._param_execute_rate) 
        while not rospy.is_shutdown():
            try:
                self.on_execute()
                rate.sleep()
            except scalemodel.protocol.FormatError, e:
                traceback.print_exc()
            pass

#foreach( $t in ${node.inTopics} )
    def on_${t.name}(self, msg):
        """
        Subscribing Topic ${t.name}
        """
        # self._${t.name} = msg
        # print msg
        pass
#end

if __name__ == '__main__':
    n = ${node.name}_node()
