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 on_enter_steady(self):
self._sendFeedback(state="steady", status=self.feedback.STEADY)
Logger.log("State: STEADY")
self._sendResult(success=True, command=self.current_command)
self.trajectory_correct = False
self.current_command = None
self.idle()
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 shape(self, shape_name, time=rospy.Time(0)):
""" Applies a Shape to robot """
if self.robot_shapes.get(shape_name):
self.getController().applyShape(self.robot_shapes.get(shape_name), time)
else:
Logger.warning("No shape with name '{}' for robot '{}'".format(
shape_name, self.robot_name))
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 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 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 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 reset(self):
self.is_active = False
self.done = False
# self.velocity = 0
# self.axis = PyKDL.Vector()
thr = self._estimateDisturbance(20) * 1.3
self.setParameters({"damp_force_threshold": thr})
Logger.warning("damp_force_threshold={}".format(thr))
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=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 _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 on_enter_ready(self):
Logger.log("State: READY")
self._sendFeedback(state="ready")
self.start_moving_timer = self.init_start_moving_timer
# if self.robot.getController().isMoving(epsilon=self.vel_noise_mean * 1.05): # TODO start-moving timer disabled!!!!!!!!!!!!!
if len(self.trajectory_generator.current_trajectory) > 0:
self.moving()
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 stepForward(self):
""" Moves one step forward (1 cycle) the robot's state machine """
try:
self.sfm.loop()
if self.robotSFM.isTrajectoryAdmissible() and self.sfm.getModel(
).state == "moving":
self.robotSFM.applyTrajectory()
except Exception as e:
Logger.warning("Exception: {}".format(e))
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 on_enter_generatetrajectory(self):
Logger.log("State: GENERATETRAJECTORY")
self._sendFeedback(state="generatetrajectory",
command=self.current_command)
self.generate_timer = self.init_generate_timer
self.t_gen = -1
self.trajectoryThread = threading.Thread(
target=self._generateTrajectoryFromCommand)
self.trajectoryThread.start()
def start(self, robot_list, sensor_list, instruction_list, subtask_list):
Logger.log("\n\n ************* SFM start ***********")
self.managerSFM.loadInstructions(instruction_list)
for robot_name in robot_list:
self.managerSFM.addRobot(robot_name)
for sensor_name in sensor_list:
self.managerSFM.addSensor(sensor_name)
for subtask_name in subtask_list:
self.managerSFM.addSubTask(subtask_name)
self.sfm.getModel().next()
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 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 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 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.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 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 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 __init__(self, namespace):
self.namespace = namespace
self.action_client = actionlib.SimpleActionClient(
CommandProxyConfiguration.getProxyName(self.namespace),
TextCommandAction)
Logger.log(
"Command Proxy '{}' is waiting for server...".format(namespace))
self.action_client.wait_for_server()
Logger.log("Command Proxy '{}' ready!".format(namespace))
#####
self.feedback_callbacks = []
self.done_callbacks = []
self.active_callbacks = []
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, robot_name):
self.robot_name = robot_name
# CREATE STATE MACHINE
self.supervisorSFM = SFMachineRobotSupervisorDefinition(
self.robot_name)
self.sfm = machines.SFMachine(name="sfm_" + self.robot_name +
"_supervisor",
model=self.supervisorSFM)
# DEFINE SM STATES
self.sfm.addState("start")
self.sfm.addState("idle")
self.sfm.addState("controlling")
self.sfm.addState("trajectory")
self.sfm.addState("alarm")
self.sfm.addState("direct")
# DEFINE SM TRANSITIONS
# start ...
self.sfm.addTransition("idle", "start", "idle")
# idle ...
self.sfm.addTransition("idle", "idle", "idle")
self.sfm.addTransition("alarm", "idle", "alarm")
self.sfm.addTransition("direct", "idle", "direct")
self.sfm.addTransition("trajectory", "idle", "trajectory")
# controlling ...
self.sfm.addTransition("idle", "controlling", "idle")
self.sfm.addTransition("controlling", "controlling", "controlling")
self.sfm.addTransition("trajectory", "controlling", "trajectory")
self.sfm.addTransition("alarm", "controlling", "alarm")
# alarm ...
self.sfm.addTransition("idle", "alarm", "idle")
self.sfm.addTransition("alarm", "alarm", "alarm")
# direct ...
self.sfm.addTransition("idle", "direct", "idle")
self.sfm.addTransition("direct", "direct", "direct")
self.sfm.addTransition("controlling", "direct", "controlling")
self.sfm.addTransition("trajectory", "direct", "trajectory")
self.sfm.addTransition("alarm", "direct", "alarm")
# trajectory ...
self.sfm.addTransition("trajectory", "trajectory", "trajectory")
self.sfm.addTransition("idle", "trajectory", "idle")
self.sfm.addTransition("alarm", "trajectory", "alarm")
self.sfm.create()
self.sfm.set_state("start")
Logger.log("\n\n ************* SFM ready to start ***********")
def on_enter_checktrajectory(self):
Logger.log("State: CHECKTRAJECTORY")
self._sendFeedback(state="checktrajectory")
# print("{}".format(self.trajectory_generator.current_trajectory))
if not self.trajectory_generator.computationSuccess():
self.error("NO_TRAJECTORY")
else:
if self._isOnTarget():
Logger.log("Already on Target")
self._sendFeedback(state="checktrajectory",
text="Already on Target")
self.steady()
else:
self.trajectory_correct = True
self.ready()
