def handle_modification_change(self, mod):
"""Handle modification change input
Arguments:
mod {dict} -- modification
"""
current_mod = self.current_bead.get_modification(self.step, True)
if current_mod is None:
current_mod = Modification()
self.current_bead.add_modification(self.step, True, current_mod)
current_mod.offset.z = float(mod['z'])
current_mod.offset.y = float(mod['y'])
current_mod.angle = float(mod['angle'])
current_mod.delta_r = float(mod['delta_r'])
if current_mod.welding_parameters is None:
current_mod.welding_parameters = WeldingState()
current_mod.welding_parameters.amperage = float(mod['amperage'])
current_mod.welding_parameters.voltage = float(mod['voltage'])
current_mod.welding_parameters.filler_speed = float(
mod['filler_speed'])
if Modification.is_zero(current_mod):
self.current_bead.remove_modification(self.step, True)
RobotControllerHandler().current_controller.store_poses(
self.get_robot_path())
self.move_to_step({'step': self.step})
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 get_robot_path(self, offset=Point(0, 0, 0), rotation=np.zeros(3)):
"""Convert the current path to robot path
including the modifications (plan and runtime)
Arguments:
offset {Point} -- offset vector from the current path (default: {Point(0,0,0)})
rotation {np.array} -- additional rotation
Returns:
Move[] -- Robot moves
"""
path = Transformation.apply_path(
self.path.points,
RobotControllerHandler().current_transformation)
moves = []
for pose in \
self.current_bead.apply_modifications_to_path(path, self.path.angle, offset, rotation):
move = Move()
move.pose = pose
move.speed = self.speed
moves.append(move)
return moves
def move_to_step(self, data):
"""Set the current step in the move
Arguments:
data {dict} -- dictionary, containing additional params for the step change
"""
step = int(data['step'])
RobotControllerHandler().current_controller.move_between(
self.step, step)
def safe_goto(self, speed=1.0, distance=SAFETY_DISTANCE):
"""Safe goto to the selected position (the Z is increased with 2cm)
Keyword Arguments:
speed {float} -- move speed (default: {1.0})
distance {float} -- safety distance (default: {SAFETY_DISTANCE})
"""
moves = []
point = copy.deepcopy(
self.poses[self.selected_point].measured.position)
orientation = self.poses[self.selected_point].measured.orientation
point.z += distance
move = Move()
move.pose.position = point
move.pose.orientation = orientation
move.speed = speed
moves.append(move)
RobotControllerHandler().current_controller.set_speed(speed)
RobotControllerHandler().current_controller.move_pose(moves)
def test_handle_robot(self):
with mock.patch.object(RosProxy, 'advertise_service', return_value=None) as mock_adv_service, \
mock.patch.object(RosProxy, 'publish_topic', return_value=None) as mock_pub_topic, \
mock.patch.object(RosProxy, 'notify', return_value=None), \
mock.patch.object(RosProxy, 'subscribe_topic', return_value=None) as mock_sub_topic, \
mock.patch.object(RobotController, 'check_alive', return_value=None), \
mock.patch.object(RosProxy, '__init__', return_value=None):
irc = RobotControllerHandler()
irc._set_simulation(SetServiceRequest(command="true"))
sc = irc._controllers['simulation']['controller']
rc = irc._controllers['robot']['controller']
state1 = RobotState(step=1)
state2 = RobotState(step=2)
assert state1 != state2
sc.set_current_state(state1)
assert irc.current_state == state1
assert sc.current_state == state1
rc.set_current_state(state2)
assert rc.current_state == state2
assert irc.current_state == state1 #the global state is the same
irc._set_simulation(SetServiceRequest(command="false"))
assert irc.current_state == state2
def test_init(self):
with mock.patch.object(RosProxy, 'advertise_service', return_value=None) as mock_adv_service, \
mock.patch.object(RosProxy, 'publish_topic', return_value=None) as mock_pub_topic, \
mock.patch.object(RosProxy, 'notify', return_value=None), \
mock.patch.object(RosProxy, 'subscribe_topic', return_value=None) as mock_sub_topic, \
mock.patch.object(RobotController, 'check_alive', return_value=None), \
mock.patch.object(RosProxy, '__init__', return_value=None):
irc = RobotControllerHandler()
assert irc._controllers['simulation'] is not None
assert irc._controllers['robot'] is not None
def test_commands(self):
with mock.patch.object(RosProxy, 'advertise_service', return_value=None) as mock_adv_service, \
mock.patch.object(RosProxy, 'publish_topic', return_value=None) as mock_pub_topic, \
mock.patch.object(RosProxy, 'notify', return_value=None), \
mock.patch.object(RosProxy, 'call_service', return_value=None) as mock_call_service, \
mock.patch.object(RosProxy, 'subscribe_topic', return_value=None) as mock_sub_topic, \
mock.patch.object(RobotController, 'check_alive', return_value=None), \
mock.patch.object(RosProxy, '__init__', return_value=None):
moves = []
moves.append(Move())
moves.append(Move())
moves.append(Move())
irc = RobotControllerHandler()
irc.current_controller.move_along(moves)
call_count = 1
assert mock_call_service.call_count == call_count
assert "move_along" in mock_call_service.call_args[0][0]
assert mock_call_service.call_args[0][2] == moves
irc.current_controller.store_poses(moves)
call_count = call_count + 1
assert mock_call_service.call_count == call_count
assert "store_poses" in mock_call_service.call_args[0][0]
assert mock_call_service.call_args[0][2] == moves
irc.current_controller.move_pose(moves)
call_count = call_count + 1
assert mock_call_service.call_count == call_count
assert "move_pose" in mock_call_service.call_args[0][0]
assert mock_call_service.call_args[0][2] == moves
irc.current_controller.move_between(0, 0, None)
call_count = call_count + 1
assert mock_call_service.call_count == call_count
assert "move_between" in mock_call_service.call_args[0][0]
assert mock_call_service.call_args[0][2] == MoveBetweenRequest(
start=0, end=0)
irc.current_controller.abort()
call_count = call_count + 1
assert mock_call_service.call_count == call_count
assert "abort" in mock_call_service.call_args[0][0]
irc.current_controller.set_speed(1)
call_count = call_count + 1
assert mock_call_service.call_count == call_count
assert "set_speed" in mock_call_service.call_args[0][0]
assert mock_call_service.call_args[0][2] == SetSpeedRequest(
value=1)
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 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 test_controller_change(self):
with mock.patch.object(RosProxy, 'advertise_service', return_value=None) as mock_adv_service, \
mock.patch.object(RosProxy, 'publish_topic', return_value=None) as mock_pub_topic, \
mock.patch.object(RosProxy, 'notify', return_value=None), \
mock.patch.object(RosProxy, 'subscribe_topic', return_value=None) as mock_sub_topic, \
mock.patch.object(RobotController, 'check_alive', return_value=None), \
mock.patch.object(RosProxy, '__init__', return_value=None):
irc = RobotControllerHandler()
irc._set_simulation(SetServiceRequest(command="true"))
assert irc.current_controller == irc._controllers['simulation'][
'controller']
irc._set_simulation(SetServiceRequest(command="false"))
assert irc.current_controller == irc._controllers['robot'][
'controller']
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 pause(self):
"""Pause task
"""
RobotControllerHandler().current_controller.abort()
def __project_control(self, req): # pragma: no cover
"""Handle project control service calls
Arguments:
req {rosweld_tools.srv.SetService} -- request
Returns:
bool -- the request is handled or not
"""
cmd = req.command
if cmd == "play":
data = json.loads(req.json)
self.plan.play(data)
elif cmd == "publish":
self.publish_plan()
return True
elif cmd == "pause":
self.plan.pause()
elif cmd == "save":
self.save()
elif cmd == "cancel":
self.load()
elif cmd == "input":
data = json.loads(req.json)
self.plan.input(data)
elif cmd == "update":
RosProxy().publish_last()
RobotControllerHandler().update()
elif cmd == "plan":
data = json.loads(req.json)
action = data["action"]
if action == "select":
self.plan.set_current_task(int(data["index"]))
elif action == "add":
cont_id = data["container_id"]
container = self.plan.get_task(cont_id)
if container is not None:
task_type = data["type"]
if task_type == "move":
container.tasks.append(
MoveTask(
task_type,
0,
task_type,
task_type,
0,
1.0))
if task_type == "weld":
container.tasks.append(
WeldTask(
task_type,
0,
task_type,
task_type,
0,
1.0,
None))
if task_type == "scan":
container.tasks.append(LaserScanTask(
task_type, 0, task_type, task_type, 0, 1.0, None))
elif action == "delete":
container = self.plan.get_task(data["container_id"])
task = container.get_task(data["task_id"])
container.tasks.remove(task)
else:
RosProxy().notify("Invalid input", STATE.ERROR)
return False
self.publish_plan()
return True