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 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 goToTf(tf_name, tool="camera"):
#⬢⬢⬢⬢⬢➤ Go To Tf Helper Function
global command_proxy_client, moving_flag
message = SimpleMessage(receiver="{}_supervisor".format(robot_name),
command="jumptotf")
message.setData("tf_name", tf_name)
message.setData("tool_name", tool)
# print("Sending", message.toString())
active_command = command_proxy_client.sendCommand(message.toString())
moving_flag = 1
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 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 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 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 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 _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 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 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 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)