Refactoring: added a Servo class that stores the servo status. (6ce65b0e) · Commits · INAF / DISCOS

SRT/Servers/SRTMinorServo/src/SRTMinorServoTest/mscu/commands.py

deleted100644 → 0

+0 −250

Original line number	Diff line number	Diff line
		#!/usr/bin/env python
		# Author: Marco Buttu <m.buttu@oa-cagliari.inaf.it>

		"""This module defines several functions that make the command answers."""

		from parameters import closers, time_stamp, number_of_axis, app_nr, app_state_max, cab_state_max
		import random
		import posutils

		# The actual value of SRP position
		srp_act_position = [1570.20]*number_of_axis['SRP']

		# Drive cabinet states
		dc_startup_state = 1
		dc_park_state = 3
		dc_ok = 0

		# Application states
		app_remote_auto = 4

		def getpos(cmd_num, app_num, response_policy):
		"""This function make an answer for a getpos request.

		Parameters:

		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `response_policy`: the type of response: expected, mixed

		Return a list of answers.
		"""
		servo_type = app_nr[app_num]
		# The default response value 13477... is the time
		mixed = []
		mixed.append('?' + 'getpos' + ':0=%d> %s' % (random.randrange(0,5), time_stamp))
		mixed.append('?' + 'getpos' + ':0=%d> %s' % (app_num, time_stamp))
		mixed.append('?' + 'getpos' + ':0=#none> 1000')
		db = posutils.PositionDB()
		data = db.get(servo_type);
		expected = '?' + 'getpos' + ':%d=%d> %s' % (cmd_num, app_num, posutils.now())
		# Read the positions stored in a shelve db by a setpos command
		for item in data[1:]:
		expected += ',%s' %item

		return response_list('getpos', cmd_num, app_num, expected, mixed, response_policy)


		def getappstatus(cmd_num, app_num, response_policy):
		"""Return a list of answers for a getappstatus request.

		Parameters:

		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `response_policy`: the type of response: expected, mixed
		"""
		# The default response value 13477... is the time
		value = '0000030D'
		expected = '?' + 'getappstatus' + ':%d=%d> %s' %(cmd_num, app_num, value)
		mixed = []
		mixed.append('?' + 'getappstatus' + ':0=%d> %s' % (random.randrange(0,5), value))
		mixed.append('?' + 'getappstatus' + ':0=%d> %s' % (app_num, value))
		mixed.append('?' + 'getappstatus' + ':0=#none> 1000')

		return response_list('getappstatus', cmd_num, app_num, expected, mixed, response_policy)


		def getstatus(cmd_num, app_num, response_policy):
		"""Return a list of answers for a getstatus request.

		Parameters:

		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `response_policy`: the type of response: expected, mixed

		Return a list of answers.
		"""
		servo_type = app_nr[app_num]

		mixed = []
		mixed.append('?' + 'getstatus' + ':0=%d> ' % (random.randrange(0,5),))
		mixed.append('?' + 'getstatus' + ':0=%d> ' % (app_num,))
		mixed.append('?' + 'getstatus' + ':0=#none> 1000')

		# Random state
		# app_state = random.randrange(0, app_state_max)
		# app_status = ''.join([str(item) for item in [hex(random.randrange(16)).split('x')[-1].upper() for i in range(4)]])
		# cab_state = random.randrange(0, cab_state_max)

		# Setup state
		# cab_state = dc_startup_state

		# Park state
		# cab_state = dc_park_state

		# Ready
		app_state = app_remote_auto
		app_status = "FFFF"
		cab_state = dc_ok

		expected = '?' + 'getstatus' + ':%d=%d> ' % (cmd_num, app_num)
		db = posutils.PositionDB()
		data = db.get(servo_type)
		expected += '%d,%d,%s,%d' %( posutils.now(), app_state, app_status, cab_state)
		# Read the positions stored in a shelve db by a setpos command
		for item in data[1:]:
		expected += ',%s' %item

		return response_list('getstatus', cmd_num, app_num, expected, mixed, response_policy)


		def setpos(cmd_num, app_num, response_policy, *params):
		"""Return a list of answers for a setpos request.

		Parameters:

		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `response_policy`: the type of response: expected, mixed, ...
		"""
		servo_type = app_nr[app_num]
		db = posutils.PositionDB()
		db.insert(servo_type, params[-number_of_axis[servo_type]:], params[0])
		expected = '@' + 'setpos' + ':%d=%d' % (cmd_num, app_num)
		for param in params:
		expected += ",%s" %param
		mixed = []
		mixed.append('?' + 'setpos' + ':0=%d> 0' %random.randrange(0,5))
		mixed.append('?' + 'setpos' + ':0=%d> 0' %app_num)
		mixed.append('@' + 'setpos' + ':0=foo>')

		return response_list('setpos', cmd_num, app_num, expected, mixed, response_policy)


		def setup(cmd_num, app_num, response_policy, *params):
		"""This function make an answer for a setup request.

		Parameters:

		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `response_policy`: the type of response: expected, mixed

		Return a list of answers.
		"""
		expected = '@' + 'setup' + ':%d=%d' % (cmd_num, app_num)
		for param in params:
		expected += ",%s" %param
		mixed = []
		mixed.append('?' + 'stow' + ':0=%d> 0' %random.randrange(0,5))
		mixed.append('?' + 'setup' + ':0=%d> 0' %app_num)

		return response_list('setup', cmd_num, app_num, expected, mixed, response_policy)


		def stow(cmd_num, app_num, response_policy, *params):
		"""This function make an answer for a stow request.

		Parameters:

		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `response_policy`: the type of response: expected, mixed

		Return a list of answers.
		"""
		expected = '@' + 'stow' + ':%d=%d' % (cmd_num, app_num)
		for param in params:
		expected += ",%s" %param
		mixed = []
		mixed.append('?' + 'setup' + ':0=%d> 0' %random.randrange(0,5))
		mixed.append('?' + 'stow' + ':0=%d> 0' %app_num)

		return response_list('stow', cmd_num, app_num, expected, mixed, response_policy)


		def clean(cmd_num, app_num, response_policy, *params):
		"""This function make an answer for a setup request.

		Parameters:

		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `response_policy`: the type of response: expected, mixed

		Return a list of answers.
		"""
		expected = '@' + 'clean' + ':%d=%d' % (cmd_num, app_num)
		for param in params:
		expected += ",%s" %param
		mixed = []
		mixed.append('?' + 'stow' + ':0=%d> 0' %random.randrange(0,5))
		mixed.append('?' + 'clean' + ':0=%d> 0' %app_num)
		return response_list('clean', cmd_num, app_num, expected, mixed, response_policy)

		def getspar(cmd_num, app_num, response_policy, *params):
		"""Return a list of answers for all the getspar requestes.

		Parameters:

		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `response_policy`: the type of response: expected, mixed
		- `params`: a tuple containing the index and sub-index of a parameter request.
		"""
		expected = '?' + 'getspar' + ':%d=%d' % (cmd_num, app_num)
		nfm = '?' + 'getspar' + ':0=%d' %app_num

		for param in params[1:]:
		expected += ',%s' %param
		nfm += ',%s' %param

		expected += '> %d' % (sum([ int(param) for param in params]))
		nfm += '> %d' % (sum([ int(param) for param in params]))
		mixed = []
		mixed.append(nfm)
		mixed.append('?' + 'getspar' + ':0=%d> 0' %random.randrange(0,5))

		return response_list('getspar', cmd_num, app_num, expected, mixed, response_policy)


		def response_list(cmd, cmd_num, app_num, expected, mixed, response_policy):
		"""Return a list of answers for a request.

		Parameters:

		- `cmd`: the command (getpos, getspar, etc.)
		- `cmd_num`: the command identification number
		- `app_num`: the application number (minor servo address)
		- `expected`: the expected response
		- `mixed`: a mixed answer
		- `response_policy`: the type of response: expected, mixed
		"""
		answers = []
		if response_policy == "expected":
		answers.append(expected + closers[0])
		elif response_policy == "mixed":
		for message in mixed:
		answers.append(message + closers[0])
		if(random.randrange(0,10) == 5):
		answers.append(('!NAK_' + cmd + ':%d=%d> Wrong State = Application ...' %(cmd_num, app_num)) + closers[0])
		else:
		answers.append(('!NAK_' + cmd + ':0=%d> Wrong State = Application ...' %app_num) + closers[0])
		answers.append('invalid')
		answers.append('?invalid')
		answers.append(expected + closers[0])
		answers.append('?invalid' + closers[0])

		return answers

SRT/Servers/SRTMinorServo/src/SRTMinorServoTest/mscu/mscu.py

+20 −38

Original line number	Diff line number	Diff line
		@@ -6,10 +6,9 @@ import traceback
		import os
		import sys

		import commands
		import posutils
		import servo

		from parameters import headers, closers, app_nr, filtered
		from parameters import headers, closers, filtered, app_nr


		class MSCU(object):
		@@ -18,16 +17,9 @@ class MSCU(object):
		self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
		self.socket.bind((host, port))
		self.set_response_policy()

		db = posutils.PositionDB()
		db.initialize()

		def set_response_policy(self, policy='expected'):
		if policy in ('expected', 'mixed'):
		self.response_policy = policy
		else:
		raise ValueError('response policy %s not allowed' %policy)
		self.servos = {}
		for address in app_nr:
		self.servos[address] = servo.Servo(address)

		def run(self):
		self.socket.listen(1)
		@@ -84,46 +76,37 @@ class MSCU(object):
		cmd_num = int(cmd_num)
		# Retrieve the command parameters
		all_params = [eval(param.strip()) for param in params_str.split(',')]
		app_num, params = all_params[0], all_params[1:]
		address, params = all_params[0], all_params[1:]
		except:
		cause = "Invalid syntax!\n"
		print cause
		print MSCU.error_message(data)
		# Send a general error message
		connection.send(cause + MSCU.error_message(data))
		connection.send(MSCU.error_message(data))
		# An invalid command pass silently
		continue

		servo = self.servos[address]
		# Print a general error message if we received an invalid command
		if header not in headers or not hasattr(commands, cmd):
		cause = "Unexpected command or header.\n"
		print cause
		if header not in headers or not hasattr(servo, cmd):
		print MSCU.error_message(data)
		# Send a general error message
		connection.send(cause + MSCU.error_message(data))
		connection.send(MSCU.error_message(data))
		# An invalid command pass silently
		continue

		if not cmd in filtered:
		print "\nReceived message from %s: %r" % (connection.getpeername(), data)

		try:
		servo_type = app_nr[app_num]
		except KeyError:
		cause = "Unexpected application number. Allowed values: %s\n" % app_nr.keys()
		print cause
		# Send a general error message
		connection.send(cause + MSCU.error_message(data))
		# An invalid command pass silently
		continue

		# Call the appropriate command whose name is the string cmd
		for ans in getattr(commands, cmd)(cmd_num, app_num, self.response_policy, *params):
		ans = getattr(servo, cmd)(cmd_num, *params)

		if not cmd in filtered:
		print "Answer from %s: %r" % (servo_type, ans)
		print "Answer from %s: %r" % (servo.name, ans)

		connection.send('%s' %ans)
		except (KeyboardInterrupt, SystemExit):
		raise
		except:
		pass
		raise

		# Close the connection
		try:
		@@ -141,11 +124,10 @@ class MSCU(object):
		print ""43
		print "MSCU_simulator - Waiting for connections..."
		print ""43
		print "Response type: %s\n" %self.response_policy

		@staticmethod
		def error_message(msg):
		return "message %s discarded" %msg
		return "Invalid message: %s" %msg

		@staticmethod
		def reap():

SRT/Servers/SRTMinorServo/src/SRTMinorServoTest/mscu/parameters.py

+1 −1

Original line number	Diff line number	Diff line
		@@ -38,7 +38,7 @@ app_nr = {
		}

		# Number of axis of each minor servo
		number_of_axis = {
		axes = {
		'PFP': 3,
		'SRP': 6,
		'GFR': 1,

SRT/Servers/SRTMinorServo/src/SRTMinorServoTest/mscu/posutils.py

deleted100644 → 0

+0 −93

Original line number	Diff line number	Diff line
		import os
		import time
		import shelve
		from parameters import number_of_axis, app_nr


		def now():
		"""Return the actual time in OMG format"""
		acstime_ACE_BEGIN = 122192928000000000L
		return int(acstime_ACE_BEGIN + time.time() * 10000000L)

		class PositionDB:
		def __init__(self, name='/tmp/positions.db'):
		self.name = name

		def clear(self):
		os.remove(self.name)

		def initialize(self):
		db = shelve.open(self.name, writeback = True)
		for servo_type in app_nr.values():
		db[servo_type] = [[now()] + [0] * number_of_axis[servo_type]]
		db.close()

		def insert(self, servo_type, positions, timestamp=None):
		if not timestamp:
		timestamp = now()
		db = shelve.open(self.name, writeback = True)
		list_of_lists = db[servo_type]
		index = None
		for idx, lst in enumerate(list_of_lists):
		if lst[0] <= timestamp:
		if idx + 1 == len(list_of_lists):
		index = idx + 1
		break
		else:
		if list_of_lists[idx + 1][0] > timestamp:
		index = idx + 1
		break
		else:
		continue
		else:
		index = 0
		break
		list_of_lists.insert(index, [timestamp] + list(positions))
		db.close()

		def print_positions(self, servo_type=None):
		db = shelve.open(self.name, writeback = True)
		if servo_type:
		list_of_lists = db[servo_type]
		for lst in list_of_lists:
		print lst
		else:
		for servo_type in app_nr.values():
		list_of_lists = db[servo_type]
		print ("%s" %servo_type).upper()
		for lst in list_of_lists:
		print lst
		print
		db.close()

		def get(self, servo_type, time_ref=now):
		"""Return the actual position as [timestamp, axis A, ..., axis N]"""
		# timestamp = now() if not time_ref else time_ref
		timestamp = time_ref
		if hasattr(time_ref, "__call__"):
		timestamp = time_ref()
		db = shelve.open(self.name, writeback = True)
		list_of_lists = db[servo_type]
		index = None
		for idx, lst in enumerate(list_of_lists):
		if lst[0] <= timestamp:
		if idx + 1 == len(list_of_lists):
		index = idx
		break
		else:
		if list_of_lists[idx + 1][0] > timestamp:
		index = idx
		break
		else:
		continue
		else:
		index = 0
		break
		value = list_of_lists[index]
		db[servo_type] = list_of_lists
		if hasattr(time_ref, "__call__"):
		db[servo_type] = list_of_lists[index::]
		db.update()
		db.close()
		return value

SRT/Servers/SRTMinorServo/src/SRTMinorServoTest/mscu/servo.py

0 → 100644

+179 −0

Original line number	Diff line number	Diff line
		import time
		import shelve
		from parameters import closers, app_nr, axes

		# REMOVE -------------------
		# Drive cabinet states
		dc_startup_state = 1
		dc_park_state = 3
		dc_ok = 0

		# Application states
		app_remote_auto = 4
		# END REMOVE ---------------


		class Servo(object):

		def __init__(self, address):
		self.id = address
		self.name = app_nr[self.id] # GRF, PFP, SRP, M3R
		self.axes = axes[self.name] # Number of axes
		self.history = History(self.name, self.axes)

		def set_app_state(self, value):
		self.app_state = int(value)

		def set_cab_state(self, value):
		self.cab_state = int(value)

		def set_app_status(self, value):
		self.app_status = int(value)

		def getpos(self, cmd_num):
		data = self.history.get();
		answer = '?getpos' + ':%d=%d> %s' % (cmd_num, self.id, Servo.time())
		# Read the positions stored in a shelve db by a setpos command
		for position in data[1:]:
		answer += ',%s' %position
		else:
		answer += closers[0]

		return answer

		def getappstatus(self, cmd_num):
		value = '0000030D'
		answer = '?getappstatus' + ':%d=%d> %s' %(cmd_num, self.id, value)
		return answer + closers[0]

		def getstatus(self, cmd_num):
		# Ready
		app_state = app_remote_auto
		app_status = "FFFF"
		cab_state = dc_ok

		answer = '?getstatus' + ':%d=%d> ' % (cmd_num, self.id)
		answer += '%d,%d,%s,%d' %(Servo.time(), app_state, app_status, cab_state)

		# Read the positions stored in a shelve db by a setpos command
		data = self.history.get()
		for position in data[1:]:
		answer += ',%s' %position
		else:
		answer += closers[0]
		return answer


		def setpos(self, cmd_num, *params):
		positions = params[-self.axes:], params[0]
		self.history.insert(positions)
		answer = '@setpos' + ':%d=%d' %(cmd_num, self.id)
		for param in params:
		answer += ",%s" %param
		else:
		answer += closers[0]
		return answer


		def setup(self, cmd_num, *params):
		answer = '@setup' + ':%d=%d' % (cmd_num, self.id)
		for param in params:
		answer += ",%s" %param
		else:
		answer += closers[0]
		return answer


		def stow(self, cmd_num, *params):
		answer = '@stow' + ':%d=%d' % (cmd_num, self.id)
		for param in params:
		answer += ",%s" %param
		else:
		answer += closers[0]
		return answer

		def clean(self, cmd_num, *params):
		answer = '@clean' + ':%d=%d' % (cmd_num, self.id)
		for param in params:
		answer += ",%s" %param
		else:
		answer += closers[0]
		return answer

		def getspar(self, cmd_num, *params):
		answer = '?getspar' + ':%d=%d' % (cmd_num, self.id)

		for param in params[1:]:
		answer += ',%s' %param

		answer += '> %d' % (sum([ int(param) for param in params]))
		answer += closers[0]
		return answer

		@staticmethod
		def time():
		"""Return the actual time in OMG format"""
		acstime_ACE_BEGIN = 122192928000000000L
		return int(acstime_ACE_BEGIN + time.time() * 10000000L)


		class History(object):
		def __init__(self, servo_name, servo_axes):
		self.servo_name = servo_name
		self.servo_axes = servo_axes
		self.path = '/tmp/%s_positions.db' %servo_name
		db = shelve.open(self.path, writeback=True)
		db[servo_name] = [[Servo.time()] + [0]*servo_axes]
		db.close()

		def insert(self, positions, timestamp=None):
		target_time = timestamp if timestamp else Servo.time()
		db = shelve.open(self.path, writeback=True)
		list_of_lists = db[self.servo_name]
		index = None
		for idx, lst in enumerate(list_of_lists):
		if lst[0] <= target_time:
		if idx + 1 == len(list_of_lists):
		index = idx + 1
		break
		else:
		if list_of_lists[idx + 1][0] > target_time:
		index = idx + 1
		break
		else:
		continue
		else:
		index = 0
		break
		list_of_lists.insert(index, [target_time] + list(positions))
		db.close()


		def get(self, time_ref=Servo.time):
		"""Return the actual position as [timestamp, axis A, ..., axis N]"""
		timestamp = time_ref() if callable(time_ref) else time_ref
		db = shelve.open(self.path, writeback=True)
		list_of_lists = db[self.servo_name]
		index = None
		for idx, lst in enumerate(list_of_lists):
		if lst[0] <= timestamp:
		if idx + 1 == len(list_of_lists):
		index = idx
		break
		else:
		if list_of_lists[idx + 1][0] > timestamp:
		index = idx
		break
		else:
		continue
		else:
		index = 0
		break
		value = list_of_lists[index]
		db[self.servo_name] = list_of_lists
		if callable(time_ref):
		db[self.servo_name] = list_of_lists[index::]
		db.update()
		db.close()
		return value