def __init__(self):
self.command_proxy_server_name = "grasshopper_grasp_supervisor"
self.command_server = CommandProxyServer(self.command_proxy_server_name)
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
self.message_proxy = SimpleMessageProxy()
self.message_proxy.register(self.command_callback)
def __init__(self, sensor_name):
self.sensor_name = sensor_name
self.message_proxy = SimpleMessageProxy()
self.message_proxy.register(self.command_callback)
self.last_msg = None
# robot outer command
self.command_server = CommandProxyServer("{}_supervisor".format(
self.sensor_name))
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
def __init__(self):
self.filtered_tf_map = {}
self.filtered_tf_slots = {}
self.current_tf_name = ""
self.default_sls = 100
self.command_server = CommandProxyServer("tf_filter_supervisor")
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
self.message_proxy = SimpleMessageProxy()
self.message_proxy.register(self.command_callback)
class ConnectionsScanProxyServer(object):
def __init__(self):
self.command_proxy_server_name = "grasshopper_grasp_supervisor"
self.command_server = CommandProxyServer(self.command_proxy_server_name)
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
self.message_proxy = SimpleMessageProxy()
self.message_proxy.register(self.command_callback)
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):
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 sendResponse(self, success):
if self.command_server_last_message:
print("\n" + self.command_proxy_server_name.replace("_supervisor", "")
+ ": " + Hg_open + "Task Ended!!!" + H_close + " \t Success: {}".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 __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, task_name):
self.task_name = task_name
self.active_command = None
self.instruction_index = 0
self.task_end = False
self.last_action_success = False
self.repete = True
self.keep_tray_trajectory = False
self.robot_name_list = []
self.robot_proxy_client_dict = {}
self.sensor_name_list = []
self.sensor_proxy_client_dict = {}
self.subtask_name_list = []
self.subtask_proxy_client_dict = {}
self.message_proxy = SimpleMessageProxy()
def __init__(self, namespace='common', sender_name=""):
self.message_proxy = SimpleMessageProxy(name=namespace)
self.sender_name = sender_name
class TfManagerStub(object):
DEFAULT_RECEIVER_NAME = "tf_manager"
def __init__(self, namespace='common', sender_name=""):
self.message_proxy = SimpleMessageProxy(name=namespace)
self.sender_name = sender_name
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 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 deleteAll(self):
message = SimpleMessage.messageFromDict({
'receiver': TfManagerStub.DEFAULT_RECEIVER_NAME,
'command': 'delete_all',
'sender': self.sender_name
})
self.message_proxy.send(message)
def startPublish(self):
message = SimpleMessage.messageFromDict({
'receiver': TfManagerStub.DEFAULT_RECEIVER_NAME,
'command': 'start_publish',
'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 update(self):
message = SimpleMessage.messageFromDict({
'receiver': TfManagerStub.DEFAULT_RECEIVER_NAME,
'command': 'update',
'sender': self.sender_name
})
self.message_proxy.send(message)
import math
import sys
import random
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("tf_manager")
node.setupParameter("hz", 30)
node.setupParameter("world_tf", "world")
node.setupParameter("target_tf", "target")
node.setupParameter("storage_prefix", "saved_")
node.setupParameter("max_save_iterations", 30)
node.setHz(node.getParameter("hz"))
#⬢⬢⬢⬢⬢➤ variables
message_proxy = SimpleMessageProxy()
tf_storage = TfStorage()
all_tfs = []
publishing = True
def command_cb(message):
global current_tf_name, publishing, all_tfs
if message.isValid():
receiver = message.getReceiver()
command = message.getCommand()
if receiver == node.getName():
print("New command received", message.toString())
#⬢⬢⬢⬢⬢➤ UPDATE
if command == 'update':
all_tfs = tf_storage.allTfs()
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 SensorManager(object):
def __init__(self, sensor_name):
self.sensor_name = sensor_name
self.message_proxy = SimpleMessageProxy()
self.message_proxy.register(self.command_callback)
self.last_msg = None
# robot outer command
self.command_server = CommandProxyServer("{}_supervisor".format(
self.sensor_name))
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
def update(self, msg):
self.last_msg = msg
def uploadResetPublisher(self, pub):
self.reset_publisher = pub
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):
try:
if msg.isValid():
if msg.getReceiver() == "{}_supervisor".format(
self.sensor_name):
command = msg.getCommand()
self.result_flg = True
if command == "reset":
self.reset_publisher.publish("")
time.sleep(0.5)
Logger.warning("Sensor '{}': Reset".format(
self.sensor_name))
elif command == "filteron":
Logger.warning("Sensor '{}': Filter ON".format(
self.sensor_name))
# TODO
elif command == "filteroff":
Logger.warning("Sensor '{}': Filter OFF".format(
self.sensor_name))
# TODO
elif command == "filterreset":
Logger.warning("Sensor '{}': Filter Reset".format(
self.sensor_name))
# TODO
else:
self.result_flg = False
Logger.error("INVALID input")
self._send_command_result(self.result_flg)
except Exception as e:
print(e)
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
class TfFilter(object):
def __init__(self):
self.filtered_tf_map = {}
self.filtered_tf_slots = {}
self.current_tf_name = ""
self.default_sls = 100
self.command_server = CommandProxyServer("tf_filter_supervisor")
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
self.message_proxy = SimpleMessageProxy()
self.message_proxy.register(self.command_callback)
def resetFiltering(self):
Logger.warning("FILTER RESET")
self.filtered_tf_map = {}
self.filtered_tf_slots = {}
# self._send_command_result(True)
def clearFiltering(self, name):
Logger.warning("FILTER CLEAR: {}".format(name))
self.filtered_tf_map[name] = []
self.filtered_tf_slots[name] = 0
# self._send_command_result(True)
def startFiltering(self, name, slot_size):
Logger.warning("FILTER START: {}".format(name))
self.current_tf_name = name
self.filtered_tf_slots[name] = slot_size
if name not in self.filtered_tf_map:
self.filtered_tf_map[name] = []
def stopFiltering(self):
Logger.warning("FILTER STOP")
self.current_tf_name = ''
# self._send_command_result(True)
def updateAndBroadcast(self, node, current_tf, size):
if self.current_tf_name != '':
if current_tf:
if self.filtered_tf_slots[self.current_tf_name] > 0:
self.filtered_tf_slots[self.current_tf_name] -= 1
print(self.current_tf_name,
self.filtered_tf_slots[self.current_tf_name])
if len(self.filtered_tf_map[self.current_tf_name]) < size:
self.filtered_tf_map[self.current_tf_name].append(
current_tf)
else:
Logger.warning(
"TF Buffer for '{}' is full! Clear it...".format(
current_tf_name))
else:
self._send_command_result(True)
p = np.array([0, 0, 0])
q = np.array([0, 0, 0, 0])
for tf_name, tf_list in self.filtered_tf_map.iteritems():
if len(tf_list) <= 0:
continue
for tf in tf_list:
newp = np.array([tf.p.x(), tf.p.y(), tf.p.z()])
qx, qy, qz, qw = tf.M.GetQuaternion()
newq = np.array([qx, qy, qz, qw])
p = p + newp
q = q + newq
p = p / float(len(tf_list))
q = q / float(len(tf_list))
q = q / np.linalg.norm(q)
filtered_tf = PyKDL.Frame()
filtered_tf.p = PyKDL.Vector(p[0], p[1], p[2])
filtered_tf.M = PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3])
node.broadcastTransform(filtered_tf, tf_name + "_filtered",
node.getParameter("world_tf"),
node.getCurrentTime())
# return filtered_tf
def _send_command_result(self, success):
if self.command_server_last_message:
print("\nTask Ended!!! \t Success: {}".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
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ CALLBACKS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
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):
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)