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 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 sendNewMessage():
global counter
if counter >= len(msgs):
return
command = msgs[counter % len(msgs)]
counter += 1
message = SimpleMessage(receiver="comau_smart_six_supervisor",
command="gotoshape")
message.setData("shape_name", command)
print("Sending", command)
active_command = command_proxy_client.sendCommand(message.toString())
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 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 update(self):
message = SimpleMessage.messageFromDict({
'receiver': TfManagerStub.DEFAULT_RECEIVER_NAME,
'command': 'update',
'sender': self.sender_name
})
self.message_proxy.send(message)
def stopPublish(self):
message = SimpleMessage.messageFromDict({
'receiver': TfManagerStub.DEFAULT_RECEIVER_NAME,
'command': 'stop_publish',
'sender': self.sender_name
})
self.message_proxy.send(message)
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 delete(self, saving_name):
message = SimpleMessage.messageFromDict({
'receiver': TfManagerStub.DEFAULT_RECEIVER_NAME,
'command': 'delete',
'saving_name': saving_name,
'sender': self.sender_name
})
self.message_proxy.send(message)
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 save(self, tf_name, tf_parent, saving_name):
message = SimpleMessage.messageFromDict({
'receiver': TfManagerStub.DEFAULT_RECEIVER_NAME,
'command': 'save',
'tf_name': tf_name,
'tf_parent': tf_parent,
'saving_name': saving_name,
'sender': self.sender_name
})
self.message_proxy.send(message)
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)
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 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 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 _send_command(self, supervisor_id, command, data={}):
Logger.warning("Sending {} {} to {}".format(command, data,
supervisor_id))
message = SimpleMessage(receiver="{}_supervisor".format(supervisor_id),
command=command)
for k in data.keys():
message.setData(k, data[k])
if supervisor_id in self.sensor_name_list:
self.active_command = self.sensor_proxy_client_dict[
supervisor_id].sendCommand(message.toString())
self.action()
elif supervisor_id in self.robot_name_list:
self.active_command = self.robot_proxy_client_dict[
supervisor_id].sendCommand(message.toString())
self.action()
elif supervisor_id in self.subtask_name_list:
self.active_command = self.subtask_proxy_client_dict[
supervisor_id].sendCommand(message.toString())
self.action()
else:
self.alarm()
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 getSentMessage(self):
return SimpleMessage.messageFromString(self.string_message)
def _resetDirectCommander(self):
simple_message = SimpleMessage(command="_reset_",
receiver="{}_direct_commander".format(
self.robot_name))
self._sendCommandActionToController(simple_message)
def directReset(self):
simple_message = SimpleMessage(command="directreset",
receiver="{}_supervisor".format(
self.robot_name))
self.robot_message_proxy.send(simple_message)
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")