#!/usr/bin/env python
# -*- Python -*-

"""
 \file RHdisp.py
 \brief visual model of RefHard
 \date $Date$

 \author FSI
"""
import sys
import time
sys.path.append(".")
save_path = sys.path[:]

# Import RTM module
import OpenRTM_aist
import RTC

# MyRtcDir=".."
sys.path.append("../../etc")
from set_env import *
sys.path.append(SrcDir+'/tools/v_robot')
from v_refhard import *
sys.path = save_path


# Import Service implementation class
# <rtc-template block="service_impl">

# </rtc-template>

# Import Service stub modules
# <rtc-template block="consumer_import">
# </rtc-template>


# This module's spesification
# <rtc-template block="module_spec">
rhdisp_spec = ["implementation_id", "RHdisp", 
		 "type_name",         "RHdisp", 
		 "description",       "visual model of RefHard", 
		 "version",           "1.0.0", 
		 "vendor",            "FSI", 
		 "category",          "6dof_arm", 
		 "activity_type",     "STATIC", 
		 "max_instance",      "10", 
		 "language",          "Python", 
		 "lang_type",         "SCRIPT",
		 ""]
# </rtc-template>

class RHdisp(OpenRTM_aist.DataFlowComponentBase):
	
	"""
	\class RHdisp
	\brief visual model of RefHard

	"""
	def __init__(self, manager):
		"""
		\brief constructor
		\param manager Maneger Object
		"""
		OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)

		self._d_th = RTC.TimedDoubleSeq(RTC.Time(0,0),[])
		"""
		 - Type: TimedDoubleSeq
		 - Number: 7
		"""
		self._thIn = OpenRTM_aist.InPort("th", self._d_th)
		self._d_handwidth = RTC.TimedDouble(RTC.Time(0,0),0)
		"""
		 - Type: TimedFloat
		 - Number: 1
		"""
		self._handwidthIn = OpenRTM_aist.InPort("handwidth", self._d_handwidth)


		


		# initialize of configuration-data.
		# <rtc-template block="init_conf_param">
		
		# </rtc-template>


		 
	def onInitialize(self):
		"""
		The initialize action (on CREATED->ALIVE transition)
		formaer rtc_init_entry() 
		
		\return RTC::ReturnCode_t
		
		"""
		# Bind variables and configuration variable
		
		# Set InPort buffers
		self.addInPort("th",self._thIn)
		self.addInPort("handwidth",self._handwidthIn)
		
		# Set OutPort buffers
		
		# Set service provider to Ports
		
		# Set service consumers to Ports
		
		# Set CORBA Service Ports

		visual.rate(10.0)
		print"initialized"

		return RTC.RTC_OK
	
	def onFinalize(self, ec_id):
		"""
	
		The finalize action (on ALIVE->END transition)
		formaer rtc_exiting_entry()
	
		\return RTC::ReturnCode_t
	
		"""
	
		return RTC.RTC_OK
	
	#def onStartup(self, ec_id):
	#	"""
	#
	#	The startup action when ExecutionContext startup
	#	former rtc_starting_entry()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	#def onShutdown(self, ec_id):
	#	"""
	#
	#	The shutdown action when ExecutionContext stop
	#	former rtc_stopping_entry()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	def onActivated(self, ec_id):
		"""
		The activated action (Active state entry action)
		former rtc_active_entry()
	
		\param ec_id target ExecutionContext Id
	
		\return RTC::ReturnCode_t
	
		"""
	
		return RTC.RTC_OK
	
	def onDeactivated(self, ec_id):
		"""
	
		The deactivated action (Active state exit action)
		former rtc_active_exit()
	
		\param ec_id target ExecutionContext Id
	
		\return RTC::ReturnCode_t
	
		"""
	
		return RTC.RTC_OK
	
	def onExecute(self, ec_id):
		"""
		The execution action that is invoked periodically
		former rtc_active_do()
	
		\param ec_id target ExecutionContext Id
	
		\return RTC::ReturnCode_t
	
		"""
        	if self._thIn.isNew() :
			th=self._thIn.read().data
			arm.links[1].set_joint(th[0])
           		arm.links[2].set_joint(th[1])
           		arm.links[3].set_joint(th[2])
           		arm.links[4].set_joint(th[3])
           		arm.links[5].set_joint(th[4])
           		arm.links[6].set_joint(th[5])
			set_hand_width(th[6])
		return RTC.RTC_OK
	
	def onAborting(self, ec_id):
		"""
	
		The aborting action when main logic error occurred.
		former rtc_aborting_entry()
	
		\param ec_id target ExecutionContext Id
	
		\return RTC::ReturnCode_t
	
		"""
	
		return RTC.RTC_OK
	
	def onError(self, ec_id):
		"""
	
		The error action in ERROR state
		former rtc_error_do()
	
		\param ec_id target ExecutionContext Id
	
		\return RTC::ReturnCode_t
	
		"""
	
		return RTC.RTC_OK
	
	def onReset(self, ec_id):
		"""
	
		The reset action that is invoked resetting
		This is same but different the former rtc_init_entry()
	
		\param ec_id target ExecutionContext Id
	
		\return RTC::ReturnCode_t
	
		"""
	
		return RTC.RTC_OK
	
	#def onStateUpdate(self, ec_id):
	#	"""
	#
	#	The state update action that is invoked after onExecute() action
	#	no corresponding operation exists in OpenRTm-aist-0.2.0
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	#def onRateChanged(self, ec_id):
	#	"""
	#
	#	The action that is invoked when execution context's rate is changed
	#	no corresponding operation exists in OpenRTm-aist-0.2.0
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	



def RHdispInit(manager):
    profile = OpenRTM_aist.Properties(defaults_str=rhdisp_spec)
    manager.registerFactory(profile,
                            RHdisp,
                            OpenRTM_aist.Delete)

def MyModuleInit(manager):
    RHdispInit(manager)

    # Create a component
    comp = manager.createComponent("RHdisp")

def main():
	mgr = OpenRTM_aist.Manager.init(sys.argv)
	mgr.setModuleInitProc(MyModuleInit)
	mgr.activateManager()
	mgr.runManager()

if __name__ == "__main__":
	main()

