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 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_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}
rs.joint_state.name = joint_names
rs.joint_state.position = joints
response = moveit_fk(header, fk_link, rs)
except rospy.ServiceException, e:
rospy.logerr("Service call failed:", e)
return (None)
quaternion = [
response.pose_stamped[1].pose.orientation.x,
response.pose_stamped[1].pose.orientation.y,
response.pose_stamped[1].pose.orientation.z,
response.pose_stamped[1].pose.orientation.w
]
rpy = get_rpy_from_quaternion(quaternion)
quaternion = Position.Quaternion(round(quaternion[0], 3),
round(quaternion[1], 3),
round(quaternion[2], 3),
round(quaternion[3], 3))
point = Position.Point(round(response.pose_stamped[1].pose.position.x, 3),
round(response.pose_stamped[1].pose.position.y, 3),
round(response.pose_stamped[1].pose.position.z, 3))
rpy = Position.RPY(round(rpy[0], 3), round(rpy[1], 3), round(rpy[2], 3))
rospy.loginfo("kinematic forward has been calculated ")
return (point, rpy, quaternion)
def get_rpy_from_quaternion(rot):
PI = 3.14159
euler = tf.transformations.euler_from_quaternion(rot)
rpy = [0, 0, 0]
rpy[0] = euler[1] * (-1.0)
rpy[1] = euler[0] - PI / 2.0
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")