def close(self): """Close the connection and stop reading """ self.stop() status(self.name, "Laser scanner - stopped")
def move_between(req): """Handler for move between service request Moves along the robot between two poses Arguments: req {MoveBetweenRequest} -- request object, containing the start and stop index Returns: MoveBetweengResponse -- response object, empty """ resp = rosweld_drivers.srv.MoveBetweenResponse() print req, len(allPositions) if req.start < 0 or req.start >= len( allPositions) or req.end < 0 or req.end >= len(allPositions): resp.result = "Invalid move" global name status( name, "Invalid move, the selected steps has to be between 0 and %d" % (len(allPositions)), STATE.ERROR) return resp resp.result = "OK" # Add +1 to the indexes, because NACHI's list is indexed from 1, not zero sendPlay(req.start + 1, req.end + 1, -1 if req.end < req.start else 1) return resp
def arc_start(data=None): """Start the arc on the WPS Keyword Arguments: data {EmptyRequest} -- empty (default: {None}) Returns: EmptyResponse -- empty """ global udp if str(data.value) not in jobs: status( name, "Can not start arc with job number %s: not found" % (data.value)) return SetJobNumberResponse() status(name, "Arc start called: %s with job" % (data.value)) # create message and start the arc msg = bytearray() msg.extend(pack('<i', Commands.arc_start)[::-1]) msg.extend(pack('<i', data.value)[::-1]) # append message to udp queue udp.appendToQueue(msg, "send_arc_start") return SetJobNumberResponse()
def set_job_number(data): """Set the current job number on the WPS Arguments: data {SetJobNumber} -- set job number request object Returns: EmptyResponse -- empty """ global udp if str(data.value) not in jobs: status(name, "Can not set job number %s: not found" % (data.value)) return SetJobNumberResponse() status(name, "Set job number: %s" % (data.value)) # create message and start the arc msg = bytearray() msg.extend(pack('<i', Commands.set_job_number)[::-1]) msg.extend(pack('<i', data.value)[::-1]) # append message to udp queue udp.appendToQueue(msg, "send_set_job_number") return SetJobNumberResponse()
def sendPlay(start=1, end=-1, d=1): """Moves the robot between two given poses Arguments: start {int32} -- start index end {int32} -- end index d {int32} -- direction 1: forward, -1: backward Keyword Arguments: poses {Move[]} -- set of poses, if None, use allPositions (default: {None}) """ global udp msg = bytearray() # add command - play msg.extend(pack('<i', Commands.playback)[::-1]) # start index msg.extend(pack('<i', start)[::-1]) # end index msg.extend(pack('<i', end)[::-1]) # direction msg.extend(pack('<i', d)[::-1]) udp['command'].appendToQueue(msg, "send_play") status( name, "Play sent from %d to %d with direction: %d" % (start, len(allPositions) if end == -1 else end, d))
def move_between(req): """Handler for move between service request Moves the robot between two poses Arguments: req {MoveBetweenRequest} -- request object, containing the start and stop index Returns: MoveBetweengResponse -- response object, empty """ resp = rosweld_drivers.srv.MoveBetweenResponse() print req, len(allPositions) if req.start < 0 or req.start >= len( allPositions) or req.end < 0 or req.end >= len(allPositions): resp.result = "Invalid move" global name status( name, "Invalid move, the selected steps has to be between 0 and %d" % (len(allPositions)), STATE.ERROR) return resp resp = "OK" d = -1 if req.start > req.end else 1 sendPlay(req.start, req.end, d) return resp
def wps_state_publisher(): """Publishes the wps state Runs in a different thread, polling and publishing the wps state every second """ global update_response global name update_response = False rate = rospy.Rate(20) # 5hz t = currentThread() timeout = 60 while not rospy.is_shutdown() and getattr(t, "do_run", True): try: if (not update_response): timeout -= 1 if timeout == 0: ws = WeldingState() ws.error = "No response from the WPS" t_current_params.publish(ws) else: timeout = 60 update_response = False thread = Thread(target=update_asynch) thread.start() except Exception as e: status(name, "State publish error %s" % (e), STATE.ERROR) rate.sleep() status(name, "WPS state publisher stopped")
def update_jobs(): """Download the config files from the WPS Parses the downladed parameters Returns: WeldingState[] -- welding jobs """ global ftp global jobs global file_name files = [] try: files = ftp.nlst() except ftplib.error_perm as resp: status(name, "Error updating job list: %s" % (resp), STATE.ERROR) return jobs for f in files: filename, file_extension = os.path.splitext(f) if filename == file_name: try: cfg = _download_config(f) nr = int(file_extension[1:]) jobs[str(nr)] = parse_job(cfg, nr) except Exception, e: status(name, "Error downloading %s: %s" % (f, str(e)), STATE.ERROR)
def send(self, sock, msg): """Send a message on a socket Arguments: sock {socket} -- socket to use msg {bytearray} -- message """ try: sock.send(msg) except Exception as e: status(self.name, "Socket send failed: %s" % (str(e)), STATE.ERROR)
def __init__(self, name="laser_scanner", resolution=1280, serial=None): """Init MEL M2D Driver Arguments: host {str} -- scanner ip port {int} -- scanner port Keyword Arguments: name {str} -- node name (default: {"laser_scanner"}) robot_name {str} -- name of the current robot (default: {""}) this parameters will define the RobotState object to use """ global driver self.matrix = None self.name = name self.scanning = ScanningMode.Disabled self.measuring = False self.resolution = resolution self.serial = serial self.scanning_started = None # init MEL laser scanner node rospy.init_node(name, anonymous=True) # create ROS topics, services and subscriptions rospy.Service('start', Empty, self.start) rospy.Service('stop', Empty, self.stop) self.reading_publisher_pcl2 = rospy.Publisher('laser_scan', PointCloud2, queue_size=100, latch=True) self.reading_publisher_pcl = rospy.Publisher('meassured_z', PointCloud, queue_size=100, latch=True) self.status_publisher = rospy.Publisher('is_scanning', Bool, queue_size=1, latch=True) # Publish stopped self.status_publisher.publish(Bool(data=False)) status(self.name, "Laser scanner - Ready (%s)" % (name)) driver = self # Try to connect to the interface self.connect(True) self.disconnect()
def set_config(data, outputfile_nr=None): """Set the given config on the WPS Arguments: data {WeldingState} -- data with the current configuration Keyword Arguments: outputfile_nr {int32} -- which wps config nr to use when storing the config file on the WPS (default: {None}) Returns: EmptyResponse -- empty """ global file_name #print data try: f_in = "{0}.{1}".format(file_name, str(data.params.job_number).zfill(3)) f_out = "{0}.{1}".format( file_name, str(data.params.job_number if outputfile_nr is None else outputfile_nr).zfill(3)) cfg = _download_config(f_in) if data.params.amperage != 0: cfg.set("AST_CONDITION", "A_CURRENT", str(data.params.amperage)) if data.params.mode != 0: cfg.set("AST_CONDITION", "A_CURRENT_OUTPUT", str(data.params.mode)) if data.params.filler_speed != 0: cfg.set("AST_CONDITION", "A_FILLER_CONTROL", "1") cfg.set("AST_CONDITION", "A_FILLER_SPEED", str(data.params.filler_speed)) if data.params.speed != 0: cfg.set("AST_CONDITION", "A_SPEED", str(data.params.speed)) f = open(f_out, 'w') cfg.write(f, space_around_delimiters=False) f.close() # Upload to FTP f = open(f_out, 'r') ftp.storlines('STOR {0}'.format(f_out), f) except Exception as e: global name status(name, str(e), STATE.ERROR) sys.exit(0) return SetWeldingParametersResponse()
def sendAbort(): """Aborts the current movement Stops the robot executing the current movement """ global udp msg = bytearray() # add command - abort msg.extend(pack('<i', Commands.abort)[::-1]) udp['command'].appendToQueue(msg, "send_abort") status(name, "Abort sent.")
def abort(req): """Aborts the current movement Stops the robot executing the current movement Arguments: req {EmptyRequest} -- Empty Returns: EmptyResponse -- Empty """ global name group.stop() status(name, "Abort sent.") return EmptyResponse()
def welding_config_publisher(): """Send welding config to ROS """ global jobs global current_job_idx global name global auto_update rate = rospy.Rate(1) # 1hz t = currentThread() while not rospy.is_shutdown() and getattr(t, "do_run", True): try: old_jobs = deepcopy(jobs) update_jobs() changed = False for key in jobs: if key not in old_jobs or jobs[key] != old_jobs[key]: changed = True if len(old_jobs) != len(jobs) or len( set(old_jobs.keys()) & set(jobs.keys())) != len(jobs) or changed: config = WeldingJobs() config.configurations = [] for job in jobs: config.configurations.append(jobs[job]) config.current_index = current_job_idx config.auto_update = auto_update t_config.publish(config) except Exception as e: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] status( name, "Config publish error: %s (%s: %d)" % (e, fname, exc_tb.tb_lineno), STATE.ERROR) sys.exit(0) rate.sleep() status(name, "WPS config publisher stopped")
def abort(req): """Aborts the current movement Stops the robot executing the current movement Arguments: req {EmptyRequest} -- Empty Returns: EmptyResponse -- Empty """ # sendAbort() global name status(name, "Abort is not currently supported.", STATE.ERROR) return EmptyResponse()
def __init__(self, host, port, name="laser_scanner"): """Init MEL M2D Driver Arguments: host {str} -- scanner ip port {int} -- scanner port Keyword Arguments: name {str} -- node name (default: {"laser_scanner"}) """ self.matrix = None self.name = name self.host = host self.port = port self.scanning = ScanningMode.Disabled self.measuring = False self.sock = None self.scanning_started = None self.update_thread = None # init MEL laser scanner node rospy.init_node(name, anonymous=True) # init the laser scanner self.init_scanner() # create ROS topics, services and subscriptions rospy.Service('start', Empty, self.start) rospy.Service('stop', Empty, self.stop) self.reading_publisher_pcl2 = rospy.Publisher('laser_scan', PointCloud2, queue_size=100, latch=True) self.reading_publisher_pcl = rospy.Publisher('meassured_z', PointCloud, queue_size=100, latch=True) self.status_publisher = rospy.Publisher('is_scanning', Bool, queue_size=1, latch=True) # Publish stopped self.status_publisher.publish(Bool(data=False)) status(self.name, "Laser scanner - Ready (%s)" % (name))
def get_robot_step(currentPos): """Publishes the current approximate step on the path Searches for the closest point on the given path and publishes it's index The algorithm searches in a circle, trying to find a point after the current one Arguments: currentPos {Position} -- current position x,y,z Returns: int32 -- current pose index in the path """ global lastStep global allPositions global min_d try: minD = 9999 minI = -1 inThreshold = False if len(allPositions) != 0: posCount = len(allPositions) for i in range(0, posCount): # Go in a circle, starting from the current position i = (lastStep + i) % posCount p = allPositions[i].position # The distance of the current robot pose and the checked pose d = abs(p.x - currentPos.x) + \ abs(p.y - currentPos.y) + abs(p.z - currentPos.z) # Marking a new best match if d < minD and d < min_d / 2: minD = d minI = i # Catches, if the allPositions is not yet defined except NameError as e: global name status(name, e, STATE.ERROR) lastStep = minI return minI
def set_speed(req): """Handler for the set speed service request Sets the global moveit speed Arguments: req {SetSpeedRequest} -- request object, containing the value Returns: SetSpeedResponse -- response objet, empty """ global speed global name speed = req.value status(name, "SetSpeed sent: %d." % (req.value)) return rosweld_drivers.srv.SetSpeedResponse()
def receive(self, sock, buff=4096): """Receive message from a socket Arguments: sock {socket} -- socket to use Keyword Arguments: buff {int} -- size of the buffer (default: {4096}) Returns: bytearray -- data """ try: return sock.recv(buff) except Exception as e: status(self.name, "Socket receive failed: %s" % (str(e)), STATE.ERROR)
def __init__(self, host, port, binary_mode=True, local_port=None, name=""): """UdpConnector init function """ # The ip address of the other party self.host = host # The TCP port of the other party self.port = port # The address of the other party self.addr = (self.host, self.port) # The first message identifier, for matching requests and responses self.seq = 1 # The message queue self.sock_queue = [] # True, if the queue consumer is processing a request self.queue_waiting = False # param to stop the consuming thread self.consume = True # param to set the encoding mode self.binary_mode = binary_mode # name of the parent self.name = "UDP %s:%d" % (name, port) # connect try: # Create the socket self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Setting the timeout to 3s self.sock.settimeout(3) # Setting the local port if local_port is not None: self.sock.bind(("0.0.0.0", local_port)) # Try to initiate the connection self.sock.connect(self.addr) except Exception as e: print e status(self.name, 'ERROR: Failed to create socket: %s' % (str(e)), STATE.ERROR) sys.exit() # Starts the network message queue consumer self.consumeThread = Thread(target=self.consumeQueue) self.startConsumeThread()
def update(self): """Update scanner data with triggering the scanner """ t = currentThread() status(self.name, "Continous scanning started") sock = None while not rospy.is_shutdown() and getattr(t, "do_run", True): if sock is None: sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.connect((self.host, self.port)) self.measure(sock) status(self.name, "Continous scanning finished") sock.close()
def sendSetSpeed(value): """Sends a set speed command to the robot Sets the global movement speed of the robot Arguments: value {int32} -- speed """ global udp msg = bytearray() # add command msg.extend(pack('<i', Commands.setSpeed)[::-1]) # add speed msg.extend(pack('<i', value)[::-1]) udp['command'].appendToQueue(msg, "send_set_speed") status(name, "SetSpeed sent: %d." % (value))
def sendSetSpeed(value): """Sends a set speed command to the robot Sets the global movement speed of the robot Arguments: value {int32} -- speed """ global udp global name msg = bytearray() # add command msg.extend(encode(Commands.setSpeed)) # add speed msg.extend(encode(value)) udp.appendToQueue(msg, "send_set_speed") status(name, "SetSpeed sent: %d." % (value), STATE.INFO)
def robot_state_publisher(): """Publishes the robot pose Runs in a different thread, polling and publishing the robot pose every second """ global update_response update_response = False global last_state global name last_state = None rate = rospy.Rate(5) # 5Hz t = currentThread() timeout = 10 while not rospy.is_shutdown() and getattr(t, "do_run", True): try: if not update_response: timeout -= 1 if timeout == 0: status(name, "No response from the robot", STATE.ERROR) timeout = 10 else: timeout = 10 update_response = False sendUpdate() except Exception as e: status(name, e, STATE.ERROR) rate.sleep() status(name, "Robot state publisher stopped")
def init(): """ Initalizes the robot controller Creates all variables required to control the robot. Moves the robot to a neutral position. """ global thread global name name = rospy.get_param("robot_name", "move_it_robot") rospy.init_node(name, anonymous=True) try: rospy.wait_for_service('move_group/load_map', 1) except rospy.exceptions.ROSException: status(name, "Please start MoveIt first", STATE.ERROR) rospy.wait_for_service('move_group/load_map') moveit_commander.roscpp_initialize(sys.argv) moveit_commander.RobotCommander() moveit_commander.PlanningSceneInterface() global group group_name = rospy.get_param("move_group_name", "welding") group = moveit_commander.MoveGroupCommander(group_name) # # Registering services rospy.Service('move_along', rosweld_drivers.srv.MoveAlong, move_along) rospy.Service('abort', Empty, abort) rospy.Service('update', Empty, update) rospy.Service('store_poses', rosweld_drivers.srv.MoveAlong, store_poses) rospy.Service('move_pose', rosweld_drivers.srv.MoveAlong, move_pose) rospy.Service('set_speed', rosweld_drivers.srv.SetSpeed, set_speed) rospy.Service('move_between', rosweld_drivers.srv.MoveBetween, move_between) # Registering publishers global p_pose p_pose = rospy.Publisher('robot_state', RobotState, queue_size=100, latch=True) status(name, "MoveIt Robot Driver - ready (%s)" % (name)) # Starts the robot pose publisher on a new thread thread = Thread(target=robot_pose_publisher) thread.do_run = True thread.start() try: # Wait for service calls or user interruption rospy.spin() finally: thread.do_run = False thread.join() status(name, "MoveIt Robot Driver - stopped")
def __init__(self, url, topic, user, password): try: status("IPCamera", "Trying to connect to the camera: %s" % (url)) # setup the stream and authenticate with the username and password self.stream = requests.get( url, auth=(user, password), stream=True).raw status("IPCamera", "IPCamera - connected.") except BaseException: status("IPCamera", "Error: %s" % (sys.exc_info()[1]), STATE.ERROR) sys.exit() self.bytes = '' # publish Image topic with the selected topic name self.image_pub = rospy.Publisher(topic, Image, queue_size=1) self.bridge = CvBridge()
def robot_state_publisher(): """Publishes the robot pose Runs in a different thread, polling and publishing the robot pose every second """ global name global update_port # Create the socket sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Setting the timeout to 3s sock.settimeout(3) # Setting the local port sock.bind(("0.0.0.0", update_port)) global update_response update_response = False global last_state last_state = None t = currentThread() timeout = 10 while not rospy.is_shutdown() and getattr(t, "do_run", True): try: if not update_response: timeout -= 1 if timeout == 0: status(name, "No response from the robot", STATE.ERROR) timeout = 10 else: timeout = 10 update_response = False # Wait for an icoming message handleUpdateResponse(sock.recv(1024)) except Exception as e: status(name, e, STATE.ERROR) status(name, "Robot state publisher stopped", STATE.INFO)
def sendPlay(start, end, d, poses=None): """Moves the robot between two given poses Arguments: start {int32} -- start index end {int32} -- end index d {int32} -- direction 1: forward, -1: backward Keyword Arguments: poses {Move[]} -- set of poses, if None, use allPositions (default: {None}) """ # The last step along the multiPath global lastStep global allPositions global speed global name if poses is None: poses = allPositions lastStep = -1 # Create plan plan = moveit_msgs.msg.RobotTrajectory() # Used to append plans with different speeds currentMove = 0 # Going through all moves waypoints = [] for currentMove in range(min(start, end), max(start, end) + 1)[::d]: p = poses[currentMove] waypoints.append(p) # Set planning start to the last waypoint - in case it exists group.set_start_state_to_current_state() # We want the Cartesian path to be interpolated at a resolution of 1 cm # which is why we will specify 0.01 as the eef_step in Cartesian # translation. We will disable the jump threshold by setting it to 0.0 # disabling: (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold if fraction < sys.float_info.epsilon: status(name, "Can't follow path (fraction: %d)" % (fraction), STATE.ERROR) return # Divide the timing using the given speed for this plan for p in plan.joint_trajectory.points: p.time_from_start /= speed # Going through the multiPlan sync group.execute(plan, wait=False) status( name, "Play sent from %d to %d with direction: %d" % (start, len(allPositions) if end == -1 else end, d))
def consumeQueue(self): """Consumes a network message from the queue If there are messages in the queue, it takes one, and sends the message If the response function pointer is defined, it also waits for the response Runs in a new thread, to avoid blocks on the main thread """ t = currentThread() while getattr(t, "do_run", True): #print "consumeQueue", len(sock_queue) # If there is no message in the queue, waits for 10ms if len(self.sock_queue) == 0: time.sleep(0.01) continue item = self.sock_queue[0] #rospy.loginfo("Sending item %s %d..."%(item.desc, item.seq)) try: # Send the message item.tryStamps.append(datetime.now().microsecond) #if item.desc != "send_update": # print "sending: ", item.desc #rospy.loginfo("Sent: %s" % item.msg) self.send(item.msg) r = self.receive(1024) # If we are expecting a response if item.handler is not None: #rospy.loginfo("answer received") # read the seq in the response seq_back = None if self.binary_mode: #rospy.loginfo("Sent: %s %s" % (binascii.hexlify(item.msg), len(item.msg))) #rospy.loginfo("Received: %s %s" % (binascii.hexlify(r), len(r))) seq_back = unpack('>i', r[4:8])[0] else: #rospy.loginfo("Received: %s" % r[:-1]) seq_back = int(r[:-1].split(" ")[1]) # if the seq is not correct, we need to discard the # response if seq_back != item.seq: status( self.name, "Error: invalid seq. The response is discarded. %d != %d" % (seq_back, item.seq), STATE.ERROR) # receive any additional invalid messages self.receive(1024) self.receive(1024) self.receive(1024) continue # call the handler function pointer item.answered = datetime.now().microsecond if item.desc != "send_update": status( self.name, "Handled: %s, time: %dms, tries: %d" % (item.desc, abs(item.answered - item.requested) / 1000, len(item.tryStamps)), STATE.INFO) item.handler(r) #print "handled", # remove the first msg from the queue self.sock_queue.pop(0) #print "done" except socket.timeout: status(self.name, "timeout: %d - %s" % (item.seq, item.desc), STATE.ERROR) except Exception as e: status(self.name, "%s UDP Error: %s" % (self.addr, str(e)), STATE.ERROR) status(self.name, "UDP socket closed: %s:%d" % (self.host, self.port))
def init(): """Initalizes the WPS controller Creates all variables required to control the wps. """ global udp global t_config global t_sp_params global t_current_params global host global port global thread_state_update global thread_config_update global name try: host = rospy.get_param("ip") if host is None: raise Exception("Please set the ip of the WPS: wps_ip") port = rospy.get_param("port", 8002) name = rospy.get_param("wps_name", "welding_driver") # init driver node rospy.init_node(name, anonymous=True) if (host is None or port is None): status( name, "Please set WPS ip and port through rosparam: wps_ip, wps_port", STATE.ERROR) sys.exit() # UDP handler udp = UdpConnector(host, port, name=name) # Connect to FTP with anonymus user ftp_connect() # define services and topics rospy.Service('edit_config', SetWeldingParameters, set_config) rospy.Service('set_job_number', SetJobNumber, set_job_number) rospy.Service('set_params', SetWeldingParameters, set_params) rospy.Service('arc_start', SetJobNumber, arc_start) rospy.Service('arc_stop', Empty, arc_stop) t_config = rospy.Publisher('jobs', WeldingJobs, queue_size=1, latch=True) t_sp_params = rospy.Publisher('current_set_points', WeldingState, queue_size=1, latch=True) t_current_params = rospy.Publisher('current_params', WeldingState, queue_size=1, latch=True) thread_state_update = Thread(target=wps_state_publisher) thread_state_update.do_run = True thread_state_update.start() thread_config_update = Thread(target=welding_config_publisher) thread_config_update.do_run = True thread_config_update.start() status(name, "OTC WPS - ready") # Wait for service calls or user interruption rospy.spin() except Exception as e: status(name, "OTC WPS Error: %s" % (str(e)), STATE.ERROR) finally: udp.stopConsumeThread() if thread_state_update is not None: thread_state_update.do_run = False thread_state_update.join() if thread_config_update is not None: thread_config_update.do_run = False thread_config_update.join() status("welding_driver", "OTC WPS - stopped")