def __init__(self, trajectory_dir):
self.fh = TrajectoryFileHandler(trajectory_dir)
self.manage_position_server = rospy.Service(
'niryo_one/trajectory/manage_trajectory', ManageTrajectory,
self.callback_manage_trajectory)
rospy.loginfo(
"niryo_one/trajectory/manage_trajectory service has been created ")
self.get_trajectory_list_server = rospy.Service(
'niryo_one/trajectory/get_trajectory_list', GetTrajectoryList,
self.callback_get_trajectory_list)
rospy.loginfo("niryo_one/trajectory/get_trajectory_list")
self.validation = rospy.get_param("niryo_one/robot_command_validation")
self.parameters_validation = ParametersValidation(self.validation)
class TrajectoryManager:
def __init__(self, trajectory_dir):
self.fh = TrajectoryFileHandler(trajectory_dir)
self.manage_position_server = rospy.Service(
'/niryo_one/trajectory/manage_trajectory', ManageTrajectory,
self.callback_manage_trajectory)
rospy.loginfo(
"/niryo_one/trajectory/manage_trajectory service has been created "
)
self.get_trajectory_list_server = rospy.Service(
'/niryo_one/trajectory/get_trajectory_list', GetTrajectoryList,
self.callback_get_trajectory_list)
rospy.loginfo("/niryo_one/trajectory/get_trajectory_list")
self.validation = rospy.get_param(
"/niryo_one/robot_command_validation")
self.parameters_validation = ParametersValidation(self.validation)
def callback_get_trajectory_list(self, req=None):
traj_list = self.get_all_trajectories()
msg_list = []
for traj in traj_list:
trajectory_msg = Trajectory()
trajectory_msg.description = traj.description
trajectory_msg.name = traj.name
trajectory_msg.id = traj.id
trajectory_msg.trajectory_plan = traj.trajectory_plan
msg_list.append(trajectory_msg)
return {'trajectories': msg_list}
def get_all_trajectories(self):
filenames = self.fh.get_all_filenames()
trajectory_list = []
for f in filenames:
try:
trajectory_id = self.fh.trajectory_id_from_filename(f)
traj = self.get_trajectory(trajectory_id)
if traj is not None:
trajectory_list.append(traj)
except NiryoOneFileException as e:
pass
return trajectory_list
@staticmethod
def create_trajectory_response(status, message, trajectory=None):
trajectory_msg = Trajectory()
if trajectory is not None:
trajectory_msg.trajectory_plan = trajectory.trajectory_plan
trajectory_msg.id = trajectory.id
trajectory_msg.name = trajectory.name
trajectory_msg.description = trajectory.description
return {
'status': status,
'message': message,
'trajectory': trajectory_msg
}
return {'status': status, 'message': message}
def callback_manage_trajectory(self, req):
cmd_type = req.cmd_type
trajectory_id = req.trajectory_id
trajectory_data = Trajectory(
name=req.trajectory.name,
description=req.trajectory.description,
trajectory_plan=req.trajectory.trajectory_plan)
# GET an existing trajectory
if cmd_type == TrajectoryCommandType.GET:
traj = self.get_trajectory(trajectory_id)
if traj is None:
return self.create_trajectory_response(
400, "No trajectory found with id : " + str(trajectory_id))
return self.create_trajectory_response(
200, "trajectory has been found", traj)
# Create a new trajectory
elif cmd_type == TrajectoryCommandType.CREATE:
(new_trajectory_id,
msg) = self.create_new_trajectory(trajectory_data)
new_trajectory = self.get_trajectory(new_trajectory_id)
if new_trajectory is None:
return self.create_trajectory_response(400, msg)
return self.create_trajectory_response(200, msg, new_trajectory)
# UPDATE existing trajectory
elif cmd_type == TrajectoryCommandType.UPDATE:
traj = self.get_trajectory(trajectory_id)
if traj is None:
return self.create_trajectory_response(
400, "No trajectory found with id : " + str(trajectory_id))
success, update_msg = self.update_trajectory(traj, trajectory_data)
if not success:
return self.create_trajectory_response(
400, update_msg + str(trajectory_id))
return self.create_trajectory_response(200, update_msg, traj)
# DELETE sequence
elif cmd_type == TrajectoryCommandType.DELETE:
success = self.delete_trajectory(trajectory_id)
if not success:
return self.create_trajectory_response(
400, "Could not delete trajectory with id : " +
str(trajectory_id))
return self.create_trajectory_response(
200, "Trajectory has been deleted")
# Wrong cmd_type
else:
return self.create_trajectory_response(400, "Wrong command type")
def delete_trajectory(self, tarjectory_id):
try:
self.fh.remove_trajectory(tarjectory_id)
except NiryoOneFileException as e:
return False
return True
def update_trajectory(self, traj, trajectory_data):
traj.name = trajectory_data.name
traj.description = trajectory_data.description
traj.trajectory_plan = trajectory_data.trajectory_plan
try:
self.parameters_validation.validate_trajectory(
traj.trajectory_plan)
except RobotCommanderException as e:
rospy.logwarn(str(e) + " Invalid trajectory ")
return False, "Could not update trajectory : Invalid trajectory with id : "
try:
self.fh.write_trajectroy(traj)
except NiryoOneFileException as e:
return False, " Could not update trajectory with id : "
return True, " Trajectory has been updated : "
def create_new_trajectory(self, traj):
new_id = self.fh.pick_new_id()
traj.id = new_id
try:
self.parameters_validation.validate_trajectory(
traj.trajectory_plan)
except RobotCommanderException as e:
rospy.logwarn(str(e) + " Invalid trajectory")
return -1, "Failed to create trajectory: invalid trajectory "
try:
self.fh.write_trajectroy(traj)
except NiryoOneFileException as e:
return -1, "Failed to create trajectory "
return new_id, "trajectory has been created : "
def get_trajectory(self, trajectory_id):
try:
return self.fh.read_trajectory(trajectory_id)
except NiryoOneFileException as e:
return None