def read_position(self,position_name):
filename = self.filename_from_position_name(position_name)
# Check if exists
if not self.does_file_exist(filename):
raise GaussFileException(' ' + str(position_name)+ ' does not exist')
with self.lock:
with open(self.base_dir + filename, 'r') as f:
pos = Position()
for line in f:
try:
if line.startswith('Position_Name:'):
pos.name = str(next(f).rstrip())
if line.startswith("Joints:"):
pos.joints = list(str(next(f).rstrip()).split(','))
pos.joints = map(float ,pos.joints)
if line.startswith("RPY:"):
pos.rpy.roll = float(str(next(f).rstrip()))
pos.rpy.pitch = float(str(next(f).rstrip()))
pos.rpy.yaw = float(str(next(f).rstrip()))
if line.startswith("Point:"):
pos.point.x = float(str(next(f).rstrip()))
pos.point.y = float(str(next(f).rstrip()))
pos.point.z = float(str(next(f).rstrip()))
if line.startswith("Quaternion:"):
pos.quaternion.x = float(str(next(f).rstrip()))
pos.quaternion.y = float(str(next(f).rstrip()))
pos.quaternion.z = float(str(next(f).rstrip()))
pos.quaternion.w = float(str(next(f).rstrip()))
except Exception as e:
raise GaussFileException("Could not read position "
+ position_name + " : " + str(e) )
return pos
def callback_manage_position(self, req):
cmd_type = req.cmd_type
position_name = req.position_name
position_msg = req.position
rpy = Position.RPY(position_msg.rpy.roll, position_msg.rpy.pitch,
position_msg.rpy.yaw)
point = Position.Point(position_msg.point.x, position_msg.point.y,
position_msg.point.z)
quaternion = Position.Quaternion(position_msg.quaternion.x,
position_msg.quaternion.y,
position_msg.quaternion.z,
position_msg.quaternion.w)
position_data = Position(name=position_msg.name,
joints=position_msg.joints,
rpy=rpy,
point=point,
quaternion=quaternion)
# GET an existing position
if cmd_type == PositionCommandType.GET:
pos = self.get_position(position_name)
if pos == None:
return self.create_position_response(
400, "No position found with name : " + str(position_name))
return self.create_position_response(200,
"Position has been found",
pos)
# CREATE new position
elif cmd_type == PositionCommandType.CREATE:
(new_position_name, msg) = self.create_new_position(position_data)
if new_position_name == None:
return self.create_position_response(400, msg)
new_position = self.get_position(new_position_name)
if new_position == None:
return self.create_position_response(
400, "Failed to create new position")
return self.create_position_response(200, msg, new_position)
# UPDATE existing sequence
elif cmd_type == PositionCommandType.UPDATE:
pos = self.get_position(position_name)
if pos == None:
return self.create_position_response(
400, "No position found with name : " + position_name)
(success, update_msg) = self.update_position(pos, position_data)
if not success:
return self.create_position_response(400, update_msg)
return self.create_position_response(200, update_msg, pos)
# DELETE sequence
elif cmd_type == PositionCommandType.DELETE:
success = self.delete_position(position_name)
if not success:
return self.create_position_response(
400,
"Could not delete position with name : " + position_name)
return self.create_position_response(200,
"Position has been deleted")
# Wrong cmd_type
else:
return self.create_sequence_response(400, "Wrong command type")