def command_callback(self, msg):
try:
if msg.isValid():
if msg.getReceiver() == "tf_filter_supervisor":
command = msg.getCommand()
Logger.log(command)
if command == 'start':
sls = msg.getData('slot_size', float)
if sls is None:
sls = self.default_sls
self.startFiltering(msg.getData('tf_name'),
slot_size=sls)
elif command == 'stop':
self.stopFiltering()
self._send_command_result(True)
elif command == 'clear':
self.clearFiltering(msg.getData('tf_name'))
self._send_command_result(True)
elif command == 'clearstart':
self.clearFiltering(msg.getData('tf_name'))
sls = msg.getData('slot_size', float)
if sls is None:
sls = self.default_sls
self.startFiltering(msg.getData('tf_name'),
slot_size=sls)
elif command == 'reset':
self.resetFiltering()
self._send_command_result(True)
else:
Logger.error("INVALID input")
self._send_command_result(False)
except Exception as e:
print(e)
self._send_command_result(False)
def on_enter_steady(self):
self._sendFeedback(state="steady", status=self.feedback.STEADY)
Logger.log("State: STEADY")
self._sendResult(success=True, command=self.current_command)
self.trajectory_correct = False
self.current_command = None
self.idle()
def on_enter_ready(self):
Logger.log("State: READY")
self._sendFeedback(state="ready")
self.start_moving_timer = self.init_start_moving_timer
# if self.robot.getController().isMoving(epsilon=self.vel_noise_mean * 1.05): # TODO start-moving timer disabled!!!!!!!!!!!!!
if len(self.trajectory_generator.current_trajectory) > 0:
self.moving()
def start(self, robot_list, sensor_list, instruction_list, subtask_list):
Logger.log("\n\n ************* SFM start ***********")
self.managerSFM.loadInstructions(instruction_list)
for robot_name in robot_list:
self.managerSFM.addRobot(robot_name)
for sensor_name in sensor_list:
self.managerSFM.addSensor(sensor_name)
for subtask_name in subtask_list:
self.managerSFM.addSubTask(subtask_name)
self.sfm.getModel().next()
def on_enter_generatetrajectory(self):
Logger.log("State: GENERATETRAJECTORY")
self._sendFeedback(state="generatetrajectory",
command=self.current_command)
self.generate_timer = self.init_generate_timer
self.t_gen = -1
self.trajectoryThread = threading.Thread(
target=self._generateTrajectoryFromCommand)
self.trajectoryThread.start()
def joystick_key_callback(self, down, up):
# if self.joystick.KEY_BACK in down:
# Logger.log(" ALARM: CLEAR ")
# self.target_tf = self.current_tf
# self.alarm_proxy.resetAlarm()
# self.target_follower.resetAlarm()
if self.joystick.KEY_START in down:
Logger.log(" USER ALARM: ON ")
self.alarm_proxy.setAlarm()
self.target_follower.setAlarm()
if self.joystick.KEY_LOG in down:
self.target_tf = self.current_tf
def __init__(self, namespace):
self.namespace = namespace
self.action_client = actionlib.SimpleActionClient(
CommandProxyConfiguration.getProxyName(self.namespace),
TextCommandAction)
Logger.log(
"Command Proxy '{}' is waiting for server...".format(namespace))
self.action_client.wait_for_server()
Logger.log("Command Proxy '{}' ready!".format(namespace))
#####
self.feedback_callbacks = []
self.done_callbacks = []
self.active_callbacks = []
def __init__(self, robot_name):
self.robot_name = robot_name
# CREATE STATE MACHINE
self.supervisorSFM = SFMachineRobotSupervisorDefinition(
self.robot_name)
self.sfm = machines.SFMachine(name="sfm_" + self.robot_name +
"_supervisor",
model=self.supervisorSFM)
# DEFINE SM STATES
self.sfm.addState("start")
self.sfm.addState("idle")
self.sfm.addState("controlling")
self.sfm.addState("trajectory")
self.sfm.addState("alarm")
self.sfm.addState("direct")
# DEFINE SM TRANSITIONS
# start ...
self.sfm.addTransition("idle", "start", "idle")
# idle ...
self.sfm.addTransition("idle", "idle", "idle")
self.sfm.addTransition("alarm", "idle", "alarm")
self.sfm.addTransition("direct", "idle", "direct")
self.sfm.addTransition("trajectory", "idle", "trajectory")
# controlling ...
self.sfm.addTransition("idle", "controlling", "idle")
self.sfm.addTransition("controlling", "controlling", "controlling")
self.sfm.addTransition("trajectory", "controlling", "trajectory")
self.sfm.addTransition("alarm", "controlling", "alarm")
# alarm ...
self.sfm.addTransition("idle", "alarm", "idle")
self.sfm.addTransition("alarm", "alarm", "alarm")
# direct ...
self.sfm.addTransition("idle", "direct", "idle")
self.sfm.addTransition("direct", "direct", "direct")
self.sfm.addTransition("controlling", "direct", "controlling")
self.sfm.addTransition("trajectory", "direct", "trajectory")
self.sfm.addTransition("alarm", "direct", "alarm")
# trajectory ...
self.sfm.addTransition("trajectory", "trajectory", "trajectory")
self.sfm.addTransition("idle", "trajectory", "idle")
self.sfm.addTransition("alarm", "trajectory", "alarm")
self.sfm.create()
self.sfm.set_state("start")
Logger.log("\n\n ************* SFM ready to start ***********")
def on_enter_checktrajectory(self):
Logger.log("State: CHECKTRAJECTORY")
self._sendFeedback(state="checktrajectory")
# print("{}".format(self.trajectory_generator.current_trajectory))
if not self.trajectory_generator.computationSuccess():
self.error("NO_TRAJECTORY")
else:
if self._isOnTarget():
Logger.log("Already on Target")
self._sendFeedback(state="checktrajectory",
text="Already on Target")
self.steady()
else:
self.trajectory_correct = True
self.ready()
def __init__(self, service_name, service_manifest=IKService, timeout=5.0):
self.service_name = service_name
self.service_manifest = service_manifest
self.active = False
Logger.log("GeneralIKService: waiting for service " +
service_name + "...")
try:
rospy.wait_for_service(self.service_name, timeout=timeout)
except rospy.ROSException as e:
Logger.error("Service timeout:" + str(e))
return
self.service = rospy.ServiceProxy(
self.service_name, service_manifest)
self.active = True
Logger.log("Service Connected!")
def start(self, robot_command):
self.robotSFM.uploadCurrentCommand(robot_command)
if self.sfm.getModel().state != "error":
try:
self.sfm.getModel().sendcommand()
Logger.log("\n\n ************* SFM start ***********")
except:
self.sfm.getModel().error("transition_error")
else:
error_code = self.sfm.getModel().err
self.sfm.getModel()._sendResult(
success=False,
error=error_code,
command=self.sfm.getModel().current_command)
Logger.error(
"\n\n ************* persist error {} *********** \n\n".format(
error_code))
def on_loop_generatetrajectory(self):
self._sendFeedback(text=str(self.init_generate_timer),
state="generatetrajectory",
command=self.current_command,
status=self.feedback.WAITING_FOR_MOVING)
t_new = self.generate_timer / self.node_frequency
if self.t_gen != t_new:
self.t_gen = t_new
Logger.log("waiting start (time: {}s)".format(self.t_gen))
if self.trajectory_generator.isTrajectoryAvailable():
self.trajectoryThread.join()
self.checktrajectory()
elif self.generate_timer <= 0 or self.generate_trajectory_failure:
self.trajectoryThread.join()
self.error("NO_TRAJECTORY")
self.generate_timer -= 1
def __init__(self, task_name):
self.task_name = task_name
# CREATE STATE MACHINE
self.managerSFM = SFMachineTaskManagerDefinition(self.task_name)
self.sfm = machines.SFMachine(name="sfm_" + self.task_name +
"_manager",
model=self.managerSFM)
# DEFINE SM STATES
self.sfm.addState("start")
self.sfm.addState("idle")
self.sfm.addState("next")
self.sfm.addState("action")
self.sfm.addState("alarm")
# DEFINE SM TRANSITIONS
# start ...
self.sfm.addTransition("idle", "start", "idle")
self.sfm.addTransition("next", "start", "next")
# idle ...
self.sfm.addTransition("idle", "idle", "idle")
self.sfm.addTransition("alarm", "idle", "alarm")
self.sfm.addTransition("next", "idle", "next")
self.sfm.addTransition("action", "idle", "action")
# next ...
self.sfm.addTransition("next", "next", "next")
self.sfm.addTransition("alarm", "next", "alarm")
self.sfm.addTransition("action", "next", "action")
self.sfm.addTransition("idle", "next", "idle")
# alarm ...
self.sfm.addTransition("idle", "alarm", "idle")
self.sfm.addTransition("action", "alarm", "action")
self.sfm.addTransition("next", "alarm", "next")
self.sfm.addTransition("alarm", "alarm", "alarm")
# action ...
self.sfm.addTransition("idle", "action", "idle")
self.sfm.addTransition("next", "action", "next")
self.sfm.addTransition("action", "action", "action")
self.sfm.create()
self.sfm.set_state("start")
Logger.log("\n\n ************* SFM ready to start ***********")
def command_callback(self, msg):
try:
if msg.isValid():
if msg.getReceiver() == self.command_proxy_server_name:
command = msg.getCommand()
try:
data = msg.getData("data")
except:
data = None
Trigger(data)
Logger.log(self.command_proxy_server_name.replace("_supervisor", "") + ": " + command)
else:
self.command_server_last_message = None
else:
self.command_server_last_message = None
Logger.error(self.command_proxy_server_name.replace("_supervisor", "") + ": invalid message!!")
self.sendResponse(False)
except Exception as e:
self.sendResponse(False)
def on_enter_error(self, error_type=""):
Logger.log("State: ERROR")
self._sendFeedback(state="error")
self.trajectory_generator.clearTrajectory()
if error_type == "NO_COMMAND":
self.err = self.result.ERROR_NO_COMMAND
Logger.warning(
"Command string NOT recognized. Waiting for a new command")
elif error_type == "NO_TRAJECTORY":
self.err = self.result.ERROR_NO_TRAJECTORY
Logger.warning(
"Trajectory NOT computed. Waiting for a new command")
elif error_type == "START_MOVING":
self.err = self.result.ERROR_START_MOVING
Logger.warning(
"Moving Time Out. Target NOT Reached. Waiting for a new command"
)
elif error_type == "END_TRAJECTORY":
self.err = self.result.ERROR_END_TRAJECTORY
Logger.error(
"Trajectory Time Out. Target NOT Reached. Waiting for a new command"
)
elif error_type == "ROBOT_ALARM":
self.err = self.result.ERROR_ROBOT_ALARM
self.in_allarm = True
Logger.error(
"\n\n\n ROBOT ALARM!!! \n Restore the machine and Reset the Alarm \n\n\n"
)
elif error_type == "COMMAND_CONFLICT":
self.err = self.result.ERROR_COMMAND_CONFLICT
Logger.error(
"\n\n\n COMMAND CONFLICT!!! \n Waiting for a new command \n\n\n"
)
else:
self.err = self.result.ERROR_UNKNOWN
Logger.error("Unknown Error --- {}".format(error_type))
# send result
self._sendResult(success=False,
error=self.err,
command=self.current_command)
def generateTrajectoryFromFrames(self, q_in, frame1, frame2, hz, time):
""" Generates a linear trajectory between two cartesian frames """
cartesian_trajectory = self.generateCartesianFrames(
frame1, frame2, hz, time)
trajectory_points = []
if not self.checkIKService():
self.setCurrentTrajectory(trajectory_points)
return trajectory_points
else:
current_q = q_in
last_q = q_in
success_counter = 0.0
for frame in cartesian_trajectory:
trajectory_point = TrajectoryPoint(frame,
last_q,
0.0,
invalid=True)
response = self.robot.getIKService().simple_ik(frame, last_q)
if response.status == IKServiceResponse.STATUS_OK:
trajectory_point.q = response.q_out.data
trajectory_point.invalid = False
success_counter += 1.0
trajectory_points.append(trajectory_point)
last_q = trajectory_point.q
success_counter = success_counter / float(
len(cartesian_trajectory))
Logger.log("Trajectory Computation Success {}%".format(
success_counter * 100))
if success_counter > 0.99:
self.status = FuzzyTrajectoryGenerator.STATUS_OK
else:
self.status = FuzzyTrajectoryGenerator.STATUS_INVALID_POINTS
self.setCurrentTrajectory(trajectory_points)
return trajectory_points
def on_enter_action(self):
Logger.log("State: ACTION")
def on_enter_preparing(self):
Logger.log("State: PREPARING")
self._sendFeedback(state="preparing")
self.ee_frame = self.robot.getEEFrame()
self.vel_noise_samples = []
def on_enter_sendcommand(self):
Logger.log("State: SENDCOMMAND")
self._sendFeedback(state="sendcommand")
def start(self):
Logger.log("\n\n ************* SFM start ***********")
self.sfm.getModel().idle()
def on_enter_checkcommand(self):
Logger.log("State: CHECKCOMMAND")
self._sendFeedback(state="checkcommand", command=self.current_command)
self.target = self.interpreter.interpretCommand(self.current_command)
def on_enter_alarm(self):
Logger.log("State: ALARM")
self._reject_input_command(GlobalErrorList.ROBOT_ALARM)
def on_enter_trajectory(self):
Logger.log("State: TRAJECTORY")
def on_enter_direct(self):
Logger.log("State: DIRECT")
def on_enter_controlling(self):
Logger.log("State: CONTROLLING")
def on_enter_idle(self):
Logger.log("State: IDLE")
def command_callback(self, msg):
try:
self.command_success_flg = True
if msg.isValid():
receiver = msg.getReceiver()
if msg.getReceiver() == "{}_direct_commander".format(
self.robot_name):
command = msg.getCommand()
#⬢⬢⬢⬢⬢➤ ON
if command == "_on_":
Logger.log(
" ******** Direct Commander: ENABLE ******** ")
self._reset()
self._removeController()
self.active = True
#⬢⬢⬢⬢⬢➤ OFF
elif command == "_off_":
Logger.log(
" ******** Direct Commander: DISABLE ******** ")
self.current_tf = None
self._reset()
self._removeController()
self.active = False
#⬢⬢⬢⬢⬢➤ RESET
elif command == "_reset_":
Logger.log(" ******** RESET ******** ")
self._reset()
#⬢⬢⬢⬢⬢➤ JOYSTICK
elif command == "_joyon_":
Logger.log(" Joystick: ENABLE ")
self.joystick_flg = True
elif command == "_joyoff_":
Logger.log(" Joystick: DISABLE ")
self.joystick_flg = False
#⬢⬢⬢⬢⬢➤ SELECT CONTROLLER
elif command.startswith("controllerselect"):
ctrl_id_list = msg.getData("id")
Logger.log(
" Controller Selection: {}".format(ctrl_id_list))
for ctrl_id in ctrl_id_list:
if ctrl_id == "none":
Logger.log(" Neutral Controller ")
self._removeController()
else:
try:
self._addController(ctrl_id)
except:
self.command_success_flg = False
Logger.error(
"Wrong Controller ID: {}".format(
ctrl_id))
#⬢⬢⬢⬢⬢➤ CONTROLLER PARAMETERS
elif command.startswith("controllerparameters"):
parameters = msg.getData("parameters")
Logger.log(
" Parameters Update: {} ".format(parameters))
for ctrl_id in parameters.keys():
if ctrl_id in self.active_controllers.keys():
self.active_controllers[ctrl_id].setParameters(
parameters[ctrl_id])
else:
self.command_success_flg = False
Logger.error(
"Wrong Controller ID: {}".format(ctrl_id))
#⬢⬢⬢⬢⬢➤ REMOVE CONTROLLER
elif command.startswith("controllerdisable"):
Logger.log(" Removing Controller ")
ctrl_id_list = msg.getData("id")
for ctrl_id in ctrl_id_list:
if ctrl_id == "_all_":
self._reset()
self._removeController()
elif ctrl_id in self.active_controllers.keys():
self._reset(ctrl_id)
self._removeController(ctrl_id)
else:
self.command_success_flg = False
Logger.error(
"Wrong Controller ID: {}".format(ctrl_id))
# ⬢⬢⬢⬢⬢➤ START CONTROLLERS
elif command.startswith("controllerstart"):
Logger.log(" Controllers Start {}".format(
self.active_controllers.keys()))
input_data = msg.getData("input_data")
time.sleep(0.1)
for ctrl_id in input_data.keys():
if ctrl_id in self.active_controllers.keys():
self.active_controllers[ctrl_id].start(
input_data[ctrl_id])
else:
self.command_success_flg = False
Logger.error(
"Wrong Controller ID: {}".format(ctrl_id))
else:
self.command_success_flg = False
Logger.warning("INVALID COMMAND: {}".format(command))
else:
receiver = msg.getReceiver()
if msg.getReceiver() == "{}_direct_commander".format(
self.robot_name):
self.command_success_flg = False
except Exception as e:
self.command_success_flg = False
print(e)
Logger.log(" \nActive Controllers: {}\n".format(
self.active_controllers.keys()))
def on_enter_moving(self):
Logger.log("State: MOVING")
self._sendFeedback(state="moving", status=self.feedback.MOVING)
delay = int(0.6 * float(self.trajectory_time * self.node_frequency))
self.end_moving_timer = self.trajectory_time * self.node_frequency + delay
self.movement_progress = 0
def __init__(self, robot_name="", action_server=None):
self.robot_name = robot_name
self.robot = RobotMarket.createRobotByName(self.robot_name)
# CREATE STATE MACHINE
self.robotSFM = SFMachineRobotDefinition(self.robot)
self.sfm = machines.SFMachine(name="sfm_" + self.robot_name,
model=self.robotSFM)
# DEFINE SM STATES
self.sfm.addState("start")
self.sfm.addState("idle")
self.sfm.addState("preparing")
self.sfm.addState("sendcommand")
self.sfm.addState("checkcommand")
self.sfm.addState("generatetrajectory")
self.sfm.addState("checktrajectory")
self.sfm.addState("ready")
self.sfm.addState("moving")
self.sfm.addState("steady")
self.sfm.addState("error")
# DEFINE SM TRANSITIONS
# start ...
self.sfm.addTransition("prepare", "start", "preparing")
self.sfm.addTransition("sendcommand", "start", "sendcommand")
# idle ...
self.sfm.addTransition("sendcommand", "idle", "sendcommand")
# preparing ...
self.sfm.addTransition("idle", "preparing", "idle")
# sendcommand ...
self.sfm.addTransition("checkcommand", "sendcommand", "checkcommand")
# checkcommand ...
self.sfm.addTransition("generatetrajectory", "checkcommand",
"generatetrajectory")
# generatetrajectory ...
self.sfm.addTransition("checktrajectory", "generatetrajectory",
"checktrajectory")
# checktrajectory ...
self.sfm.addTransition("ready", "checktrajectory", "ready")
self.sfm.addTransition("steady", "checktrajectory", "steady")
# ready ...
self.sfm.addTransition("moving", "ready", "moving")
self.sfm.addTransition("steady", "ready", "steady")
# moving ...
self.sfm.addTransition("steady", "moving", "steady")
self.sfm.addTransition("sendcommand", "moving", "sendcommand")
# steady ...
self.sfm.addTransition("idle", "steady", "idle")
# ... in error
self.sfm.addTransition("error", "start", "error")
self.sfm.addTransition("error", "idle", "error")
self.sfm.addTransition("error", "preparing", "error")
self.sfm.addTransition("error", "checkcommand", "error")
self.sfm.addTransition("error", "sendcommand", "error")
self.sfm.addTransition("error", "generatetrajectory", "error")
self.sfm.addTransition("error", "checktrajectory", "error")
self.sfm.addTransition("error", "ready", "error")
self.sfm.addTransition("error", "moving", "error")
self.sfm.addTransition("error", "steady", "error")
self.sfm.addTransition("idle", "error", "idle")
self.sfm.addTransition("error", "error", "error")
self.sfm.create()
self.sfm.set_state("start")
Logger.log("\n\n ************* SFM ready to start ***********")
def on_enter_next(self):
Logger.log("State: NEXT")
self.nextInstruction()
