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
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)
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 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 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)