def record(self):
"""Record the real world position of the selected model point
"""
if self.selected_point is None:
RosProxy().notify("No calibration point selected", STATE.ERROR)
return
if len(self.poses) == 0:
RosProxy().notify("No calibration point added", STATE.ERROR)
return
if RobotControllerHandler.current_state is None:
RosProxy().notify("The current robot state is not available",
STATE.ERROR)
return
robot_pose = RobotControllerHandler.current_state.pose
# pos = robot_pose.position
# orientation = robot_pose.orientation
# axes = 'sxyz'
# (rx, ry, rz) = tf.transformations.euler_from_quaternion(np.array(
# [orientation.x, orientation.y, orientation.z, orientation.w]), axes)
self.poses[self.selected_point].measured = robot_pose
self.calibration_changed()
def __init__(self, task_type, progress, desc, title, step, current_bead,
path):
"""Init laser scanning task
Arguments:
ITask {super class} -- super class
task_type {string} -- type as string
progress {float} -- progress
desc {string} -- description
title {string} -- title
step {int} -- current step
current_bead {Bead} -- current bead with modifications
path {float[6][]} -- Pose array: x, y, z, r, p, y
"""
super(LaserScanTask, self).__init__(task_type, progress, desc, title,
step, current_bead, path)
self.laser_name = RosProxy().get_param("laser_name", "laser_scanner")
offset = RosProxy().get_param("laser_offset", [0.105, 0., 0.03])
self.offset = Point(offset[0], offset[1], offset[2])
rot = RosProxy().get_param("laser_rotation", [0, 0, 0])
self.rotation = np.array(rot)
self.scanning = False
RosProxy().subscribe_topic("/%s/is_scanning" % (self.laser_name), Bool,
self.handle_laser_update)
def __init__(self):
"""Init the RobotControllerHandler
"""
# Add the simulation and the robot controller
self._controllers = {
'simulation': {
'controller':
RobotController(RosProxy().get_param("simulation_controller",
"move_it_robot")),
'transformation':
None,
'lastState':
None
},
'robot': {
'controller':
RobotController(RosProxy().get_param("robot_controller",
"nachi_robot")),
'transformation':
None,
'lastState':
None
}
}
# Subscribe to the robot state changed event
dispatcher.connect(self._handle_robot_state_changed,
signal=Signals['STATE_CHANGED'],
sender=dispatcher.Any)
# Advertise the simulation set service to change between simulation and
# robot control
RosProxy().advertise_service("set_simulation", SetService,
self._set_simulation)
def pause(self):
"""Stop scanning if pause called
"""
RosProxy().call_service("/%s/stop" % (self.laser_name), Empty,
EmptyRequest())
super(LaserScanTask, self).pause()
RosProxy().notify("Scanning stopped")
def main():
"""Start ROSweld main program
Load the project and create the robot controllers
"""
Project().load()
RobotControllerHandler()
RosProxy().notify("ROSweld Core - ready")
try:
rospy.spin()
finally:
RosProxy().notify("ROSweld Core - stopped")
def play(self, data=None):
"""Play task according to the defined data property
sleect the move type (based on data['type'])
Arguments:
data {dict} -- move parameter dict
"""
moves = []
ctrl = RobotControllerHandler()
ctrl.current_controller.set_speed(
1 if self.speed is None else self.speed)
if data is None or "type" not in data or data["type"] == "verify":
moves = self.get_robot_path()
elif data["type"] == "travel":
moves = self.get_robot_path()
ctrl.current_controller.store_poses(moves)
ctrl.current_controller.move_pose(moves[0:1:1])
return
elif data["type"] == "store":
moves = self.get_robot_path()
ctrl.current_controller.store_poses(moves)
return
elif data['type'] == "continue":
poses_count = 0 if ctrl.current_state is None else ctrl.current_state.storedPoses
ctrl.current_controller.move_between(self.step, poses_count - 1)
return
else:
RosProxy().notify("Unkown move type", STATE.ERROR)
ctrl.current_controller.move_along(moves)
def set_speed(self, speed):
"""Set the speed on the robot
"""
msg = SetSpeedRequest()
msg.value = int(speed)
RosProxy().call_service("/%s/set_speed" % (self.name), SetSpeed, msg)
def pause(self):
"""Weld task pause
Before stopping the movement, the weld task will stop the arc as well
"""
RosProxy().call_service("/welding_driver/arc_stop", Empty, EmptyRequest())
super(WeldTask, self).pause()
def update(self):
"""Request update from the connected controllers
"""
for ctrl in self._controllers:
controller = self._controllers[ctrl]
RosProxy().call_service(
"/%s/update" % (controller['controller'].name), Empty, None)
def calibration_changed(self):
"""Calibration changed event, publish the markers and the calibration data to ROS
"""
for marker in self.marker_array.markers:
marker.action = Marker.DELETE
for idx, pose in enumerate(self.poses):
marker = Marker()
marker.id = int(str(int(round(time.time() * 1000)) + idx)[6:])
marker.header.frame_id = Calibration.marker_workpiece_frame
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.scale.x = Calibration.marker_size
marker.scale.y = Calibration.marker_size
marker.scale.z = Calibration.marker_size
marker.color.a = 1.0
marker.color = ColorRGBA(a=1, r=1, g=0, b=0) if\
idx == self.selected_point else ColorRGBA(a=1, r=1, g=1, b=0)
marker.pose.orientation.w = 1.0
marker.pose.position = pose.model
self.marker_array.markers.append(marker)
RosProxy().publish_topic("calibration_markers",
MarkerArray,
self.marker_array,
latch=True)
# Keep only the added markers
self.marker_array.markers = [
marker for marker in self.marker_array.markers
if marker.action == Marker.ADD
]
calibration_msg = CalibrationMsg(
points=self.poses,
selected_point=-1
if self.selected_point is None else self.selected_point,
transformation=np.append([], self.transformation_matrix))
RosProxy().publish_topic("calibration_points",
CalibrationMsg,
calibration_msg,
latch=True)
RobotControllerHandler().set_transformation(self.transformation_matrix)
def store_poses(self, moves):
"""Send the poses to the robot
Arguments:
moves {rosweld_tools/Move[]} -- path poses and their data
"""
RosProxy().call_service("/%s/store_poses" % (self.name), MoveAlong,
moves)
def move_pose(self, moves):
"""Move along the robot on a given path
Arguments:
moves {rosweld_tools/Move[]} -- path poses and their data
"""
RosProxy().call_service("/%s/move_pose" % (self.name), MoveAlong,
moves)
def handle_robot_step_changed(self, step):
"""Handle the robot step change
The weld task is subscribed to the current step coming from the robot.
If there is welding parameter changes for the current step,
the task will request the WPS to change accordingly
"""
if self.path is None:
return
if self.scanning and len(
self.path.points) == step + 1 and step != self.step:
RosProxy().notify("Disabling scanning")
RosProxy().call_service("/%s/stop" % (self.laser_name), Empty,
EmptyRequest())
super(LaserScanTask, self).handle_robot_step_changed(step)
def set_current_task(self, idx):
"""Change the current task by idx
Arguments:
idx {[type]} -- [description]
"""
self.current_task_idx = idx
RosProxy().notify("Set current task id: %d" % (idx))
def input(self, data):
"""Receive input and act accordingly
Arguments:
data {dict} -- param and value pairs
Returns:
bool -- input handled or not
"""
p = data['param']
v = data['value']
if p == 'path':
self.path = Path(CamParser.convert(v), 0, None,
[Bead(None, {}, {})])
self.current_bead_index = 0
elif p == 'robot_update':
self.handle_robot_step_changed(v)
elif p == 'select_bead':
self.current_bead_index = int(v)
RobotControllerHandler().current_controller.store_poses(
self.get_robot_path())
self.move_to_step({'step': 0})
elif p == 'add_bead':
self.path.beads.append(Bead(None, {}, {}))
self.current_bead_index = len(self.path.beads) - 1
RobotControllerHandler().current_controller.store_poses(
self.get_robot_path())
self.move_to_step({'step': 0})
elif p == 'remove_bead':
del self.path.beads[self.current_bead_index]
elif p == 'step':
self.move_to_step(v)
elif p == 'modification':
self.handle_modification_change(v)
elif p == 'speed':
self.speed = float(v)
RobotControllerHandler().current_controller.set_speed(self.speed)
elif p == 'setup_bead':
self.current_bead.wps_job_number = None if "job_number" not in v or \
v['job_number'] is None else int(v['job_number'])
self.current_bead.offset.z = float(v['z'])
self.current_bead.offset.y = float(v['y'])
self.current_bead.angle = float(v['angle'])
RobotControllerHandler().current_controller.store_poses(
self.get_robot_path())
self.move_to_step({'step': self.step})
else:
RosProxy().notify("Invalid input")
return False
return True
def __setstate__(self, d):
"""Restore object from save file
Arguments:
d {dict} -- dictionary of the saved object
"""
self.__dict__ = d
RosProxy().subscribe_topic("/welding_driver/jobs",
WeldingJobs, self.handle_wps_update)
def remove(self):
"""Remove selected calibration point
"""
if self.selected_point is None:
RosProxy().notify("No calibration point selected", STATE.ERROR)
return
if len(self.poses) == 0:
RosProxy().notify("No calibration point added", STATE.ERROR)
return
self.poses.remove(self.poses[self.selected_point])
if len(self.poses) == 0:
self.selected_point = None
else:
self.selected_point = min(len(self.poses) - 1, self.selected_point)
self.calibration_changed()
def current_bead_index(self, value):
"""Setter for the current bead index
Arguments:
value {int} -- current bead index to set
"""
self._current_bead_index = min(max(value, 0), len(self.path.beads) - 1)
if self.current_bead_index != value:
RosProxy().notify("The bead index was invalid.", STATE.ERROR)
def _set_simulation(self, data):
"""Set simulation mode on or off
Arguments:
data {SetService} -- basic service type for the communication,
contains the command true / false to enable or disable simulation
"""
# enable / disable simulation
if data.command == "true":
RosProxy().notify("Enabling simulation")
RobotControllerHandler.simulation = True
else:
RosProxy().notify("Disabling simulation")
RobotControllerHandler.simulation = False
# publish the new state
RobotControllerHandler.current_state = self.current_controller.current_state
self.proxy_robot_state(RobotControllerHandler.current_state)
return SetServiceResponse()
def play(self, data=None):
"""Play task according to the defined data property
sleect the move type (based on data['type'])
Arguments:
data {dict} -- move parameter dict
"""
if data is not None and "type" in data and data["type"] == "ScanTask":
ctrl = RobotControllerHandler()
ctrl.current_controller.set_speed(
1 if self.speed is None else self.speed)
RosProxy().notify("Enabling scanning")
RosProxy().call_service("/%s/start" % (self.laser_name), Empty,
EmptyRequest())
moves = self.get_robot_path()
ctrl.current_controller.move_along(moves)
return
super(LaserScanTask, self).play(data)
def save(self):
"""Save calibration to a file
"""
try:
with open(self.savefile + ".rws", "w+") as f:
copy_reg.pickle(FunctionType, stub_pickler, stub_unpickler)
pickle.dump(self, f, pickle.HIGHEST_PROTOCOL)
except Exception, e:
RosProxy().notify("Saving %s failed: %s" % (self.savefile, str(e)),
STATE.ERROR)
def proxy_robot_state(self, state):
"""Forward the robot states to ROS
Arguments:
state {RobotState} -- the current robot state to forward
"""
if state is None:
return
# Forward the robot state as a general rosweld robot_state
state.isSimulation = RobotControllerHandler.simulation
RosProxy().publish_topic("robot_state", RobotState, state, latch=True)
def check_alive(self):
"""Check if the thread is still alive
"""
t = currentThread()
while not rospy.is_shutdown() and getattr(t, "do_run", True):
if self.last_update is None or \
(datetime.datetime.now() - self.last_update).seconds > 2:
self.subscriber = RosProxy().subscribe_topic(
"/%s/robot_state" % (self.name), RobotState,
self.set_current_state)
time.sleep(2)
def play(self, data=None):
"""Weld task play
Before starting the movement the weld task will set the wps job number and start the arc
Arguments:
data {dict} -- weld parameter dict
"""
if data is not None and "type" in data and data["type"] == "WeldTask":
# only start welding if the wps job number is defined, otherwise notify and not move
if self.current_bead.wps_job_number is not None:
req = SetJobNumberRequest()
req.value = self.current_bead.wps_job_number
# set job number
RosProxy().call_service(
"/welding_driver/set_job_number",
SetJobNumber,
req
)
# enable arc
RosProxy().call_service("/welding_driver/arc_start", SetJobNumber,
req, self.arc_set_complete)
# send welding parameters
RosProxy().call_service(
'/welding_driver/set_params',
SetWeldingParameters,
self.welding_parameters[0])
else:
RosProxy().notify("Missing welding job number", STATE.ERROR)
return
else:
super(WeldTask, self).play(data)
def move_between(self, start, end, handler=None):
"""Move on the path
Arguments:
start: start index on the path
end: end index on the path
handler: result handler
"""
msg = MoveBetweenRequest()
msg.start = start
msg.end = end
RosProxy().call_service("/%s/move_between" % (self.name), MoveBetween,
msg, handler)
def remove_modification(self, i, planned):
"""Remove a modification from the planned or runtime list
Arguments:
i {int} -- step index of the modification
planned {bool} -- is planned modification
"""
arr = self.planned_modifications if planned else self.runtime_modifications
if not i in arr:
RosProxy().notify("Invalid index", STATE.ERROR)
return
del arr[i]
def load(self):
"""Load the save file if possible
"""
try:
if not os.path.isfile(self.savefile + ".rws"):
self.load_default()
return
with open(self.savefile + ".rws", "rb") as f:
dump = pickle.load(f)
self.load_object(dump)
except Exception, e:
RosProxy().notify(
"Loading default %s failed: %s" % (self.savefile, str(e)),
STATE.ERROR)
def set_calibration(self, req):
"""Calibration setter service handler
Arguments:
req {rosweld_tools.srv.SetService} -- ROS Service object
Returns:
bool -- state of the service result
"""
cmd = req.command
data = json.loads(req.json)
if cmd == "add":
if "x" in data and "y" in data and "z" in data:
x, y, z = float(data["x"]), float(data["y"]), float(data["z"])
self.add(Point(x, y, z))
else:
return False
elif cmd == "edit":
if "x" in data and "y" in data and "z" in data:
x, y, z = float(data["x"]), float(data["y"]), float(data["z"])
self.edit(Point(x, y, z))
else:
return False
elif cmd == "goto":
self.goto(speed=float(data["speed"]) if "speed" in data else 1.0)
elif cmd == "save":
self.save()
elif cmd == "cancel":
self.load()
elif cmd == "safe_goto":
self.safe_goto(
speed=float(data["speed"]) if "speed" in data else 1.0,
distance=float(data["distance"])
if "distance" in data else SAFETY_DISTANCE)
elif cmd == "record":
self.record()
elif cmd == "remove":
self.remove()
elif cmd == "set_selected_point" and "index" in data:
self.set_selected_point(int(data["index"]))
else:
RosProxy().notify("Invalid calibration input", STATE.ERROR)
return False
return True
def speed(self, value):
"""Setter for speed
Arguments:
value {int} -- new speed
"""
self._speed = int(value)
if not hasattr(self, "job") or self.job is None:
return
data = SetWeldingParametersRequest()
data.params.speed = self._speed
data.params.job_number = self.job.job_number
RosProxy().call_service("/welding_driver/set_params", SetWeldingParameters, data)
def __init__(
self,
task_type,
progress,
desc,
title,
step,
current_bead_index,
speed=1.0,
path=None,
use_arc_sensor=False):
"""Weld task init
The difference from a move task is the handling of the weld parameters,
thus, the task handles a step-by-step welding parameter update on the WPS and starts/stops
the arc, when it is necessary
Arguments:
ITask {super class} -- super class
task_type {string} -- type as string
progress {float} -- progress
desc {string} -- description
title {string} -- title
step {int} -- current step
current_bead_index {int} -- current bead index
path {float[6][]} -- Pose array: x, y, z, r, p, y
use_arc_sensor {bool} -- use arc sensor or not (default: {False})
"""
super(
WeldTask,
self).__init__(
task_type,
progress,
desc,
title,
step,
current_bead_index,
speed,
path)
self.use_arc_sensor = use_arc_sensor
RosProxy().subscribe_topic("/welding_driver/jobs",
WeldingJobs, self.handle_wps_update)
self.jobs = None
