def __init__(self, robot):
# tf Listener
self.listener = tf.TransformListener()
# robot
self.robot = robot
self.robot_name = robot.getName()
# trajectory
self.trajectory_generator = FuzzyTrajectoryGenerator(self.robot)
self.trajectory = []
self.generate_trajectory_failure = False
self.trajectory_correct = False
# command
self.current_command = None
self.interpreter = CommandInterpreter(robot_name=self.robot_name,
robot_object=self.robot)
# frames
self.ee_frame = None
# target
self.target = None
self.target_q = None
self.dist_target = -1
# parameters
self.node_frequency = Parameters.get(obj=self.robot_name,
param="NODE_FREQUENCY")
self.trajectory_points = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_POINTS")
self.trajectory_time = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_TIME")
self.joints_tollerance = self.robot.getParam("joints_tollerance")
# timer
self.init_generate_timer = 15 * self.node_frequency # [sec]
self.init_start_moving_timer = 10 * self.node_frequency # [sec]
# external callback
self.feedback_ext_callback = None
self.feedback = RobotMotionFeedback()
self.result_ext_callback = None
self.result = RobotMotionResult()
# error
self.err = self.result.NO_ERROR
# noise velocity in steady state
self.vel_noise_mean = 0.015
# alarm
self.in_allarm = False
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
self.alarm_proxy.registerResetCallback(self.alarm_reset_callback)
def __init__(self, robot_name):
self.robot_name = robot_name
self.listener = tf.TransformListener()
self.message_database = MessageStorage()
# tool
self.tools_list = Parameters.get(obj=self.robot_name, param="TOOLS")
print self.tools_list
self.new_tools = {}
self.new_tools["dynamic"] = FrameVectorToKDL(
self.tools_list["gripper"])
# alarm
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
# target follower proxy
self.target_follower = TargetFollowerProxy(self.robot_name)
# robot outer message (no feedback/response)
self.outer_message_proxy = SimpleMessageProxy()
self.outer_message_proxy.register(self.command_callback)
# robot outer command
self.command_server = CommandProxyServer("{}_supervisor".format(
self.robot_name))
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
self.command_category = "unknown"
# robot inner message
self.inner_message_proxy = SimpleMessageProxy(
"{}_inner_message".format(self.robot_name))
# robot inner command
self.robot_trajectory_client = RobotMotionClient(
self.robot_name, ext_callback=self.robot_trajectory_callback)
self.robot_control_client = CommandProxyClient(
"{}_direct_commander".format(self.robot_name))
self.robot_control_client.registerDoneCallback(
self.control_done_callback)
def __init__(self, uifile, node):
super(CustomWindow, self).__init__(uifile=uifile)
self.robot_name = robot_name
############# Variable Initialization #############
self.available_tfs_map = {}
self.available_shapes_map = {}
self.available_ctrlpar_map = {}
self.wire_z_offset = 0
self.wire_x_offset = 0
self.wire_angle = 0
self.direct_control_active = False
self.controller_input = {}
self.controller_params = {}
self.selected_tf = ""
self.selected_tool = "gripper"
self.selected_shape = ""
self.selected_axis = [1, 0, 0]
self.selected_controller = "none"
self.command_sequence_index = 0
self.seq_run_flg = False
self.active_command = None
################
self.listener = tf.TransformListener()
self.node = node
self.node.createSubscriber("/tf", tfMessage, self.tf_callback)
self.robot_proxy_client = CommandProxyClient("{}_supervisor".format(
self.robot_name))
self.robot_proxy_client.registerDoneCallback(self.robot_done_callback)
self.robot_message_proxy = SimpleMessageProxy()
self.inner_message_proxy = SimpleMessageProxy(
"{}_inner_message".format(self.robot_name))
self.gripper_pub = self.node.createPublisher(
"/schunk_pg70/joint_setpoint", JointState)
# self.filter_pub = self.node.createPublisher(
# "/simple_message_stream/common", String)
################
self.tf_manager = TfManagerStub()
self.param_saver = SaveParams()
self.shape_saver = SaveShape(self.robot_name)
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
self.alarm_proxy.registerResetCallback(self.alarm_reset_callback)
self.message_database = MessageStorage()
# self.command_proxy_client = CommandProxyClient(
# "{}_supervisor".format(self.robot_name))
# self.command_proxy_client.registerDoneCallback(self.supervisor_done_callback)
############# Qt Objects ############
self.alarm_set_button.setStyleSheet("background-color: green")
self.direct_button.setStyleSheet("background-color: white")
#---
self.tool_box.currentIndexChanged.connect(self.tool_change)
self.gripper_check_box.stateChanged.connect(self.gripper)
self.joystick_check_box.stateChanged.connect(self.joystick)
self.direct_button.clicked.connect(self.directMode)
self.start_control_button.clicked.connect(self.startControl)
self.clear_control_button.clicked.connect(self.clearControlList)
self.select_control_button.clicked.connect(self.selectControl)
self.update_controller_button.clicked.connect(
self.setControlParameters)
self.load_ctrlpar_button.clicked.connect(self.loadControlParameters)
self.direct_reset_button.clicked.connect(self.directReset)
self.update_button.clicked.connect(self.tfListUpdate)
self.update_button_2.clicked.connect(self.shapeListUpdate)
self.filter_button.clicked.connect(self.startFilter)
self.clear_filter_button.clicked.connect(self.clearFilter)
self.save_shape_button.clicked.connect(self.saveShape)
self.save_controller_button.clicked.connect(self.saveController)
self.update_button_3.clicked.connect(self.ctrlParamsListUpdate)
self.delete_shape_button.clicked.connect(self.deleteShape)
self.save_tf_button.clicked.connect(self.saveTf)
self.alarm_reset_button.clicked.connect(self.reset_alarm)
self.alarm_set_button.clicked.connect(self.set_alarm)
self.tf_filter.textChanged.connect(self.tfListUpdate)
self.shape_filter.textChanged.connect(self.shapeListUpdate)
self.send_command.clicked.connect(self.sendCommand)
self.forward_button.clicked.connect(self.commandSequence)
self.forward_button.clicked.connect(self.nextCommand)
self.set_index_button.clicked.connect(self.setCommandSequenceIndex)
self.reset_index_button.clicked.connect(self.resetCommandSequenceIndex)
self.tfListUpdate()
class CustomWindow(PyQtWindow):
def __init__(self, uifile, node):
super(CustomWindow, self).__init__(uifile=uifile)
self.robot_name = robot_name
############# Variable Initialization #############
self.available_tfs_map = {}
self.available_shapes_map = {}
self.available_ctrlpar_map = {}
self.wire_z_offset = 0
self.wire_x_offset = 0
self.wire_angle = 0
self.direct_control_active = False
self.controller_input = {}
self.controller_params = {}
self.selected_tf = ""
self.selected_tool = "gripper"
self.selected_shape = ""
self.selected_axis = [1, 0, 0]
self.selected_controller = "none"
self.command_sequence_index = 0
self.seq_run_flg = False
self.active_command = None
################
self.listener = tf.TransformListener()
self.node = node
self.node.createSubscriber("/tf", tfMessage, self.tf_callback)
self.robot_proxy_client = CommandProxyClient("{}_supervisor".format(
self.robot_name))
self.robot_proxy_client.registerDoneCallback(self.robot_done_callback)
self.robot_message_proxy = SimpleMessageProxy()
self.inner_message_proxy = SimpleMessageProxy(
"{}_inner_message".format(self.robot_name))
self.gripper_pub = self.node.createPublisher(
"/schunk_pg70/joint_setpoint", JointState)
# self.filter_pub = self.node.createPublisher(
# "/simple_message_stream/common", String)
################
self.tf_manager = TfManagerStub()
self.param_saver = SaveParams()
self.shape_saver = SaveShape(self.robot_name)
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
self.alarm_proxy.registerResetCallback(self.alarm_reset_callback)
self.message_database = MessageStorage()
# self.command_proxy_client = CommandProxyClient(
# "{}_supervisor".format(self.robot_name))
# self.command_proxy_client.registerDoneCallback(self.supervisor_done_callback)
############# Qt Objects ############
self.alarm_set_button.setStyleSheet("background-color: green")
self.direct_button.setStyleSheet("background-color: white")
#---
self.tool_box.currentIndexChanged.connect(self.tool_change)
self.gripper_check_box.stateChanged.connect(self.gripper)
self.joystick_check_box.stateChanged.connect(self.joystick)
self.direct_button.clicked.connect(self.directMode)
self.start_control_button.clicked.connect(self.startControl)
self.clear_control_button.clicked.connect(self.clearControlList)
self.select_control_button.clicked.connect(self.selectControl)
self.update_controller_button.clicked.connect(
self.setControlParameters)
self.load_ctrlpar_button.clicked.connect(self.loadControlParameters)
self.direct_reset_button.clicked.connect(self.directReset)
self.update_button.clicked.connect(self.tfListUpdate)
self.update_button_2.clicked.connect(self.shapeListUpdate)
self.filter_button.clicked.connect(self.startFilter)
self.clear_filter_button.clicked.connect(self.clearFilter)
self.save_shape_button.clicked.connect(self.saveShape)
self.save_controller_button.clicked.connect(self.saveController)
self.update_button_3.clicked.connect(self.ctrlParamsListUpdate)
self.delete_shape_button.clicked.connect(self.deleteShape)
self.save_tf_button.clicked.connect(self.saveTf)
self.alarm_reset_button.clicked.connect(self.reset_alarm)
self.alarm_set_button.clicked.connect(self.set_alarm)
self.tf_filter.textChanged.connect(self.tfListUpdate)
self.shape_filter.textChanged.connect(self.shapeListUpdate)
self.send_command.clicked.connect(self.sendCommand)
self.forward_button.clicked.connect(self.commandSequence)
self.forward_button.clicked.connect(self.nextCommand)
self.set_index_button.clicked.connect(self.setCommandSequenceIndex)
self.reset_index_button.clicked.connect(self.resetCommandSequenceIndex)
self.tfListUpdate()
def setCommandSequenceIndex(self):
self.command_sequence_index = int(self.sequence_index_value.text())
def resetCommandSequenceIndex(self):
self.sequence_index_value.setText("0")
self.command_sequence_index = -1
self.nextCommand()
def commandSequence(self, command_str):
if command_str.startswith("FILTER"):
self.selected_tf = command_str.split(" ")[1]
self.clearFilter()
self.startFilter()
elif command_str.startswith("CLOSE_GRIPPER"):
self.gripper_check_box.setChecked(True)
self.gripper()
elif command_str.startswith("OPEN_GRIPPER"):
self.gripper_check_box.setChecked(False)
self.gripper()
else:
self.current_command.setText(command_str)
if self.link_to_send_check_box.isChecked():
self.sendCommand()
def sendCommand(self):
print(self.current_command.text())
cmd_str = str(self.current_command.text())
command = str(self.command_type_box.currentText())
if cmd_str.startswith("shape_"):
simple_message = SimpleMessage(command=command,
receiver="{}_supervisor".format(
self.robot_name))
shape_name = cmd_str.split(" ")[0] # self.selected_shape
simple_message.setData("shape_name", shape_name)
else:
simple_message = SimpleMessage(command=command,
receiver="{}_supervisor".format(
self.robot_name))
tf_name = cmd_str.split(" ")[0] # self.selected_tf
tool_name = cmd_str.split(" ")[1] # self.selected_tool
simple_message.setData("tf_name", tf_name)
simple_message.setData("tool_name", tool_name)
print(simple_message.toString())
self.active_command = self.robot_proxy_client.sendCommand(
simple_message.toString())
# self.robot_message_proxy.send(simple_message)
self.direct_button.setStyleSheet("background-color: white")
self.send_command.setStyleSheet("background-color: red")
def robot_done_callback(self, command):
sent_message = command.getSentMessage()
if sent_message.getCommand().startswith("goto"):
Logger.warning("trajectory DONE")
try:
source_done = command.response_data["trajectory_done"]
if source_done:
print("trajectory OK {}".format(
command.response_data["q_target_distance"]))
self.send_command.setStyleSheet("background-color: green")
else:
print("trajectory FAIL {}".format(
command.response_data["q_target_distance"]))
self.send_command.setStyleSheet("background-color: white")
except Exception as e:
print(e)
self.send_command.setStyleSheet("background-color: red")
def updateCommand(self, target_type):
if target_type == "shape":
command = "{}".format(self.selected_shape)
self.current_command.setText(command)
elif target_type == "tf":
command = "{} {}".format(self.selected_tf, self.selected_tool)
self.current_command.setText(command)
else:
return
def updateList(self, commands):
self.commands = commands
self.command_sequence_index = 0
model = QStandardItemModel(self.list)
for c in commands:
item = QStandardItem(str(c))
model.appendRow(item)
self.list.setModel(model)
self.list.selectionModel().currentChanged.disconnect()
self.list.selectionModel().currentChanged.connect(
self.listSelectionChanged)
self.setSelectedCommand(self.command_sequence_index)
def setSelectedCommand(self, index):
index = self.list.model().index(index, 0)
self.list.setCurrentIndex(index)
def nextCommand(self):
self.command_sequence_index += 1
self.command_sequence_index = self.command_sequence_index % len(
self.commands)
self.setSelectedCommand(self.command_sequence_index)
def listSelectionChanged(self, v):
print(v.data().toString())
command = v.data().toString()
self.current_command_text.setText(command)
self.commandSequence(str(command))
def directMode(self):
simple_message = SimpleMessage(command="direct",
receiver="{}_supervisor".format(
self.robot_name))
self.direct_control_active = not self.direct_control_active
if self.direct_control_active:
self.direct_button.setStyleSheet("background-color: green")
else:
self.direct_button.setStyleSheet("background-color: white")
simple_message.setData("active", self.direct_control_active)
self.robot_message_proxy.send(simple_message)
def directReset(self):
simple_message = SimpleMessage(command="directreset",
receiver="{}_supervisor".format(
self.robot_name))
self.robot_message_proxy.send(simple_message)
def set_alarm(self):
self.alarm_set_button.setStyleSheet("background-color: red")
self.enter_ready_to_reset = True
self.alarm_proxy.setAlarm()
def alarm_callback(self, alarm_info):
if alarm_info != self.alarm_proxy.NONE_ALARM:
self.alarm_set_button.setStyleSheet("background-color: red")
self.enter_ready_to_reset = True
elif alarm_info == self.alarm_proxy.NONE_ALARM and self.enter_ready_to_reset:
self.alarm_set_button.setStyleSheet("background-color: orange")
self.enter_ready_to_reset = False
def alarm_reset_callback(self):
self.alarm_set_button.setStyleSheet("background-color: green")
def reset_alarm(self):
self.alarm_set_button.setStyleSheet("background-color: green")
self.enter_ready_to_reset = False
cmd_str = "reset"
simple_message = SimpleMessage(command=cmd_str,
receiver="{}_supervisor".format(
self.robot_name))
self.robot_message_proxy.send(simple_message)
def gripper(self):
close_val = int(self.gripper_close_value.text())
open_val = int(self.gripper_open_value.text())
if self.gripper_check_box.isChecked() == True:
self._setGripper(close_val)
else:
self._setGripper(open_val)
def _setGripper(self, p, v=50, e=50):
js = JointState()
js.name = []
js.position = [p]
js.velocity = [v]
js.effort = [e]
self.gripper_pub.publish(js)
def joystick(self):
if self.joystick_check_box.isChecked() == True:
cmd_str = "_joyon_"
else:
cmd_str = "_joyoff_"
simple_message = SimpleMessage(command=cmd_str,
receiver="{}_direct_commander".format(
self.robot_name))
self.inner_message_proxy.send(simple_message)
print(cmd_str)
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ TRAJECTORY TAB ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def setSelectedTf(self, tf_name):
self.selected_tf = str(tf_name)
self.updateCommand("tf")
def setSelectedShape(self, shape_name):
self.selected_shape = shape_name
self.updateCommand("shape")
def tool_change(self, i):
self.selected_tool = str(self.tool_box.currentText())
simple_message = SimpleMessage(command="selecttool",
receiver="{}_supervisor".format(
self.robot_name))
simple_message.setData("tool_name", str(self.selected_tool))
print(simple_message.toString())
self.robot_message_proxy.send(simple_message)
self.updateCommand("tf")
def tf_change(self, v):
itms = self.tf_list.selectedIndexes()
for it in itms:
self.setSelectedTf(it.data().toString())
break
def shape_change(self, v):
itms = self.shape_list.selectedIndexes()
for it in itms:
self.setSelectedShape(it.data().toString())
break
def set_new_tool(self, transf):
simple_message = SimpleMessage(command="settool",
receiver="{}_supervisor".format(
self.robot_name))
simple_message.setData("tool_name", str(self.selected_tool))
simple_message.setData("new_tool_name", "dynamic")
simple_message.setData("transformation", transf)
print(simple_message.toString())
self.robot_message_proxy.send(simple_message)
def tf_callback(self, msg):
self.available_tfs = []
for tf in msg.transforms:
if len(tf.child_frame_id) > 0:
self.available_tfs_map[tf.child_frame_id] = tf
def tfListUpdate(self):
tf_list_model = QStandardItemModel(self.tf_list)
tf_sorted = sorted(self.available_tfs_map.keys())
filtered_str = str(self.tf_filter.text()).lower()
for tf_name in tf_sorted:
if filtered_str in tf_name.lower() or filtered_str == '':
item = QStandardItem(tf_name)
item.setEditable(False)
tf_list_model.appendRow(item)
self.tf_list.setModel(tf_list_model)
self.tf_list.selectionModel().selectionChanged.connect(self.tf_change)
self.tf_manager.update()
def shapeListUpdate(self):
shape_list_model = QStandardItemModel(self.shape_list)
db_msg = self.message_database.searchByName("shape_",
JointState,
single=False)
for i in range(0, len(db_msg)):
name = db_msg[i][1]["name"]
shape = db_msg[i][0].position
self.available_shapes_map[name] = shape
shape_sorted = sorted(self.available_shapes_map.keys())
filtered_str = str(self.shape_filter.text()).lower()
for shape_name in shape_sorted:
if filtered_str in shape_name.lower() or filtered_str == '':
item = QStandardItem(shape_name)
item.setEditable(False)
shape_list_model.appendRow(item)
self.shape_list.setModel(shape_list_model)
self.shape_list.selectionModel().selectionChanged.connect(
self.shape_change)
def startFilter(self):
msg_str = String()
tf_name = self.selected_tf
msg = SimpleMessage(receiver="tf_filter_supervisor", command="start")
msg.setData("slot_size", 200)
msg.setData("tf_name", tf_name)
self.robot_message_proxy.send(msg)
def clearFilter(self):
msg_str = String()
tf_name = self.selected_tf
msg = SimpleMessage(receiver="tf_filter_supervisor", command="clear")
msg.setData("tf_name", tf_name)
self.robot_message_proxy.send(msg)
def saveShape(self):
self.shape_saver.save("shape_" + str(self.obj_save_name_value.text()))
def deleteShape(self):
self.shape_saver.delete("shape_" +
str(self.obj_save_name_value.text()))
print("delete " + str(self.obj_save_name_value.text()))
def saveTf(self):
current_tool_tf = node.retrieveTransform(
self.robot_name + "/base_link", self.robot_name + "/tool", -1)
if current_tool_tf:
self.tf_manager.save(tf_name=self.robot_name + "/tool",
tf_parent=self.robot_name + "/base_link",
saving_name=str(
self.obj_save_name_value.text()))
time.sleep(0.5)
self.tf_manager.update()
time.sleep(0.5)
self.tf_manager.startPublish()
print("OK! Saved:{}".format(str(self.obj_save_name_value.text())))
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ CONTROLLERS TAB ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def ctrlParamsListUpdate(self):
list_model = QStandardItemModel(self.ctrlpar_list)
db_msg = self.message_database.searchByName("ctrl_",
String,
single=False)
for i in range(0, len(db_msg)):
name = db_msg[i][1]["name"]
data = json.loads(db_msg[i][0].data)
self.available_ctrlpar_map[name] = data
ctrlpar_sorted = sorted(self.available_ctrlpar_map.keys())
for ctrlpar_name in ctrlpar_sorted:
item = QStandardItem(ctrlpar_name)
item.setEditable(False)
list_model.appendRow(item)
self.ctrlpar_list.setModel(list_model)
self.ctrlpar_list.selectionModel().selectionChanged.connect(
self.ctrlpar_change)
def ctrlpar_change(self):
itms = self.ctrlpar_list.selectedIndexes()
for it in itms:
self.selected_ctrlpar.setText(it.data().toString())
break
def saveController(self):
self.setControlParameters(action="save")
print self.controller_params
self.param_saver.save("ctrl_" + str(self.obj_save_name_value.text()),
self.controller_params)
def clearControlList(self):
simple_message = SimpleMessage(command="controllerdisable",
receiver="{}_supervisor".format(
self.robot_name))
simple_message.setData("id", ["_all_"])
self.robot_message_proxy.send(simple_message)
def startControl(self):
controller_input = {}
#➤ force
if "force_spring" in self._getControllersList():
controller_input["force_spring"] = {}
if "force_dampedforward" in self._getControllersList():
controller_input["force_dampedforward"] = {
"axis": self.selected_axis
}
#➤ tactile
if "tactile_spring" in self._getControllersList():
controller_input["tactile_spring"] = {}
if "tactile_dampedforward" in self._getControllersList():
controller_input["tactile_dampedforward"] = {}
if "wire_insertion" in self._getControllersList():
hole_tf = None
while not hole_tf:
try:
hole_tf = node.retrieveTransform(
frame_id="tf_storage_hole",
parent_frame_id=self.robot_name + "/base_link",
time=-1)
hole_tf = transformations.KDLtoTf(hole_tf)
except:
print("Waiting for 'tf_storage_hole'...")
if hole_tf:
controller_input["wire_insertion"] = {
"hole_tf": hole_tf,
"wire_angle": self.wire_angle
}
else:
print("\n\n\n\nHOLE TF NOT FOUND\n\n\n\n")
if len(self._getControllersList()) == 0:
controller_input["none"] = {}
simple_message = SimpleMessage(command="controllerstart",
receiver="{}_supervisor".format(
self.robot_name))
simple_message.setData("input_data", controller_input)
self.robot_message_proxy.send(simple_message)
print(simple_message.toString())
def selectControl(self):
simple_message = SimpleMessage(command="controllerselect",
receiver="{}_supervisor".format(
self.robot_name))
if len(self._getControllersList()) == 0:
simple_message.setData("id", ["none"])
else:
simple_message.setData("id", self._getControllersList())
self.robot_message_proxy.send(simple_message)
print(simple_message.toString())
def updateControlParam(self):
simple_message = SimpleMessage(command="controllerparameters",
receiver="{}_supervisor".format(
self.robot_name))
simple_message.setData("parameters", self.controller_params)
self.robot_message_proxy.send(simple_message)
print(simple_message.toString())
def loadControlParameters(self):
self.controller_params = self.available_ctrlpar_map[str(
self.selected_ctrlpar.text())]
print self.controller_params
self.updateControlParam()
def setControlParameters(self, action="load"):
spring_force_params = {
"translation_mag": float(self.translation_mag_value.text()),
"rotation_mag": float(self.rotation_mag_value.text()),
"thresholds": eval(str(self.force_thresholds_value.text()))
}
damp_force_params = {
"velocity": float(self.velocity_value.text()),
"damp_force_threshold": float(self.damp_threshold_value.text()),
"damp_magnitude": float(self.damp_mag_value.text())
}
tactile_spring_params = {
"threshold": [
float(self.tactile_threshold_value_0.text()),
float(self.tactile_threshold_value_1.text()),
float(self.tactile_threshold_value_2.text())
],
"linear_gain": [
float(self.tactile_lin_gain_value_0.text()),
float(self.tactile_lin_gain_value_1.text()),
float(self.tactile_lin_gain_value_2.text())
],
"angular_gain": [
float(self.tactile_ang_gain_value_0.text()),
float(self.tactile_ang_gain_value_1.text()),
float(self.tactile_ang_gain_value_2.text())
],
"angular_action":
self.angular_action_check_box.isChecked(),
"linear_action":
self.linear_action_check_box.isChecked()
}
tactile_damp_params = {
"step_size":
float(self.step_size_value.text()),
"direction_gain":
float(self.direction_gain_value.text()),
"regulation_angular_action":
self.angular_reg_action_check_box.isChecked(),
"regulation_linear_action":
self.linear_reg_action_check_box.isChecked(),
"linear_regulation_gain": [
float(self.lin_reg_gain_value_0.text()),
float(self.lin_reg_gain_value_1.text()),
float(self.lin_reg_gain_value_2.text())
],
"angular_regulation_gain": [
float(self.ang_reg_gain_value_0.text()),
float(self.ang_reg_gain_value_1.text()),
float(self.ang_reg_gain_value_2.text())
],
"regulation_threshold": [
float(self.regulation_threshold_value_0.text()),
float(self.regulation_threshold_value_1.text()),
float(self.regulation_threshold_value_2.text())
],
"global_gain":
float(self.global_gain_value.text()),
"direction_correction":
self.correction_action_check_box.isChecked(),
"direction_compensation":
self.compensation_action_check_box.isChecked()
}
wire_insertion_params = {
"step_size":
float(self.in_step_size_value.text()),
"force_p_gain":
float(self.in_force_pgain_value.text()),
"force_threshold":
float(self.in_force_threshold_value.text()),
"regulation_p_gain": [
float(self.in_regulation_pgain_value_0.text()),
float(self.in_regulation_pgain_value_1.text()),
float(self.in_regulation_pgain_value_2.text())
],
"regulation_i_gain": [
float(self.in_regulation_igain_value_0.text()),
float(self.in_regulation_igain_value_1.text()),
float(self.in_regulation_igain_value_2.text())
],
"regulation_i_size": [
float(self.in_regulation_isize_value_0.text()),
float(self.in_regulation_isize_value_1.text()),
float(self.in_regulation_isize_value_2.text())
],
"threshold": [
float(self.in_regulation_threshold_value_0.text()),
float(self.in_regulation_threshold_value_1.text()),
float(self.in_regulation_threshold_value_2.text())
],
"force_projection":
self.force_projection_check_box.isChecked(),
"position_ball_radious":
float(self.position_ball_radious_value.text()),
"position_ball_offset":
float(self.position_ball_offset_value.text()),
"position_scaling_gain":
float(self.in_position_scaling_gain.text()),
"position_scaling_limits": [
float(self.in_position_scaling_limmin.text()),
float(self.in_position_scaling_limmax.text())
]
}
self.controller_params = {}
#➤ force
if "force_spring" in self._getControllersList():
self.controller_params["force_spring"] = spring_force_params
if "force_dampedforward" in self._getControllersList():
self.controller_params["force_dampedforward"] = damp_force_params
#➤ tactile
if "tactile_spring" in self._getControllersList():
self.controller_params["tactile_spring"] = tactile_spring_params
if "tactile_dampedforward" in self._getControllersList():
self.controller_params[
"tactile_dampedforward"] = tactile_damp_params
if "wire_insertion" in self._getControllersList():
self.controller_params["wire_insertion"] = wire_insertion_params
print action
if action == "save":
pass
else:
self.updateControlParam()
def _getControllersList(self):
selected_controllers = []
#➤ force
if self.spring_force_ctrl_box.isChecked():
selected_controllers.append("force_spring")
return selected_controllers
def getAxis(self, axis_name):
sign = int("{}1".format(axis_name[0]))
axis = axis_name[1]
if axis == "x":
return [sign, 0, 0]
elif axis == "y":
return [0, sign, 0]
elif axis == "z":
return [0, 0, sign]
else:
return [0, 0, 0]
class SFMachineRobotSupervisorDefinition(object):
DIRECT_COMMANDER = 100
TRAJECTORY_COMMANDER = 101
def __init__(self, robot_name):
self.robot_name = robot_name
self.listener = tf.TransformListener()
self.message_database = MessageStorage()
# tool
self.tools_list = Parameters.get(obj=self.robot_name, param="TOOLS")
print self.tools_list
self.new_tools = {}
self.new_tools["dynamic"] = FrameVectorToKDL(
self.tools_list["gripper"])
# alarm
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
# target follower proxy
self.target_follower = TargetFollowerProxy(self.robot_name)
# robot outer message (no feedback/response)
self.outer_message_proxy = SimpleMessageProxy()
self.outer_message_proxy.register(self.command_callback)
# robot outer command
self.command_server = CommandProxyServer("{}_supervisor".format(
self.robot_name))
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
self.command_category = "unknown"
# robot inner message
self.inner_message_proxy = SimpleMessageProxy(
"{}_inner_message".format(self.robot_name))
# robot inner command
self.robot_trajectory_client = RobotMotionClient(
self.robot_name, ext_callback=self.robot_trajectory_callback)
self.robot_control_client = CommandProxyClient(
"{}_direct_commander".format(self.robot_name))
self.robot_control_client.registerDoneCallback(
self.control_done_callback)
# self.robot_control_client.registerFeedbackCallback(
# self.control_feedback_callback) # TODO
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ CALLBACKS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def alarm_callback(self, alarm_info):
if alarm_info != self.alarm_proxy.NONE_ALARM:
Logger.error(" !!!!!!!! ALARM !!!!!!!! ")
self.alarm()
def robot_trajectory_callback(self, cb_type, msg):
if cb_type == self.robot_trajectory_client.FEEDBACK:
self.trajectory_sm_feedback = msg
# self._send_trajectory_feedback(msg) # TODO
elif cb_type == self.robot_trajectory_client.RESULT:
self._send_trajectory_command_result(msg)
if self.state != "alarm":
self.idle()
def control_done_callback(self, command):
# print command
if command:
success = (command.status == CommandMessage.COMMAND_STATUS_OK)
command = command.getSentMessage().getCommand()
else:
command = "NONE"
success = False
self._send_command_result(success)
print("controller: {} >>> SUCCESS={}".format(command, success))
# if self.state != "alarm":
# self.idle()
def command_action_callback(self, cmd):
self.command_server_last_message = cmd
cmd_msg = cmd.getSentMessage()
self.command_callback(cmd_msg)
def command_callback(self, msg):
if msg.isValid():
if msg.getReceiver() == "{}_supervisor".format(self.robot_name):
print msg.toString()
command = msg.getCommand()
try:
#⬢⬢⬢⬢⬢➤ RESET
if command == "reset":
self.command_category = "setting"
Logger.warning(" !!!!!!!! ALARM RESET !!!!!!!! ")
self.alarm_proxy.resetAlarm()
self.target_follower.resetAlarm()
self._reject_input_command(GlobalErrorList.ROBOT_ALARM)
self.idle()
elif self.state != "alarm":
#⬢⬢⬢⬢⬢➤ SET TOOL
if command == "settool": # change multiple data structure in a single multi-entry dictionary
self.command_category = "setting"
tool_name = msg.getData("tool_name")
new_tool_name = msg.getData("new_tool_name")
transf = msg.getData("transformation")
tool = self._getToolFromName(tool_name)
transf = FrameVectorToKDL(transf)
self.new_tools[new_tool_name] = tool * transf
print("Tools: {}".format(self.new_tools))
self._send_command_result(True)
self.idle()
#⬢⬢⬢⬢⬢➤ SELECT TOOL
elif command == "selecttool":
self.command_category = "setting"
tool_name = msg.getData("tool_name")
tool = self._getToolFromName(tool_name)
self._setTool(tool)
self._send_command_result(True)
self.idle()
#⬢⬢⬢⬢⬢➤ SET TRAJECTORY
elif command == "settrajectory":
self.command_category = "setting"
points = msg.getData(
"points"
) # if it returns none, this wasn't the set parameter
time = msg.getData("time")
if points is not None:
print Parameters.get(obj=self.robot_name,
param="TRAJECTORY_POINTS")
Parameters.change(obj=self.robot_name,
param="TRAJECTORY_POINTS",
value=int(points))
print Parameters.get(obj=self.robot_name,
param="TRAJECTORY_POINTS")
elif time is not None:
Parameters.change(obj=self.robot_name,
param="TRAJECTORY_TIME",
value=int(points))
self._send_command_result(True)
self.idle()
#⬢⬢⬢⬢⬢➤ MOVE TO TF
elif command == "movetotf":
self.command_category = "trajectory"
tf_name = msg.getData("tf_name")
tool_name = msg.getData("tool_name")
self._toTf(tf_name, tool_name, mode=command)
print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format(
tf_name))
self.trajectory()
#⬢⬢⬢⬢⬢➤ JUMP TO TF
elif command == "jumptotf":
self.command_category = "trajectory"
tf_name = msg.getData("tf_name")
tool_name = msg.getData("tool_name")
self._toTf(tf_name, tool_name, mode=command)
print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format(
tf_name))
self.trajectory()
#⬢⬢⬢⬢⬢➤ GO TO TF
elif command == "gototf":
self.command_category = "trajectory"
tf_name = msg.getData("tf_name")
tool_name = msg.getData("tool_name")
self._toTf(tf_name, tool_name, mode=command)
print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format(
tf_name))
self.trajectory()
#⬢⬢⬢⬢⬢➤ GO TO JOINTS
elif command == "gotojoints":
self.command_category = "trajectory"
joints_str = msg.getData("joints_vector")
self._toJoints(joints_str)
print("Joints: \033[1m\033[4m\033[94m{}\033[0m".
format(joints_str))
self.trajectory()
#⬢⬢⬢⬢⬢➤ GO TO SHAPE
elif command == "gotoshape":
self.command_category = "trajectory"
shape_name = msg.getData("shape_name", str)
self._toShape(shape_name)
print("Shape: \033[1m\033[4m\033[94m{}\033[0m".
format(shape_name))
self.trajectory()
#⬢⬢⬢⬢⬢➤ RESET CONTROLLERS
elif command == "directreset":
self.command_category = "setting"
self._resetDirectCommander()
#⬢⬢⬢⬢⬢➤ ENABLE CONTROLLERS
elif command == "direct":
self.command_category = "setting"
active = msg.getData("active")
if active:
self._switchCommander(self.DIRECT_COMMANDER)
self.direct()
else:
self._switchCommander(
self.TRAJECTORY_COMMANDER)
self.idle()
#⬢⬢⬢⬢⬢➤ START CONTROLLERS
elif command == "controllerstart":
self.command_category = "setting"
self._sendCommandActionToController(msg)
print("Inputs: {}".format(
msg.getData("input_data")))
self.controlling()
#⬢⬢⬢⬢⬢➤ UPDATE CONTROLLERS PARAMETERS
elif command == "controllerparameters":
self.command_category = "setting"
self._sendCommandActionToController(msg)
print("Params: {}".format(
msg.getData("parameters")))
#⬢⬢⬢⬢⬢➤ SELECT CONTROLLERS
elif command == "controllerselect":
self.command_category = "setting"
self._sendCommandActionToController(msg)
print("Controller selected: {}".format(
msg.getData("id")))
#⬢⬢⬢⬢⬢➤ DISABLE CONTROLLERS
elif command == "controllerdisable":
self.command_category = "setting"
self._sendCommandActionToController(msg)
print("Controllers removed: {}".format(
msg.getData("id")))
else:
self.command_category = "unknown"
Logger.error("INVALID input")
self._reject_input_command()
except Exception as e:
Logger.error(e)
self._reject_input_command()
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PRIVATE METHODS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def _getToolFromName(self, tool_name):
tool_v = self.tools_list[tool_name]
tool = FrameVectorToKDL(tool_v)
return tool
def _sendCommandActionToController(self, msg):
msg.setReceiver("{}_direct_commander".format(self.robot_name))
msg.setData("type", "force")
self.robot_control_client.sendCommand(msg.toString())
def _sendMessageToController(self, msg):
msg.setReceiver("{}_direct_commander".format(self.robot_name))
msg.setData("type", "force")
self.inner_message_proxy.send(msg)
def _resetDirectCommander(self):
simple_message = SimpleMessage(command="_reset_",
receiver="{}_direct_commander".format(
self.robot_name))
self._sendCommandActionToController(simple_message)
def _setTool(self, tool):
""" tool is a PyKDL frame"""
self.target_follower.setTool(tool)
def _toJoints(self, joint_str):
self._switchCommander(self.TRAJECTORY_COMMANDER,
expect_done_response=False)
q_str = joint_str
txt_cmd = "joints {}".format(q_str)
self.robot_trajectory_client.generateNewGoal(txt_cmd)
def _toShape(self, shape_name):
self._switchCommander(self.TRAJECTORY_COMMANDER,
expect_done_response=False)
txt_cmd = self._getCommandFromShape(shape_name)
self.robot_trajectory_client.generateNewGoal(txt_cmd)
def _toTf(self, tf_name, tool_name, mode="goto"):
self._switchCommander(self.TRAJECTORY_COMMANDER,
expect_done_response=False)
if tool_name.startswith("dynamic"):
tool = self.new_tools[tool_name]
else:
tool = self._getToolFromName(tool_name)
self._setTool(tool)
txt_cmd = "{} {}".format(mode, tf_name)
self.robot_trajectory_client.generateNewGoal(txt_cmd)
def _getCommandFromShape(self, shape_name):
db_msg = self.message_database.searchByName(shape_name,
JointState,
single=False)
q_str = str(db_msg[0][0].position).split("(")[1].split(")")[0].replace(
" ", "")
txt_cmd = "joints {}".format(q_str)
return txt_cmd
def _switchCommander(self, id, expect_done_response=True):
if id == self.DIRECT_COMMANDER:
simple_message = SimpleMessage(
command="_on_",
receiver="{}_direct_commander".format(self.robot_name))
self.target_follower.setSource(TargetFollowerProxy.SOURCE_DIRECT)
elif id == self.TRAJECTORY_COMMANDER:
simple_message = SimpleMessage(
command="_off_",
receiver="{}_direct_commander".format(self.robot_name))
self.target_follower.setSource(
TargetFollowerProxy.SOURCE_TRAJECTORY)
if expect_done_response:
self._sendCommandActionToController(simple_message)
else:
self._sendMessageToController(simple_message)
def _send_trajectory_command_result(self, msg):
if self.command_server_last_message:
if msg is None:
error = GlobalErrorList.TRAJECTORY_ACTION_RESPONSE_NONETYPE
Logger.error("Trajectory action response None-type")
trajectory_done = False
q_target_distance = -999
else:
error = msg.error
q_target_distance = msg.q_target_distance
trajectory_done = msg.success
self.command_server_last_message.addResponseData("error", error)
self.command_server_last_message.addResponseData(
"q_target_distance", q_target_distance)
self.command_server_last_message.addResponseData(
"trajectory_done", trajectory_done)
self._send_command_result(trajectory_done)
def _reject_input_command(self, error_type=GlobalErrorList.UNKNOWN_ERROR):
if self.command_server_last_message:
self.command_server_last_message.addResponseData(
"error", error_type)
if self.command_category == "trajectory":
self._send_trajectory_command_result(None)
else:
self._send_command_result(False)
def _send_command_result(self, success):
if self.command_server_last_message:
if success:
self.command_server.resolveCommand(
self.command_server_last_message)
else:
self.command_server.rejectCommand(
self.command_server_last_message)
self.command_server_last_message = None
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ STATES ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ IDLE ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
# ➤ ➤ ➤ ➤ ➤ IDLE: enter
def on_enter_idle(self):
Logger.log("State: IDLE")
# ➤ ➤ ➤ ➤ ➤ IDLE: loop
def on_loop_idle(self):
pass
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ CONTROLLING ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
# ➤ ➤ ➤ ➤ ➤ CONTROLLING: enter
def on_enter_controlling(self):
Logger.log("State: CONTROLLING")
# ➤ ➤ ➤ ➤ ➤ CONTROLLING: loop
def on_loop_controlling(self):
pass
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ DIRECT ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
# ➤ ➤ ➤ ➤ ➤ DIRECT: enter
def on_enter_direct(self):
Logger.log("State: DIRECT")
# ➤ ➤ ➤ ➤ ➤ DIRECT: loop
def on_loop_direct(self):
if not rosnode.rosnode_ping("{}_direct_motion".format(
self.robot_name)):
self.alarm_proxy.setAlarm(AlarmProxy.NODE_DIED)
self.alarm()
pass
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ TRAJECTORY ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
# ➤ ➤ ➤ ➤ ➤ TRAJECTORY: enter
def on_enter_trajectory(self):
Logger.log("State: TRAJECTORY")
# self._switchCommander(self.TRAJECTORY_COMMANDER)
# ➤ ➤ ➤ ➤ ➤ TRAJECTORY: loop
def on_loop_trajectory(self):
if not rosnode.rosnode_ping("{}_trajectory_motion".format(
self.robot_name)):
self.alarm_proxy.setAlarm(AlarmProxy.NODE_DIED)
self.alarm()
pass
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ ALARM ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
# ➤ ➤ ➤ ➤ ➤ ALARM: enter
def on_enter_alarm(self):
Logger.log("State: ALARM")
self._reject_input_command(GlobalErrorList.ROBOT_ALARM)
# ➤ ➤ ➤ ➤ ➤ ALARM: loop
def on_loop_alarm(self):
self._reject_input_command(GlobalErrorList.ROBOT_ALARM)
def __init__(self, robot_name, controllers_dict=None):
self.robot_name = robot_name
self.current_tf = None
self.target_tf = None
self.tf_target_name = "follow_target"
self.active = False
self.command_success_flg = False
# target follower
self.target_follower = TargetFollowerProxy(self.robot_name)
# alarm
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
self.alarm_proxy.registerResetCallback(self.alarm_reset_callback)
# command message
self.message_proxy = SimpleMessageProxy("{}_inner_message".format(
self.robot_name))
self.message_proxy.register(self.command_callback)
# command action
self.command_server_last_message = None
self.command_server = CommandProxyServer("{}_direct_commander".format(
self.robot_name))
self.command_server.registerCallback(self.command_action_callback)
# tf
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
# Joystick Contorol
joy_sub = rospy.Subscriber('/joy',
Joy,
self.joy_callback,
queue_size=1)
self.joystick = Joystick(translation_mag=0.0005, rotation_mag=0.0008)
self.joystick.registerCallback(self.joystick_key_callback)
self.joystick_flg = False # Disable by default
# Closed-Loop Control
if controllers_dict is None or controllers_dict == {}:
controllers_dict = {"none": NeutralController()}
self.controllers_dict = controllers_dict
self.active_controllers = {}
self.active_controllers["none"] = NeutralController()
Logger.warning("READY!!!")
# Feedback
self.feedback_data = {}
# Force
rospy.Subscriber('/atift',
Twist,
self.force_feedback_callback,
queue_size=1)
# Tactile
rospy.Subscriber('/tactile',
Float64MultiArray,
self.tactile_feedback_callback,
queue_size=1)
class DirectCommander(object):
def __init__(self, robot_name, controllers_dict=None):
self.robot_name = robot_name
self.current_tf = None
self.target_tf = None
self.tf_target_name = "follow_target"
self.active = False
self.command_success_flg = False
# target follower
self.target_follower = TargetFollowerProxy(self.robot_name)
# alarm
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
self.alarm_proxy.registerResetCallback(self.alarm_reset_callback)
# command message
self.message_proxy = SimpleMessageProxy("{}_inner_message".format(
self.robot_name))
self.message_proxy.register(self.command_callback)
# command action
self.command_server_last_message = None
self.command_server = CommandProxyServer("{}_direct_commander".format(
self.robot_name))
self.command_server.registerCallback(self.command_action_callback)
# tf
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
# Joystick Contorol
joy_sub = rospy.Subscriber('/joy',
Joy,
self.joy_callback,
queue_size=1)
self.joystick = Joystick(translation_mag=0.0005, rotation_mag=0.0008)
self.joystick.registerCallback(self.joystick_key_callback)
self.joystick_flg = False # Disable by default
# Closed-Loop Control
if controllers_dict is None or controllers_dict == {}:
controllers_dict = {"none": NeutralController()}
self.controllers_dict = controllers_dict
self.active_controllers = {}
self.active_controllers["none"] = NeutralController()
Logger.warning("READY!!!")
# Feedback
self.feedback_data = {}
# Force
rospy.Subscriber('/atift',
Twist,
self.force_feedback_callback,
queue_size=1)
# Tactile
rospy.Subscriber('/tactile',
Float64MultiArray,
self.tactile_feedback_callback,
queue_size=1)
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ CALLBACKS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# TODO problema: OGNI VOLTA CHE SI AGGIUNGE UN FEEDBACK OCCORRE CREARE UNA CALLBACK!!!!
def force_feedback_callback(self, msg):
msg = self._getForceOnTool(msg)
self.feedback_data["atift"] = msg
def tactile_feedback_callback(self, msg):
self.feedback_data["tactile"] = msg
def joy_callback(self, msg):
self.joystick.update(msg)
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 command_action_callback(self, cmd):
self.command_server_last_message = cmd
cmd_msg = cmd.getSentMessage()
self.command_callback(cmd_msg)
time.sleep(0.1)
self._send_command_result(self.command_success_flg)
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 alarm_callback(self, alarm_info):
if alarm_info != self.alarm_proxy.NONE_ALARM:
Logger.error(" !!!!!!!! ALARM !!!!!!!! ")
self.target_tf = self.current_tf
self.active = False
def alarm_reset_callback(self):
Logger.warning(" ******** ALARM CLEAR ******** ")
self.target_tf = self.current_tf
self._reset()
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PRIVATE METHODS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def _send_command_result(self, success):
if self.command_server_last_message:
Logger.warning("Command result:{}".format(success))
if success:
self.command_server.resolveCommand(
self.command_server_last_message)
else:
self.command_server.rejectCommand(
self.command_server_last_message)
self.command_server_last_message = None
def _removeController(self, ctrl_id=None):
if ctrl_id is None:
self.active_controllers = {}
self.active_controllers["none"] = NeutralController()
else:
if ctrl_id in self.active_controllers.keys():
self.active_controllers.pop(ctrl_id)
if self.active_controllers == {}:
self.active_controllers["none"] = NeutralController()
else:
Logger.error(
"Trying to remove a deactivate controller: {}".format(
ctrl_id))
def _addController(self, ctrl_id=None):
if ctrl_id:
self.active_controllers[ctrl_id] = self.controllers_dict[ctrl_id]
if "none" in self.active_controllers.keys():
self._removeController("none")
else:
Logger.error(
"Trying to activate an unlisted controller: {}".format(
ctrl_id))
def _reset(self, ctrlid=None):
self.target_tf = self.current_tf
if ctrlid is None:
for ctrl_id in self.active_controllers.keys():
self.active_controllers[ctrl_id].reset()
else:
self.active_controllers[ctrlid].reset()
def _getForceOnTool(self, twist_on_sensor):
linear_force_on_sensor = PyKDL.Frame(TwistToKDLVector(twist_on_sensor))
tr_link6_fs = PyKDL.Frame(
PyKDL.Vector(WRIST_FT_SENSOR_POSITION[0],
WRIST_FT_SENSOR_POSITION[1],
WRIST_FT_SENSOR_POSITION[2]))
tr_link6_fs.M = tr_link6_fs.M.RotX(-scipy.pi)
tr_tool_link6 = transformations.retrieveTransform(
self.listener,
self.robot_name + "/tool",
self.robot_name + "/link6",
none_error=True,
time=rospy.Time(0),
print_error=True)
# TODO aggiungere una tf eef (in comau coincide con link6 in shunk con finger1 (me media tra i 2))
try:
tr_tool_fs = tr_tool_link6 * tr_link6_fs
linear_force_on_tool = tr_tool_fs * linear_force_on_sensor
twist_on_tool = TwistFormKDLVector(linear_force_on_tool.p)
return twist_on_tool
except Exception as e:
print(e)
def _derivateLinearForce(self, twist):
dT = Twist()
dT.linear.x = self.force_calculator.derivate(twist.linear.x)
dT.linear.y = self.force_calculator.derivate(twist.linear.y)
dT.linear.z = self.force_calculator.derivate(twist.linear.z)
np.savetxt(self.file_handler,
[[time.time(), dT.linear.x, dT.linear.y, dT.linear.z]],
fmt='%.18e %.18e %.18e %.18e',
delimiter=' ')
return dT
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PUBLIC METHODS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def stepForward(self):
try:
if self.active:
self.current_tf = transformations.retrieveTransform(
self.listener,
self.robot_name +
"/base_link", # TODO aggiungere base_link in shunck
self.robot_name + "/tool",
none_error=True,
time=rospy.Time(0),
print_error=True)
if self.current_tf is not None:
if self.target_tf is None:
self.target_tf = self.current_tf
#➤ Joystick
if self.joystick_flg:
self.target_tf = self.joystick.transformT(
self.target_tf)
#➤ Control Action
else:
self.feedback_data["current_tf"] = self.current_tf
self.feedback_data["target_tf"] = self.target_tf
for ctrl_id in self.active_controllers.keys():
self.target_tf = self.active_controllers[
ctrl_id].output(self.feedback_data)
####### Publish and Set Target #######
transformations.broadcastTransform(self.br,
self.target_tf,
self.tf_target_name,
self.robot_name +
"/base_link",
time=rospy.Time.now())
dist = 1
# diff_tf = self.current_tf.Inverse() * self.target_tf
# transformations.broadcastTransform(self.br, diff_tf, "diff_tf",
# self.robot_name + "/base_link", time = rospy.Time.now())
# dist_lin, distr_ang=transformations.frameDistance(diff_tf,
# PyKDL.Frame())
# distr_ang=np.linalg.norm(distr_ang)
# dist_lin=np.linalg.norm(
# transformations.KDLVectorToNumpyArray(dist_lin))
# dist=distr_ang + dist_lin
# print dist
if dist > 0.0003 or self.joystick_flg:
self.target_follower.setTarget(
target=self.target_tf,
target_type=TargetFollowerProxy.TYPE_POSE,
target_source=TargetFollowerProxy.SOURCE_DIRECT)
else:
Logger.error("tf not ready!!")
else:
pass
except Exception as e:
Logger.warning(e)
pass
class SFMachineRobotDefinition(object):
def __init__(self, robot):
# tf Listener
self.listener = tf.TransformListener()
# robot
self.robot = robot
self.robot_name = robot.getName()
# trajectory
self.trajectory_generator = FuzzyTrajectoryGenerator(self.robot)
self.trajectory = []
self.generate_trajectory_failure = False
self.trajectory_correct = False
# command
self.current_command = None
self.interpreter = CommandInterpreter(robot_name=self.robot_name,
robot_object=self.robot)
# frames
self.ee_frame = None
# target
self.target = None
self.target_q = None
self.dist_target = -1
# parameters
self.node_frequency = Parameters.get(obj=self.robot_name,
param="NODE_FREQUENCY")
self.trajectory_points = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_POINTS")
self.trajectory_time = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_TIME")
self.joints_tollerance = self.robot.getParam("joints_tollerance")
# timer
self.init_generate_timer = 15 * self.node_frequency # [sec]
self.init_start_moving_timer = 10 * self.node_frequency # [sec]
# external callback
self.feedback_ext_callback = None
self.feedback = RobotMotionFeedback()
self.result_ext_callback = None
self.result = RobotMotionResult()
# error
self.err = self.result.NO_ERROR
# noise velocity in steady state
self.vel_noise_mean = 0.015
# alarm
self.in_allarm = False
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
self.alarm_proxy.registerResetCallback(self.alarm_reset_callback)
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PUBLIC METHODS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def uploadCurrentCommand(self, command):
if self.current_command != None:
self.error("COMMAND_CONFLICT")
else:
self.current_command = command
def isTrajectoryAdmissible(self):
if self.trajectory_generator != None:
if self.trajectory_generator.isTrajectoryAvailable():
return self.trajectory_correct
else:
return False
def applyTrajectory(self):
if len(self.trajectory_generator.current_trajectory) > 0:
self.trajectory_generator.applyTrajectory(rospy.Time.now())
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PRIVATE METHODS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
def _sendFeedback(self,
text="",
state="",
command="",
status=0,
progress=-1.0,
warning=0):
if self.feedback_ext_callback:
self.feedback.current_state = state
self.feedback.text = text
self.feedback.executing_command = command
self.feedback.status = status
self.feedback.progress = progress
self.feedback.warning = warning
self.feedback_ext_callback(self.feedback)
else:
Logger.error("External callback for FEEDBACK update NOT found!")
def _sendResult(self, success=False, command="", error=0):
if self.current_command != None:
if self.result_ext_callback:
q = self.robot.getController().getQ()
pose = geometry_msgs.msg.Pose()
eef_frame = self.robot.getEEFrame()
pose.position.x = eef_frame.p.x()
pose.position.y = eef_frame.p.y()
pose.position.z = eef_frame.p.z()
eef_quaternion = eef_frame.M.GetQuaternion()
pose.orientation.x = eef_quaternion[0]
pose.orientation.y = eef_quaternion[1]
pose.orientation.z = eef_quaternion[2]
pose.orientation.w = eef_quaternion[3]
self.result.pose = pose
self.result.q.data = list(q)
self.result.success = success
self.result.error = error
self.result.executed_command = command
self.result.q_target_distance = self.dist_target
self.result_ext_callback(self.result)
succ = "\033[91m" + "\033[92m" * success + "\033[1m\033[4m{}\033[0m".format(
success)
print("{}: {}".format(self.current_command, succ))
else:
Logger.error("External callback for RESULT update NOT found!")
self.current_command = None
def _computeProgress(self):
current_q = self.robot.getController().getQ()
target_q = self.target_q
initial_q = self.initial_q
self.movement_progress = 100.0 * (1.0 - abs(
(norm(subtract(current_q, target_q))) /
(norm(subtract(initial_q, target_q)))))
def _isOnTarget(self, tollerance=None):
if tollerance == None:
tollerance = self.joints_tollerance
current_q = self.robot.getController().getQ()
self.dist_target = norm(subtract(self.target_q, current_q))
sys.stdout.write("Working --> Distance From Target: {} \r".format(
self.dist_target))
sys.stdout.flush()
return self.dist_target < tollerance
def _generateTrajectoryFromCommand(self):
self.initial_q = self.robot.getController().getQ()
self.trajectory_generator.clearTrajectory()
self.generate_trajectory_failure = False
steps = self.trajectory_time * self.node_frequency
if self.target:
self.trajectory_points = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_POINTS")
if self.trajectory_points is None:
self.trajectory_points = self.node_frequency * self.trajectory_time
try:
if self.interpreter.trajectory_specs == "linear_joints":
self.trajectory = self.trajectory_generator.generateTrajectoryFromShapes(
self.initial_q, self.target, self.trajectory_points, 1)
elif self.interpreter.trajectory_specs == "linear_frames_to_free_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFromFrames(
self.initial_q, self.robot.getEEFrame(), self.target,
self.node_frequency, self.trajectory_time)
elif self.interpreter.trajectory_specs == "free_frames_to_linear_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFreeFromFrames(
self.initial_q,
self.robot.getEEFrame(),
self.target,
steps=steps,
agumented_middle_joints=True,
middle_frames=10,
number_of_tries=1,
perturbate_middle_frames=False)
elif self.interpreter.trajectory_specs == "target_frame_to_target_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFreeFromFrames(
self.initial_q,
self.robot.getEEFrame(),
self.target,
steps=1,
agumented_middle_joints=False,
middle_frames=20,
number_of_tries=5,
perturbate_middle_frames=False)
else:
self.trajectory = []
except Exception as e:
Logger.error(e)
self.trajectory = []
else:
self.trajectory = []
# Trajectory target update
if len(self.trajectory) > 0:
self.target_q = self.trajectory_generator.getTargetQ()
else:
self.generate_trajectory_failure = True
self.target_q = self.initial_q
def alarm_callback(self, alarm_info):
if alarm_info != self.alarm_proxy.NONE_ALARM:
self.error("ROBOT_ALARM")
def alarm_reset_callback(self):
if self.state == "error" and self.in_allarm:
Logger.warning("\nAlarm Reset\n")
self.in_allarm = False
# self.idle()
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ STATES ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ IDLE ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
# ➤ ➤ ➤ ➤ ➤ IDLE: enter
def on_enter_idle(self):
Logger.log("State: IDLE")
self._sendFeedback(state="idle")
def on_loop_idle(self):
pass
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ PREPARING ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ PREPARING: enter
def on_enter_preparing(self):
Logger.log("State: PREPARING")
self._sendFeedback(state="preparing")
self.ee_frame = self.robot.getEEFrame()
self.vel_noise_samples = []
#➤ ➤ ➤ ➤ ➤ PREPARING: loop
def on_loop_preparing(self):
if self.vel_noise_samples >= 30:
self.vel_noise_mean = mean(self.vel_noise_samples)
self.idle()
else:
self.vel_noise_samples.append(self.robot.getController().getQ())
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ SENDCOMMAND ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ SENDCOMMAND: enter
def on_enter_sendcommand(self):
Logger.log("State: SENDCOMMAND")
self._sendFeedback(state="sendcommand")
#➤ ➤ ➤ ➤ ➤ SENDCOMMAND: loop
def on_loop_sendcommand(self):
self._sendFeedback(status=self.feedback.WAITING_FOR_COMMAND)
if self.current_command != None:
self.checkcommand()
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ CHECKCOMMAND ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ CHECKCOMMAND: enter
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)
#➤ ➤ ➤ ➤ ➤ CHECKCOMMAND: loop
def on_loop_checkcommand(self):
if self.interpreter.isCommandValid():
self.generatetrajectory()
else:
self.error("NO_COMMAND")
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ GENERATETRAJECTORY ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ GENERATETRAJECTORY: enter
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()
#➤ ➤ ➤ ➤ ➤ GENERATETRAJECTORY: loop
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
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ CHECKTRAJECTORY ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ CHECKTRAJECTORY: enter
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()
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ READY ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ READY: enter
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()
#➤ ➤ ➤ ➤ ➤ READY: loop
def on_loop_ready(self):
self._sendFeedback(text=str(self.start_moving_timer),
state="ready",
status=self.feedback.WAITING_FOR_MOVING)
# if self.robot.getController().isMoving(epsilon=self.vel_noise_mean * 1.05):
if len(self.trajectory_generator.current_trajectory) > 0:
self.moving()
elif self.start_moving_timer <= 0:
self.error("START_MOVING")
self.start_moving_timer -= 1
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ MOVING ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ MOVING: enter
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
# self.progressThread = threading.Thread(target=self._computeProgress)
# self.progressThread.start()
#➤ ➤ ➤ ➤ ➤ MOVING: loop
def on_loop_moving(self):
# Logger.log("waiting end (timer: {})".format(self.end_moving_timer))
self._computeProgress()
if self.end_moving_timer > 0:
wrn = self.feedback.NO_WARNING
else:
wrn = self.feedback.WARNING_TRAJACTORY_TIME_OUT
self._sendFeedback(text=str(self.end_moving_timer),
progress=self.movement_progress,
state="moving",
status=self.feedback.MOVING,
warning=wrn)
if not self.robot.getController().isMoving(
epsilon=self.vel_noise_mean * 1.05):
if self._isOnTarget():
print("On Target --> Distance: {}".format(self.dist_target))
self.steady()
elif self.end_moving_timer <= 0:
print("Distance From Target: {}".format(self.dist_target))
self.error("END_TRAJECTORY")
else:
self.end_moving_timer -= 1
else:
self.end_moving_timer -= 1
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ STEADY ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ STEADY: enter
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()
# #➤ ➤ ➤ ➤ ➤ STEADY: loop
# def on_loop_steady(self):
#
#⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ ERROR ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢
#
#➤ ➤ ➤ ➤ ➤ ERROR: enter
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)
#➤ ➤ ➤ ➤ ➤ ERROR: loop
def on_loop_error(self):
# send result multiple times (necessary? it seem that sometimes it's not received)
# self._sendResult(success=False,
# error=self.err,
# command=self.current_command)
if not self.in_allarm:
self.idle()