def write_freq(fillChar=0): bfile = "/dev/mmcblk0" try: with open(bfile, "w") as f: f.write(str(fillChar) + "¥n") except IOError: rospy.logger("can't write to " + bfile)
def write_freq(hz=0): bfile = "/dev/rtbuzzer0" try: with open(bfile,"w") as f: f.write(str(hz) + "\n") except IOError: rospy.logger("can't write to " + bfile)
def _getHoughLines(self, pixelRes=None, angleRes=None, minNumIntersections=None, minLineLength=None, maxLineGap=None): """ Determines the edges of a given source image via Canny transform for edge detection and a probablistic Hough transform for line detection. Outputs the set of edges and lines. """ hough_pixelRes = self._hough_pixelRes if pixelRes == None else pixelRes hough_angleRes = self._hough_angleRes if angleRes == None else angleRes hough_minNumIntersections = self._hough_minNumIntersections if minNumIntersections == None else minNumIntersections hough_minLineLength = self._hough_minLineLength if minLineLength == None else minLineLength hough_maxLineGap = self._hough_maxLineGap if maxLineGap == None else maxLineGap # get lines from probabilistic hough xform lines = cv.HoughLinesP(self.dst, hough_pixelRes, hough_angleRes, hough_minNumIntersections, None, hough_minLineLength, hough_maxLineGap) try: self._hough_lines = [ Line(Point(line[0, 0], line[0, 1]), Point(line[0, 2], line[0, 3])) for line in lines ] except TypeError as e: rospy.logger("Failed to detect ANY hough lines") self._hough_lines = []
def init(): rospy.init_node("follow_me") ser.baudrate = 9600 ser.port='/dev/rfcomm0' rospy.Subscriber("/dist", String, handler) ser.timeout = 2 ser.open() rate = rospy.Rate(10) print "jkl;" if not(ser.isOpen()): rospy.logerr("Eroor: could not open bluetooth serial port") while not rospy.is_shutdown(): try: p("Front Distance") distance = eval(ser.readline()) if(distance<10): p("Stop") print distance," stop" else: p("Forward") print distance,"forward" except: if(ser.isOpen()):ser.close() if not(ser.isOpen()):ser.open() if not(ser.isOpen()): rospy.logger("could not open porsdt") rate.sleep() rospy.spin()
def main(): global pub, active_ rospy.init_node('go_to_point') pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom) srv = rospy.Service('go_to_point_switch', SetBool, go_to_point_switch) rate = rospy.Rate(20) while not rospy.is_shutdown(): if not active_: continue else: if state_ == 0: fix_heading(des_position_) elif state_ == 1: go_straight_ahead(des_position_) elif state_ == 2: done() else: rospy.logger('unknown state!') rate.sleep()
def _handle_task_running(self, command): rospy.loginfo("Handle task running: ") if (self._taskname == "uavTakeOff"): if (command.arm > 0 and command.altitude > 0): #Arming the vehicle try: self._armService(bool(command.arm)) except rospy.ServiceException, e: rospy.logerr("Service arming call failed: %s" % e) #Put the vehicle in offboard mode # We need to send few setpoint messages, then activate OFFBOARD mode, to take effect k = 0 while k < 10: self._sp_pub.publish(command.sp) self._rate.sleep() k = k + 1 try: self._flightModeService(custom_mode=command.mode) except rospy.ServiceException, e: rospy.logger( "service set_mode call failed: %s. Offboard Mode could not be set." % e) command.sp.position.z = command.altitude while (abs(self._local_pos.pose.pose.position.z - command.altitude) > 0.2): self._sp_pub.publish(command.sp) self._rate.sleep()
def init(): rospy.init_node("follow_me") ser.baudrate = 9600 ser.port = '/dev/rfcomm0' rospy.Subscriber("/dist", String, handler) ser.timeout = 2 ser.open() rate = rospy.Rate(10) print "jkl;" if not (ser.isOpen()): rospy.logerr("Eroor: could not open bluetooth serial port") while not rospy.is_shutdown(): try: p("Front Distance") distance = eval(ser.readline()) if (distance < 10): p("Stop") print distance, " stop" else: p("Forward") print distance, "forward" except: if (ser.isOpen()): ser.close() if not (ser.isOpen()): ser.open() if not (ser.isOpen()): rospy.logger("could not open porsdt") rate.sleep() rospy.spin()
def write_freq(hz=0): bfile = "/dev/rtbuzzer0" try: # call close() when it exists with block. so, dont need close() with open(bfile, "w") as f: f.write(str(hz) + "\n") except IOError: rospy.logger("cant write to " + bfile)
def get_freq(): f = rospy.get_param('lightsensors_freq', 10) try: if f <= 0.0: raise Exception() except: rospy.logger('value error: lightsensors_freq') sys.exit(1) return f
def mission_monitor(): try: while not rospy.is_shutdown(): files = os.listdir(path) for file in files: if file == "auto.mission": handle_mission(file) except IOError: rospy.logger("monitor error")
def set_power(self, onoff=False): en = "/dev/rtmotoren0" try: with open(en, 'w') as f: f.write("1\n" if onoff else "0\n") self.is_on = onoff return True except: rospy.logger("cannot write to " + en) return False
def home_encoder(self, request): if not self.home_encoder: rospy.logger("Not homed") return (False, "Not homed") if self.lim1high == True: return (home_encoders) if self.lim2low == True: return (home_encoders)
def home_encoder(self, request): if not self.home_encoder: rospy.logger("Not homed") return (False, "Not homed") if self.lim1high == True: self.right_current_accumulator=0 if self.lim2low == True: self.left_current_accumulator =0
def set_power(self, onoff=False): en = "/dev/rtmotoren0" try: with open(en, "w") as f: #open "/dev/rtmotoren0" in written mode f.write("1\n" if onoff else "0\n") # onoff=True -> 1, onoff=False ->0 self.is_on = onoff #record ot motor status return True except: rospy.logger(" cannot write to " +en) return False
def set_raw_freq(self,left_hz,right_hz): if not self.is_on: rospy.logger("not enpowerd") return try: with open("/dev/rtmotor_raw_l0",'w') as lf,\ open("/dev/rtmotor_raw_r0",'w') as rf: lf.write(str(int(rount(left_hz)))+"\n") rf.write(str(int(rount(right_hz)))+"\n") except: rospy.logger("cannot write to rtmotor_aw_*")
def set_raw_freq(self, left_hz, right_hz): if not self.is_on: rospy.logger("not enpowered") return try: with open("/dev/rtmotor_raw_l0",'w') as lf,\ open("/dev/rtmotor_raw_r0",'w') as rf: lf.write(str(int(round(left_hz)))+"\n") # round(): 指定した桁で四捨五入。指定がなければ整数になる rf.write(str(int(round(right_hz)))+"\n") except: rospy.logerr("cannot write to rtmotor_raw_*")
def get_freq(): f = rospy.get_param('lightsensors_freq', 10) #get_param(param_name): basic use #get_param(param_name): get the value from the parameter server #get_param(param_name,default_value) in case of the parameter doesn't exist try: if f <= 0.0: raise Exception() except: rospy.logger("value error: lightsensors_freq") sys.exit(1) return f
def callback_tm(self, message): if not self.is_on: rospy.logger("not enpowered") return False dev = "/dev/rtmotor0" try: with open(dev, 'w') as f: f.write( "%d %d %d \n" % (message.left_hz, message.right_hz, message.duration_ms)) except: rospy.logerr("cannot write to " + dev) return False return True
def get_rot(): listener = tf.TransformListener() rospy.sleep(0.2) try: ((x, y, z), rot) = listener.lookupTransform('odom', 'base_link', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') # transforms the orientation from quaternion to euler so we can use data (phi, psi, theta) = tf.transformations.euler_from_quaternion(rot) return x, y, theta
def tryRead(self, index, length): try: bytes_remaining = length result = bytearray() while bytes_remaining != 0: with self.read_lock: received = self.data[index] index = index + 1 if (index == self.data_len): index = self.data_len - 1 if len(received) != 0: result.extend(received) bytes_remaining -= len(received) return result, length # return bytes(result), length except Exception as e: rospy.logger("can data read failure : %s" % e)
def convertImgToCv2Both(self, data_left, data_right): try: img_left = self.br.imgmsg_to_cv2(data_left, 'rgb8') img_right = self.br.imgmsg_to_cv2(data_right, 'rgb8') except CvBridgeError, e: rospy.logger(e)
from pimouse_ros.msg import LightSensorValues #from (package name) import (data type) if __name__== '__main__': devfile = '/dev/rtlightsensor0' rospy.init_node('lightsensors') #node initialization pub = rospy.Publisher('lightsensors',LightSensorValues, queue_size=1) #Publisher(topic name, data type, queue size) rate = rospy.Rate(10)#interval of loop [Hz] while not rospy.is_shutdown():#judge for shutdown the program try: with open(devfile,'r') as f: data = f.readline().split() #data is split because the device send each data with "space" data = [ int(e) for e in data ] # input each value in data to d,then transform string to integer d = LightSensorValues() d.right_forward = data[0] d.right_side = data[1] d.left_side = data[2] d.left_forward = data[3] d.sum_all = sum(data) d.sum_forward = data[0]+data[3] pub.publish(d) except IOError: rospy.logger("cannot write to "+devfile) rate.sleep()
def __init__(self): # if the node shuts down do the shutdwon function rospy.on_shutdown(self.shutdown) # create a variable for the listener self.listener = tf.TransformListener() # define the publisher to publish commands self.p = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) rospy.sleep(1.0) # look up the odometry information try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') # transforms the orientation from quaternion to euler so we can use data (phi, psi, theta) = tf.transformations.euler_from_quaternion(rot) # define starting values x_start = x y_start = y distance = 0 while distance <= length: # move forward at the speed defined above self.p.publish(move) rospy.loginfo('moving forward') # get the latest odometry information try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') # calculate the distance traveled distance = np.sqrt(pow(x - x_start, 2) + pow(y - y_start, 2)) print distance # stop for 2 seconds to give the robot time to finish moving before turning. reduces error self.p.publish(stop) print 'Stopping' rospy.sleep(2) # define starting angle angle_start = theta while abs(abs(theta) - angle_start) <= turn_angle: # turn at the speed defined above self.p.publish(turn) rospy.loginfo('turning') try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') (phi, psi, theta) = tf.transformations.euler_from_quaternion(rot) print abs(abs(theta) - angle_start) self.p.publish(stop) print 'Stopping' rospy.sleep(2) x_start = x y_start = y distance = 0 while distance <= length: self.p.publish(move) rospy.loginfo('moving forward') try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') distance = np.sqrt(pow(x - x_start, 2) + pow(y - y_start, 2)) print distance self.p.publish(stop) print 'Stopping' rospy.sleep(2) angle_start = theta while abs(theta - angle_start) <= turn_angle: self.p.publish(turn) rospy.loginfo('turning') try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') (phi, psi, theta) = tf.transformations.euler_from_quaternion(rot) print abs(theta - angle_start) print angle_start print theta self.p.publish(stop) print 'Stopping' rospy.sleep(2) x_start = x y_start = y distance = 0 while distance <= length: self.p.publish(move) rospy.loginfo('moving forward') try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') distance = np.sqrt(pow(x - x_start, 2) + pow(y - y_start, 2)) print distance self.p.publish(stop) print 'Stopping' rospy.sleep(2) angle_start = abs(theta) while abs(abs(theta) - angle_start) <= turn_angle: self.p.publish(turn) rospy.loginfo('turning') try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') (phi, psi, theta) = tf.transformations.euler_from_quaternion(rot) print abs(theta - angle_start) print theta print angle_start self.p.publish(stop) print 'Stopping' rospy.sleep(2) x_start = x y_start = y distance = 0 while distance <= length: self.p.publish(move) rospy.loginfo('moving forward') try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') distance = np.sqrt(pow(x - x_start, 2) + pow(y - y_start, 2)) print distance self.p.publish(stop) print 'Stopping' rospy.sleep(2) angle_start = theta while abs(theta - angle_start) <= turn_angle: self.p.publish(turn) rospy.loginfo('turning') try: ((x, y, z), rot) = self.listener.lookupTransform('odom', 'base_footprint', now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logger('Didnt work') (phi, psi, theta) = tf.transformations.euler_from_quaternion(rot) print abs(theta - angle_start) self.p.publish(stop) print 'Stopping' rospy.sleep(2)
def _payload_msg_cb(self, msg): with self.payloads_lock: try: for p in msg.payloads: try: if p.name in self.payload_targets or p.name in self.link_markers: rospy.logerr("Payload name already in use %s", p.name) continue if p.name not in self.payloads: ros_id=self._ros_id self._ros_id += 1 payload=Payload(p, ros_id) self.payloads[p.name]=payload #self._update_payload_mesh(payload) else: payload = self.payloads[p.name] #ignore stale data if payload.payload_msg.header.stamp > p.header.stamp: continue payload.payload_msg=p #self._update_payload_mesh(payload) rospy.loginfo("Updated payload " + p.name ) except: traceback.print_exc() rospy.logger("Error processing payload " + p.name) for t in msg.payload_targets: try: if t.name in self.payloads or t.name in self.link_markers: rospy.logerr("Payload name already in use %s", t.name) continue if t.name not in self.payload_targets: self.payload_targets[t.name]=t else: payload_target = self.payload_targets[t.name] #ignore stale data if payload_target.header.stamp > t.header.stamp: continue self.payload_targets[t.name]=t rospy.loginfo("Updated payload target " + t.name) except: traceback.print_exc() rospy.logger("Error processing payload target " + t.name) for l in msg.link_markers: try: if l.header.frame_id in self.payloads or l.header.frame_id in self.payload_targets: rospy.logerr("Payload name already in use %s", t.name) continue if l.header.frame_id not in self.link_markers: self.link_markers[l.header.frame_id]=l else: link_marker = self.link_markers[l.header.frame_id] #ignore stale data if link_marker.header.stamp > l.header.stamp: continue self.link_markers[l.header.frame_id]=l rospy.loginfo("Updated link marker " + l.header.frame_id ) except: traceback.print_exc() rospy.logger("Error processing link_marker " + l.header.frame_id) delete_payloads=[] try: for d in msg.delete_payloads: if d in self.payloads: payload = self.payloads[d] del self.payloads[d] delete_payloads.append(payload) rospy.loginfo("Deleted payload " + d ) #self._delete_payload_mesh(payload) if d in self.payload_targets: del self.payload_targets[d] rospy.loginfo("Deleted payload target " + d ) if d in self.link_markers: del self.link_markers[d] rospy.loginfo("Deleted link marker " + d ) except: traceback.print_exc() rospy.logger("Error deleting payload " + d.name) try: self._publish_planning_scene(delete_payloads) except: traceback.print_exc() rospy.logger("Error publishing planning scene") try: self._publish_rviz_sim_cameras() except: traceback.print_exc() rospy.logger("Error publishing rviz markers") if (_use_tesseract): try: self._publish_tesseract_scene(delete_payloads) except: traceback.print_exc() rospy.logger("Error publishing tesseract scene") except: traceback.print_exc()
% e) command.sp.position.z = command.altitude while (abs(self._local_pos.pose.pose.position.z - command.altitude) > 0.2): self._sp_pub.publish(command.sp) self._rate.sleep() elif (self._taskname == "uavLand"): try: self._flightModeService(custom_mode=command.mode) except rospy.ServiceException, e: rospy.logger( "service set_mode call failed: %s. Offboard Mode could not be set." % e) rospy.loginfo("UAV Land and Disarm successful") elif (self._taskname == "markerTrack_xy"): mat = np.zeros((4, 4)) #print msg k = 0 while k < 20: try: mat = self._camera_pose.asMatrix(command.tag, self.tfmessage.header) output_matrix = np.matmul(mat, inv(self.ht)) quaternion_msg = quaternion.as_float_array( quaternion.from_rotation_matrix(output_matrix[0:4,
def get_image(self,img): try: self.image_org = self.bridge.imgmsg_to_cv2(img, "bgr8") except CvBridgeError as e: rospy.logger(e)
if __name__ == '__main__': devfile = '/dev/rtlightsensor0' rospy.init_node('lightsensorsSecond') pub = rospy.Publisher('LightSensors', LSV, queue_size=1) freq = get_freq() rate = rospy.Rate(freq) while not rospy.is_shutdown(): try: with open(devfile,'r') as f: data = f.readline().split() data = [ int(e) for e in data ] d = LSV() d.right_forward = data[0] d.right_side = data[1] d.left_side = data[2] d.left_forward = data[3] d.sum_all = sum(data) d.sum_forward = data[0] + data[3] pub.publish(d) except: rospy.logger("cannot open " + devfile) f = get_freq() if f != freq: freq = f rate = rospy.Rate(freq) rate.sleep()
assert (round(response.output_commandlist.commands[0].blending[0], 3) == 1.5) assert (round(response.output_commandlist.commands[0].blending[1], 3) == 0.2) assert (len( response.output_commandlist.commands[0].additional_parameters) == 2 ) assert ((response.output_commandlist.commands[0]. additional_parameters[0]) == 'item1') assert ((response.output_commandlist.commands[0]. additional_parameters[1]) == 'item2') assert (len( response.output_commandlist.commands[0].additional_values) == 2) assert ((response.output_commandlist.commands[0].additional_values[0] ) == 55) assert ((response.output_commandlist.commands[0].additional_values[1] ) == 98) assert (( response.output_commandlist.replace_previous_commands) == False) if __name__ == '__main__': rospy.init_node('dnb_tf_autotest') try: rostest.rosrun(PKG, NAME, UnitTest, sys.argv) except KeyboardInterrupt as e: rospy.logger(e) pass print("exiting")