class PositionManager:
def __init__(self, position_dir):
self.fh = PositionFileHandler(position_dir)
self.manage_position_server = rospy.Service(
'/niryo_one/position/manage_position', ManagePosition,
self.callback_manage_position)
rospy.loginfo("service manage position created")
self.get_position_list_server = rospy.Service(
'/niryo_one/position/get_position_list', GetPositionList,
self.callback_get_position_list)
rospy.loginfo("get list position created")
self.validation = rospy.get_param(
"/niryo_one/robot_command_validation")
self.parameters_validation = ParametersValidation(self.validation)
def create_position_response(self, status, message, position=None):
position_msg = PositionMessage()
if position != None:
position_msg.name = position.name
position_msg.joints = position.joints
position_msg.rpy = Position.RPY(position.rpy.roll,
position.rpy.pitch,
position.rpy.yaw)
position_msg.point = Position.Point(position.point.x,
position.point.y,
position.point.z)
position_msg.quaternion = Position.Quaternion(
position.quaternion.x, position.quaternion.y,
position.quaternion.z, position.quaternion.w)
return {'status': status, 'message': message, 'position': position_msg}
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")
def delete_position(self, position_name):
try:
self.fh.remove_position(position_name)
except NiryoOneFileException as e:
return False
return True
def update_position(self, position, position_data):
try:
self.parameters_validation.validate_joints(position_data.joints)
except RobotCommanderException as e:
rospy.logwarn("Invalid joints value when updating position : " +
str(e.message))
return False, "Could not update position : " + str(e.message)
position.joints = position_data.joints
(position.point, position.rpy,
position.quaternion) = get_forward_kinematic(position.joints)
try:
self.fh.write_position(position)
except NiryoOneFileException as e:
return False, "Could not update position : " + str(e)
return True, "Position has been updated"
def get_position(self, position_name):
try:
return self.fh.read_position(position_name)
except NiryoOneFileException as e:
return None
def create_new_position(self, position):
position.name = position.name.rstrip()
if self.fh.check_position_name(position.name) == False:
return None, "Failed to create new position : position " + str(
position.name) + " already exists"
try:
self.parameters_validation.validate_joints(position.joints)
except RobotCommanderException as e:
rospy.logwarn("Invalid joints values when creating position : " +
str(e.message))
return None, "Failed to create new position : " + str(e.message)
try:
(position.point, position.rpy,
position.quaternion) = get_forward_kinematic(position.joints)
self.fh.write_position(position)
return position.name, "Position has been created"
except NiryoOneFileException as e:
return None, "Failed to create new position : " + str(e)
def callback_get_position_list(self, req=None):
pos_list = self.get_all_positions()
msg_list = []
for pos in pos_list:
position_msg = PositionMessage()
position_msg.name = pos.name
position_msg.joints = pos.joints
position_msg.rpy = Position.RPY(pos.rpy.roll, pos.rpy.pitch,
pos.rpy.yaw)
position_msg.point = Position.Point(pos.point.x, pos.point.y,
pos.point.z)
position_msg.quaternion = Position.Quaternion(
pos.quaternion.x, pos.quaternion.y, pos.quaternion.z,
pos.quaternion.w)
msg_list.append(position_msg)
return {'positions': msg_list}
def get_all_positions(self):
filenames = self.fh.get_all_filenames()
position_list = []
for f in filenames:
try:
position_name = self.fh.position_name_from_filename(f)
pos = self.get_position(position_name)
if pos != None:
position_list.append(pos)
except NiryoOneFileException as e:
pass
return position_list
class PositionManager:
def __init__(self, position_dir):
self.fh = PositionFileHandler(position_dir)
self.manage_position_server = rospy.Service('/niryo_one/position/manage_position', ManagePosition, self.callback_manage_position)
rospy.loginfo("service manage position created")
self.get_position_list_server = rospy.Service(
'/niryo_one/position/get_position_list', GetPositionList, self.callback_get_position_list)
rospy.loginfo("get list position created")
def create_position_response(self, status, message, position=None):
position_msg = PositionMessage()
if position != None:
position_msg.position_name = position.position_name
position_msg.position_id = position.position_id
position_msg.joints = position.joints
position_msg.rpy = Position.RPY(position.rpy.roll, position.rpy.pitch, position.rpy.yaw)
position_msg.point = Position.Point( position.point.x, position.point.y, position.point.z)
position_msg.quaternion = Position.Quaternion(position.quaternion.x, position.quaternion.y,
position.quaternion.z, position.quaternion.w)
return { 'status': status, 'message': message, 'position': position_msg }
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(position_name = position_msg.position_name, position_id = position_msg.position_id,
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 id : " + position_name)
return self.create_position_response(200, "position has been found", pos)
# CREATE new position
elif cmd_type == PositionCommandType.CREATE:
new_position_name = self.create_new_position(position_data)
new_position = self.get_position(new_position_name)
if new_position == None:
return self.create_position_response(400, "Failed to create position")
return self.create_position_response(200, "position has been created", 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 this name : " + position_name)
success = self.update_position(pos, position_data)
if not success:
return self.create_position_response(400, "Could not update position with this name : " + position_name)
return self.create_position_response(200, "Position has been updated", 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")
def delete_position(self, position_name):
try:
self.fh.remove_position(position_name)
except NiryoOneFileException as e:
return False
return True
def update_position(self, position, position_data):
position.position_name = position_data.position_name
position.joints = position_data.joints
(position.point, position.rpy, position.quaternion) = get_forward_kinematic(position.joints)
try:
self.fh.write_position(position)
except NiryoOneFileException as e:
return False
return True
def get_position(self, position_name):
try:
return self.fh.read_position(position_name)
except NiryoOneFileException as e:
return None
def create_new_position(self, position) :
try:
position.position_id = self.fh.pick_new_id()
(position.point, position.rpy, position.quaternion) = get_forward_kinematic(position.joints)
self.fh.write_position(position)
return(position.position_name)
except NiryoOneFileException as e:
return None
def callback_get_position_list(self, req = None):
pos_list = self.get_all_positions()
msg_list = []
for pos in pos_list:
position_msg = PositionMessage()
position_msg.position_name = pos.position_name
position_msg.position_id = pos.position_id
position_msg.joints = pos.joints
position_msg.rpy = Position.RPY(pos.rpy.roll, pos.rpy.pitch, pos.rpy.yaw)
position_msg.point = Position.Point( pos.point.x, pos.point.y, pos.point.z)
position_msg.quaternion = Position.Quaternion(pos.quaternion.x, pos.quaternion.y,
pos.quaternion.z, pos.quaternion.w)
msg_list.append(position_msg)
return { 'positions': msg_list }
def get_all_positions(self):
filenames = self.fh.get_all_filenames()
position_list = []
for f in filenames:
try:
position_name = self.fh.position_name_from_filename(f)
pos = self.get_position(position_name)
if pos != None:
position_list.append(pos)
except NiryoOneFileException as e:
pass
return position_list