def _sendResult(self, success=False, command="", error=0):
if self.current_command != None:
if self.result_ext_callback:
q = self.robot.getController().getQ()
pose = geometry_msgs.msg.Pose()
eef_frame = self.robot.getEEFrame()
pose.position.x = eef_frame.p.x()
pose.position.y = eef_frame.p.y()
pose.position.z = eef_frame.p.z()
eef_quaternion = eef_frame.M.GetQuaternion()
pose.orientation.x = eef_quaternion[0]
pose.orientation.y = eef_quaternion[1]
pose.orientation.z = eef_quaternion[2]
pose.orientation.w = eef_quaternion[3]
self.result.pose = pose
self.result.q.data = list(q)
self.result.success = success
self.result.error = error
self.result.executed_command = command
self.result.q_target_distance = self.dist_target
self.result_ext_callback(self.result)
succ = "\033[91m" + "\033[92m" * success + "\033[1m\033[4m{}\033[0m".format(
success)
print("{}: {}".format(self.current_command, succ))
else:
Logger.error("External callback for RESULT update NOT found!")
self.current_command = None
def 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 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)
def setParameters(self, params_dict=None, standard_index="default"):
''' Set the parameters stored by name in the dictionary "params_dict".'''
if params_dict is not None:
try:
self.gain = self._param("gain", params_dict)
# @@@ add here the other parameters...
except Exception as e:
Logger.error(e)
def done_callback(self, status, result):
""" Callback chiamata al completamento del goal """
if self.ext_callback:
self.ext_callback(msg=result, cb_type=RobotMotionClient.RESULT)
else:
Logger.error("External callback for RESULT update NOT found!")
self.done_flg = True
self.current_result = result
def feedback_callback(self, feedback):
""" Callback chiamata sui feedback """
if self.ext_callback:
self.ext_callback(msg=feedback, cb_type=RobotMotionClient.FEEDBACK)
else:
Logger.error("External callback for FEEDBACK update NOT found!")
self.current_feedback = feedback
self.progress = feedback.progress
def deleteByName(self, name, message_type):
query = self.searchByName(name, message_type, single=False)
if query:
for item in query:
try:
self.msg_store.delete(item.id)
except rospy.ServiceException, e:
Logger.error(e)
def setParameters(self, params_dict):
try:
self.dampedforward_controller.setParameters(self._param("dampedforward_parameters",
params_dict))
self.spring_controller.setParameters(self._param("spring_parameters",
params_dict))
except Exception as e:
Logger.error(e)
def _generateTrajectoryFromCommand(self):
self.initial_q = self.robot.getController().getQ()
self.trajectory_generator.clearTrajectory()
self.generate_trajectory_failure = False
steps = self.trajectory_time * self.node_frequency
if self.target:
self.trajectory_points = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_POINTS")
if self.trajectory_points is None:
self.trajectory_points = self.node_frequency * self.trajectory_time
try:
if self.interpreter.trajectory_specs == "linear_joints":
self.trajectory = self.trajectory_generator.generateTrajectoryFromShapes(
self.initial_q, self.target, self.trajectory_points, 1)
elif self.interpreter.trajectory_specs == "linear_frames_to_free_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFromFrames(
self.initial_q, self.robot.getEEFrame(), self.target,
self.node_frequency, self.trajectory_time)
elif self.interpreter.trajectory_specs == "free_frames_to_linear_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFreeFromFrames(
self.initial_q,
self.robot.getEEFrame(),
self.target,
steps=steps,
agumented_middle_joints=True,
middle_frames=10,
number_of_tries=1,
perturbate_middle_frames=False)
elif self.interpreter.trajectory_specs == "target_frame_to_target_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFreeFromFrames(
self.initial_q,
self.robot.getEEFrame(),
self.target,
steps=1,
agumented_middle_joints=False,
middle_frames=20,
number_of_tries=5,
perturbate_middle_frames=False)
else:
self.trajectory = []
except Exception as e:
Logger.error(e)
self.trajectory = []
else:
self.trajectory = []
# Trajectory target update
if len(self.trajectory) > 0:
self.target_q = self.trajectory_generator.getTargetQ()
else:
self.generate_trajectory_failure = True
self.target_q = self.initial_q
def _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 searchByName(self, name, message_type, single=True):
try:
return self.msg_store.query(message_type._type,
{"_meta.name": {
'$regex': name
}},
single=single)
except rospy.ServiceException, e:
Logger.error(e)
return None
def generateNewGoal(self, goal):
""" Genera un nuovo goal e lo invia al server"""
self.done_flg = False
if goal:
self.current_goal = RobotMotionGoal(robot_command=goal)
self.action_client.send_goal(self.current_goal,
done_cb=self.done_callback,
active_cb=self.active_callback,
feedback_cb=self.feedback_callback)
else:
Logger.error("Empty Goal")
def serachByCustomMeta(self, key, value, message_type):
try:
return self.msg_store.query(
message_type._type,
{"_meta.{}".format(key): {
'$regex': value
}},
single=False)
except rospy.ServiceException, e:
Logger.error(e)
return None
def setParameters(self, params_dict):
try:
self.translation_mag = self._param("translation_mag", params_dict)
self.rotation_mag = self._param("rotation_mag", params_dict)
self.thresholds = self._param("thresholds", params_dict)
self.forceSensor.setMagnitude(self.translation_mag,
self.rotation_mag)
self.forceSensor.setTransThreshold(self.thresholds)
except Exception as e:
Logger.error(e)
def setSource(self, source):
""" Set the target 'source' as string """
self.follow_msg.action = RobotFollow.ACTION_SETSOURCE
if source == self.SOURCE_TRAJECTORY:
self.follow_msg.target_source = RobotFollow.TARGET_FROM_TRAJECTORY_PLANNER
elif source == self.SOURCE_DIRECT:
self.follow_msg.target_source = RobotFollow.TARGET_FROM_EXTERNAL_FEEDBACK
else:
Logger.error("Invalid Source")
return
self._publishToFollow(self.follow_msg, sleep_time=0.1)
def setParameters(self, params_dict):
try:
self.damp_force_threshold = self._param("damp_force_threshold",
params_dict)
self.damp_magnitude = self._param("damp_magnitude",
params_dict)
self.initial_velocity = self._param("velocity",
params_dict)
print(self.initial_velocity)
except Exception as e:
Logger.error(e)
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 createRobotByName(name, tf_listener=None):
cfg = ROBOT_MARKET_LIBRARY.get(name)
if cfg != None:
robot = controllers.Robot(configuration=cfg,
tf_listener=tf_listener)
shapes = ROBOT_MARKET_SHAPE_LIBRARIES.get(name)
if shapes:
for s in shapes:
robot_shape = controllers.RobotShape(s["name"], s["q"])
robot.addRobotShape(robot_shape)
return robot
else:
Logger.error("No Robot with name '{}'".format(name))
return None
def __init__(self, service_name, service_manifest=IKService, timeout=5.0):
self.service_name = service_name
self.service_manifest = service_manifest
self.active = False
Logger.log("GeneralIKService: waiting for service " +
service_name + "...")
try:
rospy.wait_for_service(self.service_name, timeout=timeout)
except rospy.ROSException as e:
Logger.error("Service timeout:" + str(e))
return
self.service = rospy.ServiceProxy(
self.service_name, service_manifest)
self.active = True
Logger.log("Service Connected!")
def _sendFeedback(self,
text="",
state="",
command="",
status=0,
progress=-1.0,
warning=0):
if self.feedback_ext_callback:
self.feedback.current_state = state
self.feedback.text = text
self.feedback.executing_command = command
self.feedback.status = status
self.feedback.progress = progress
self.feedback.warning = warning
self.feedback_ext_callback(self.feedback)
else:
Logger.error("External callback for FEEDBACK update NOT found!")
def start(self, robot_command):
self.robotSFM.uploadCurrentCommand(robot_command)
if self.sfm.getModel().state != "error":
try:
self.sfm.getModel().sendcommand()
Logger.log("\n\n ************* SFM start ***********")
except:
self.sfm.getModel().error("transition_error")
else:
error_code = self.sfm.getModel().err
self.sfm.getModel()._sendResult(
success=False,
error=error_code,
command=self.sfm.getModel().current_command)
Logger.error(
"\n\n ************* persist error {} *********** \n\n".format(
error_code))
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()
return
#⬢⬢⬢⬢⬢➤ START PUBLISH
if command == 'start_publish':
publishing = True
return
#⬢⬢⬢⬢⬢➤ STOP PUBLISH
if command == 'stop_publish':
publishing = False
return
#⬢⬢⬢⬢⬢➤ DELETE_ALL
if command == 'delete_all':
tf_storage.deleteAll()
all_tfs = []
return
#⬢⬢⬢⬢⬢➤ DELETE
if command == 'delete':
saving_name = message.getData("saving_name")
tf_storage.removeFrameByName(saving_name)
all_tfs = tf_storage.allTfs()
return
#⬢⬢⬢⬢⬢➤ SAVE
if command == 'save':
saving_name = message.getData("saving_name")
tf_name = message.getData("tf_name")
tf_parent = message.getData("tf_parent")
iterations = node.getParameter("max_save_iterations")
if tf_name == None:
Logger.error("'tf_name' must be not None")
return
if tf_parent == None:
Logger.error("'tf_parent' must be not None")
return
if saving_name == None:
Logger.error("'saving_name' must be not None")
return
while iterations > 0:
frame = node.retrieveTransform(tf_name, tf_parent, -1)
if frame:
tf_storage.saveFrame(frame, saving_name, tf_parent)
print("Saving ", tf_name, tf_parent)
return
Logger.error(
"Max iterations reached, retrieving '{}'".format(tf_name))
def simple_ik(self, target_frame, q_current, ik_type=0):
""" Calls a simple_ik service """
try:
p = Pose()
p.position.x = target_frame.p.x()
p.position.y = target_frame.p.y()
p.position.z = target_frame.p.z()
qu = target_frame.M.GetQuaternion()
p.orientation.x = qu[0]
p.orientation.y = qu[1]
p.orientation.z = qu[2]
p.orientation.w = qu[3]
q_in = Float64MultiArray()
q_in.data = q_current
response = self.service(p, q_in, 0)
return response
except rospy.ServiceException, e:
Logger.error("GeneralIKService: Service call failed: % s" % e)
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 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 on_enter_error(self, error_type=""):
Logger.log("State: ERROR")
self._sendFeedback(state="error")
self.trajectory_generator.clearTrajectory()
if error_type == "NO_COMMAND":
self.err = self.result.ERROR_NO_COMMAND
Logger.warning(
"Command string NOT recognized. Waiting for a new command")
elif error_type == "NO_TRAJECTORY":
self.err = self.result.ERROR_NO_TRAJECTORY
Logger.warning(
"Trajectory NOT computed. Waiting for a new command")
elif error_type == "START_MOVING":
self.err = self.result.ERROR_START_MOVING
Logger.warning(
"Moving Time Out. Target NOT Reached. Waiting for a new command"
)
elif error_type == "END_TRAJECTORY":
self.err = self.result.ERROR_END_TRAJECTORY
Logger.error(
"Trajectory Time Out. Target NOT Reached. Waiting for a new command"
)
elif error_type == "ROBOT_ALARM":
self.err = self.result.ERROR_ROBOT_ALARM
self.in_allarm = True
Logger.error(
"\n\n\n ROBOT ALARM!!! \n Restore the machine and Reset the Alarm \n\n\n"
)
elif error_type == "COMMAND_CONFLICT":
self.err = self.result.ERROR_COMMAND_CONFLICT
Logger.error(
"\n\n\n COMMAND CONFLICT!!! \n Waiting for a new command \n\n\n"
)
else:
self.err = self.result.ERROR_UNKNOWN
Logger.error("Unknown Error --- {}".format(error_type))
# send result
self._sendResult(success=False,
error=self.err,
command=self.current_command)
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_callback(self, alarm_info):
if alarm_info != self.alarm_proxy.NONE_ALARM:
Logger.error(" !!!!!!!! ALARM !!!!!!!! ")
self.alarm()
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()
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