def init_ros(): ''' ROS node initialization. Initialize the node, advertise topics, and any other ROS housekeeping. ''' global topics, param_handler rospy.init_node('sickldmrs', log_level=rospy.DEBUG) node_name = rospy.get_name().split('/')[-1] rospy.loginfo('node %s starting up.' % node_name) topics['cloud'] = rospy.Publisher('/%s/cloud' % node_name, numpy_msg(PointCloud2)) topics['scan0'] = rospy.Publisher('/%s/scan0' % node_name, numpy_msg(LaserScan)) topics['scan1'] = rospy.Publisher('/%s/scan1' % node_name, numpy_msg(LaserScan)) topics['scan2'] = rospy.Publisher('/%s/scan2' % node_name, numpy_msg(LaserScan)) topics['scan3'] = rospy.Publisher('/%s/scan3' % node_name, numpy_msg(LaserScan)) rospy.on_shutdown(node_shutdown_hook)
def __init__(self, vel_output_topic_type, max_speed, ctrl_p, def_stiffness): #Variable that checks if we can start working self.configured = False #Will only start controlling after receiving at least one desired position self.received_des_pos_values = False self.def_stiffness = def_stiffness self.def_damping = 0.7 #Controller parameters self.ctrl_p = ctrl_p self.max_speed = max_speed self.num_dof = 0; self.init_work_variables() #Chose the appropriate topic type if (vel_output_topic_type == 'MultiJointVelocityImpedanceCommand'): self.vel_output_topic_type = MultiJointVelocityImpedanceCommand elif (vel_output_topic_type == 'MultiJointVelocityCommand'): self.vel_output_topic_type = MultiJointVelocityCommand else: rospy.logfatal('%s: Velocity output topic type not supported. Extend the JController class if you want to support output to a topic of type %s.' %(rospy.get_name(), vel_output_topic_type)) raise Exception('should_exit') self.vel_pub = rospy.Publisher("~vel_command_out", numpy_msg(self.vel_output_topic_type), queue_size=3, tcp_nodelay=True, latch=False) self.js_sub = rospy.Subscriber("~joint_states", numpy_msg(JointState), self.js_cb, queue_size=3, tcp_nodelay=True) self.js_des_pos = rospy.Subscriber("~pos_command_in", numpy_msg(Float32MultiArray), self.des_pos_cb, queue_size=3, tcp_nodelay=True)
def main(self): root = Tkinter.Tk() self.root = root root.wm_title("Bootstrapping servo interface") xAchse = pylab.arange(0, 100, 1) yAchse = pylab.array([0] * 100) fig = pylab.figure(1) ax = fig.add_subplot(111) self.ax = ax ax.grid(True) ax.set_title("") ax.set_xlabel("sensel") ax.set_ylabel("") ax.axis([0, 100, -1.5, 1.5]) self.line_y = ax.plot(xAchse, yAchse, 'b.') self.line_y_goal = ax.plot(xAchse, yAchse, 'r.') canvas = FigureCanvasTkAgg(fig, master=root) canvas.show() canvas.get_tk_widget().pack(side=Tkinter.TOP, fill=Tkinter.BOTH, expand=1) toolbar = NavigationToolbar2TkAgg(canvas, root) toolbar.update() canvas._tkcanvas.pack(side=Tkinter.TOP, fill=Tkinter.BOTH, expand=1) self.canvas = canvas self.button = Tkinter.Button(master=root, text='Quit', command=self._quit) self.button.pack(side=Tkinter.BOTTOM) self.wScale = Tkinter.Scale(master=root, label="View Width:", from_=3, to=1000, sliderlength=30, length=ax.get_frame().get_window_extent().width, orient=Tkinter.HORIZONTAL) self.wScale2 = Tkinter.Scale(master=root, label="Generation Speed:", from_=1, to=200, sliderlength=30, length=ax.get_frame().get_window_extent().width, orient=Tkinter.HORIZONTAL) self.wScale2.pack(side=Tkinter.BOTTOM) self.wScale.pack(side=Tkinter.BOTTOM) self.wScale.set(100) self.wScale2.set(self.wScale2['to'] - 10) self.root.protocol("WM_DELETE_WINDOW", self._quit) # thanks aurelienvlg self.root.after(500, self.RealtimePloter) rospy.init_node('gui') rospy.Subscriber('/servo_manager/y', numpy_msg(Raw), self.callback_arrived, callback_args='y') rospy.Subscriber('/servo_manager/y_goal', numpy_msg(Raw), self.callback_arrived, callback_args='y_goal') rospy.Subscriber('/servo_demo/y', numpy_msg(Raw), self.callback_arrived, callback_args='y') rospy.Subscriber('/servo_demo/y_goal', numpy_msg(Raw), self.callback_arrived, callback_args='y_goal') # rospy.Subscriber('/servo_manager/u', numpy_msg(Raw), self.callback_arrived, callback_args='u') # self.data = {} Tkinter.mainloop()
def test_class_identity(self): from rospy.numpy_msg import numpy_msg self.assert_(isinstance(numpy_msg(Floats)(), numpy_msg(Floats))) FloatsNP = numpy_msg(Floats) FloatsNP2 = numpy_msg(Floats) self.assert_(FloatsNP is FloatsNP2)
def __init__(self, topics, params): """ Constructor @param topics: dictionary mapping topic names to publisher handles valid topic names are {"cloud", "scan0", "scan1", "scan2", "scan3"} @type topics: dict {topic_name:publisher_handle} @param params: dictionary mapping parameter names to values. Where applicable, the parameters must be in device units (e.g. ticks) @type params: dict {ros_parameter_string:value} """ self.params = params self.topics = topics # LaserScan messages (numbered 0-3, 0 is the lowest scan plane) # rewire for numpy serialization on publish self.scans = [numpy_msg(LaserScan)(), numpy_msg(LaserScan)(), numpy_msg(LaserScan)(), numpy_msg(LaserScan)()] self._init_scans() # PointCloud2 message # rewire for numpy serialization on publish self.point_cloud = numpy_msg(PointCloud2)() self._init_point_cloud() # put variables into the namespace to prevent # attribute exceptions when the class is abused self.header = None self.x = None self.y = None self.z = None self.echo = None self.layer = None self.flags = None self.echo_w = None self.h_angle = None self.rads_per_tick = None self.pc_data = None self.last_start_time = None self.rads_per_tick = None self.seq_num = -1 self.time_delta = rospy.Duration() self.n_points = 0 # timestamp smoothing self.smoothtime = None self.smoothtime_prev = None self.recv_time_prev = None self.known_delta_t = rospy.Duration(256.0/self.params['scan_frequency']) self.time_smoothing_factor = self.params['time_smoothing_factor'] self.time_error_threshold = self.params['time_error_threshold'] self.total_err = 0.0 #integrate errors self.header = {}.fromkeys(self.header_keys)
def __init__(self): self._coll_checked = False self._pub_joints = rospy.Publisher('joint_state_check', numpy_msg(Float32MultiArray), queue_size = 10, latch=True) self._sub = rospy.Subscriber('collision_check', Int16, self.sub_cb) # self._pub_path = rospy.Publisher('joint_path',numpy_msg(Float32MultiArray), queue_size = 10) self._pub_traj = rospy.Publisher('joint_traj',numpy_msg(Float32MultiArray), queue_size = 10) self._iter = 0 self._executing = True self._sub_demo_state = rospy.Subscriber('demo_state', Int16, self.sub_demo) self._sub_pos_state = rospy.Subscriber('pos_state', Int16, self.sub_pos) self._plot_demo = get_param('plot_demo', False) rospy.loginfo("Here's my demo state: ") rospy.loginfo(self._plot_demo) self._plotIndex = -1
def main(self): rospy.init_node('rviz_visualizer_node') self.r = rostopic.ROSTopicHz(-1) rospy.Subscriber('/reactive_cones', numpy_msg(Floats), self.r.callback_hz, callback_args='/global_map_markers') rospy.Subscriber('/reactive_cones', numpy_msg(Floats), self.callback_reactive) rospy.Subscriber('/global_map_markers', numpy_msg(Floats), self.callback_global) rospy.Subscriber('/odometry/filtered', Odometry, self.callback_odom) rospy.Timer(rospy.Duration(0.1), self.publish_FOV) rospy.Timer(rospy.Duration(0.1), self.lifetime_global) rospy.spin()
def ComBordersPublisher(refInf, refSup): pub = rospy.Publisher('sot_controller/com_borders', numpy_msg(Floats)) a = numpy.concatenate((numpy.array(refInf, dtype=numpy.float32), numpy.array(refSup, dtype=numpy.float32))) #r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): pub.publish(a)
def run_node(self): rospy.init_node('kb_admin', anonymous=False) rospy.loginfo('Back robot is up and running') rospy.Subscriber('config_robot_side_receiver', numpy_msg(StringArray), self.on_data_passing) self.publish() rospy.spin()
def __init__(self): print 'Scouting system for ROS topics and types...' print 'The more you decide to filter, less time the scouting will take.' self._scouter = scouter.Scouter() self._pub = rospy.Publisher('config_robot_side_sender', numpy_msg(StringArray), queue_size=10)
def talker(): cmd_publisher= rospy.Publisher('cmd_drive', cmd_drive,queue_size=10) data_pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10) rospy.Subscriber("/IMU", Odometry, ImuCallback) rospy.Subscriber("/GPS/fix",NavSatFix,retour_gps) rospy.init_node('LQR', anonymous=True) r = rospy.Rate(200) # 10hz simul_time = rospy.get_param('~simulation_time', '10') # == Dynamic Model ====================================================== # == Steady State ====================================================== # == LQR control (Gains) ====================================================== C=np.array([ [0, 1,0, 0], [0,0, 1, 0], [0,0,0, 1]]) R=np.array([ [10000,0], [0,10000]]) Q=np.array([ [100, 0,0, 0], [0,100, 0, 0], [0,0, 100, 0], [0,0,0, 100]])
def listener(): global y_offset global y_offset_list global net_name global CLASSES global N_CLASSES global N_BINS rospy.init_node('nms_detections', anonymous=True) net_name = rospy.get_param('~net', 'regular') text = '[NMS] Using %s' % (net_name) rospy.loginfo(text) y_offset = rospy.get_param('~y_offset', 0) y_offset_list = [0, y_offset, 0, y_offset] det_sub = rospy.Subscriber("cnn_detection", numpy_msg(Detection), callback) configfile = os.path.join(pkg_path, 'models', NETS[net_name][3]) cfg_from_file(configfile) CLASSES = cfg.CLASSES N_CLASSES = len(CLASSES) N_BINS = cfg.VIEWP_BINS rospy.spin()
def start(self): if not self._created: print "Starting subscription ..." rospy.Subscriber("SoundData_1", numpy_msg(Float64MultiArray), self.callback) print "Done." self._created = True
def image_callback(msg): pub = rospy.Publisher('2D_coordinates', numpy_msg(Floats),queue_size=10) # Convert your ROS Image message to OpenCV2 cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8") gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale( gray, scaleFactor=2, minNeighbors=5, minSize=(30, 30), flags=cv2.CASCADE_SCALE_IMAGE ) for (x, y, w, h) in faces: rect = cv2.rectangle(cv2_img, (x, y), (x+w, y+h), (0, 0, 255), 2) cv2.line(rect,(x+w/2,y),(x+w/2,y+h),(255,255,255),1) cv2.line(cv2_img,(320,0),(320,480),(255,255,255),3) a = numpy.array([x+w/2, y+h/2], dtype=numpy.float32) print('inter connecting..........') print(len(faces)) cv2.imshow("pic3", cv2_img) cv2.waitKey(3) pub.publish(a)
def __init__(self): print('starting [DRIVER] node') init_model() Driver.controls_pub = rospy.Publisher('controls', BDDMsg.BDDControlsMsg, queue_size=None) rospy.Subscriber('/zed/left/image_rect_color', numpy_msg(Image), self.left_callback, queue_size=5) rospy.Subscriber('/zed/right/image_rect_color', numpy_msg(Image), self.right_callback, queue_size=5)
def talker(self): pubMatrixB = rospy.Publisher('floatsBToMaster', numpy_msg(Floats), queue_size=10, latch=True) pubMatrixBRow = rospy.Publisher('rowBToMaster', Int16, queue_size=9, latch=True) pubMatrixBCol = rospy.Publisher('colBToMaster', Int16, queue_size=8, latch=True) print "Talker reached \n" rate = rospy.Rate(1) # 10hz connectionsReady = False while not connectionsReady: connectionMatrixB = pubMatrixB.get_num_connections() connectionMatrixBRow = pubMatrixBRow.get_num_connections() connectionMatrixBCol = pubMatrixBCol.get_num_connections() if connectionMatrixB > 0 and connectionMatrixBRow > 0 and connectionMatrixBCol > 0: pubMatrixB.publish(self.matrix.data) pubMatrixBRow.publish(self.matrixRow.data) pubMatrixBCol.publish(self.matrixCol.data) print "Messages to Master published. \n" connectionsReady = True else: rate.sleep()
def main(stdscr): global cmd rospy.init_node('keyboard_input', anonymous=True) pub = rospy.Publisher('kobuki/cmd_vel', numpy_msg(Twist)) rate = rospy.Rate(50) stdscr.nodelay(True) key="" stdscr.clear() stdscr.scrollok(1) stdscr.addstr("Menu:\n") stdscr.addstr("Forward: KEY_UP \n") stdscr.addstr("Backward: KEY_DOWN \n") stdscr.addstr("Left: KEY_LEFT \n") stdscr.addstr("Right: KEY_RIGHT \n") stdscr.addstr("Stop: SPACE BAR \n") stdscr.addstr("Quit: Q \n") while not rospy.is_shutdown(): try: key = str(stdscr.getkey()) processKey(stdscr, key) # print cmd if key == '\n': break except Exception as e: # No input pass pub.publish(cmd) rate.sleep()
def run_node(self): rospy.init_node('back_planner_side', anonymous=True) rospy.loginfo('Back planner is up and running') rospy.Subscriber('config_plan_side_receiver', numpy_msg(StringArray), self.on_data_passing) self.publish_data2edit() rospy.spin()
def listener(): rospy.init_node('listener_sensor1') rospy.Subscriber('wtsft_sensor1', numpy_msg(Floats), callback) #ani = animation.FuncAnimation(fig,callback,frames = 400,interval=10 ,blit =True) plt.show() rospy.spin()
def __init__(self): #a publisher for our line marker self.line_pub = rospy.Publisher(self.WALL_TOPIC, Marker, queue_size=1) #a subscriber to get the laserscan data rospy.Subscriber(self.SCAN_TOPIC, numpy_msg(LaserScan), self.laser_callback)
def __init__(self): rospy.init_node('findbox', anonymous=True) self.physical_robot = False if self.physical_robot: rospy.Subscriber("/scan/long_range",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1) else: rospy.Subscriber("/scan",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1) rospy.Subscriber('/odometry/filtered',Odometry, self.cb_odom) self.bearing_pub = rospy.Publisher("/detection",numpy_msg(Floats), queue_size=1) self.bridge = CvBridge() self.dist_min = 0.25 self.dist_max = 2.0 # 1m is real target, 41in for height self.ylen_lim = 2 self.ang_min = -1.57 self.ang_max = 1.57 self.R = None self.max_range = 60 self.arena_xpos = 100 #rospy.get_param('arena_xpos') self.arena_xneg = -100 #rospy.get_param('arena_xneg') self.arena_ypos = -100 #rospy.get_param('arena_ypos') self.arena_yneg = 100 #rospy.get_param('arena_yneg') self.rate = rospy.Rate(10) self.scan_dist_thresh = 15.0 # Distance threshold to split obj into 2 obj. self.plot_data = False self.image_output = rospy.Publisher("/output/keyevent_image",Image, queue_size=1)
def __init__(self, sink): self.text_pub = rospy.Publisher(sink, numpy_msg(Floats), queue_size=10, latch=True) self.callback()
def __init__(self): # self.image_pub = rospy.Publisher("image_topic_2",Image) self.CMT = CMT.CMT() self.bridge = CvBridge() self.first = True self.rect = None self.init_img = None self.center = None self.prev_center = None self.h = None self.w = None self.linear_speed_x = 0.1 self.k_yaw = 0.0005 self.k_alt = 0.0005 self.image_sub = rospy.Subscriber("/rexrov/rexrov/camera/camera_image", Image, self.callback) self.des_vel_pub = rospy.Publisher("/rexrov/cmd_vel", numpy_msg(geometry_msgs.Twist), queue_size=1)
def step(self, action): """ Parameters ---------- action : [change in x, change in y, change in z] Returns ------- ob, reward, episode_over, info : tuple ob (object) : either current position or an observation object, depending on the type of environment this is representing reward (float) : negative, squared, l2 distance between current position and goal position episode_over (bool) : Whether or not we have reached the goal info (dict) : For now, all this does is keep track of the total distance from goal. This is used for rlkit to get the final total distance after evaluation. See function get_diagnostics for more info. """ action = np.array(action, dtype=np.float32) # ROS specific here self.action_publisher.publish(action) self.current_pos = np.array( rospy.wait_for_message("/replab/action/observation", numpy_msg(Floats)).data) return self._generate_step_tuple()
def __init__(self): self.image_sub = rospy.Subscriber("/rexrov/rexrov/camera/camera_image", Image, self.image_callback) self.des_vel_pub = rospy.Publisher("/rexrov/cmd_vel", numpy_msg(geometry_msgs.Twist), queue_size=1) self.pose_sub = rospy.Subscriber("/rexrov/pose_gt", Odometry, self.pose_callback) self.model_state_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1) self.jerk_sub = rospy.Subscriber("/jerk", Float32, self.jerk_callback) self.position = np.array([0., 0., 0.]) self.image = None self.linear_speed = 0.3 self.bridge = CvBridge() self.reward = None self.terminal = None self.state = None self.hit = False self.state_dim = 80 * 80 self.action_dim = 2 self.action_bound = 0.5 self.now = time.time() rospy.init_node('sub_env', anonymous=True)
def __init__(self): rospy.init_node(NODE_NAME) sys.path.append( '~/Documents/RobotSemantics/semseg_ws/src/image_segnet/object_class_pcl/src' ) for name, default in PARAM_LIST: setattr(self, name, rospy.get_param(PARAM_PREFIX + name, default)) rospy.Subscriber(self.in_pcl, PointCloud2, self.pcl_cb) rospy.Subscriber(self.in_viz + '/' + self.camera_used + '/camera_info', CameraInfo, self.ci_cb) rospy.Subscriber( self.in_viz + '/' + self.camera_used + '/image/compressed', CompressedImage, self.image_cb) rospy.Subscriber(self.in_semseg + '/class_matrix', numpy_msg(Floats), self.clmat_cb) self.pcl_pub = rospy.Publisher(self.out_pcl, PointCloud2, queue_size=20) self.clmats = {} self.cis = {} self.imgsC = {} self.timestamps = [] self.counterTS = 0 self.counter = 0 #SHOULD BE CHANGED TO SOMETHING ADAPTIVE - NOW manually SETTED self.camera0PM = [ 638.81494, 0, 625.98561, 0, 0, 585.79797, 748.57858, 0, 0, 0, 1, 0 ] self.tf_listener = tf.TransformListener() self.adjustment = np.dot( tf_help.rotation_matrix(math.pi / 2, (0, 1, 0)), tf_help.rotation_matrix(math.pi, (0, 0, 1)))
def __init__(self): self.status = 'STOPPED' RG.setmode(RG.BCM) RG.setup(14, RG.OUT) self.AnglesDict = {} self.Delta = 1. self.angles_off, self.direction, self.angles_check, self.legs_motors_ids = parsser( '/home/pi/Robot_Control/angles.txt') rospy.init_node('angle_ctrl') self.publisher_status = rospy.Publisher('status', String, queue_size=1) self.publisher_legs = rospy.Publisher("/dxl/command_position", CommandPosition, queue_size=1) self.enabler = rospy.Publisher("/dxl/enable", Bool, queue_size=1) self.enabler.publish(True) self.subscriber_status = rospy.Subscriber('status', String, self.update_status) self.subscriber_status = rospy.Subscriber('led', Int8, self.SwitchLight) self.subscriber_delta = rospy.Subscriber('delta', Float32, self.UpdateDelta) rospy.Subscriber("/dxl/chain_state", ChainState, self.UpdateAngles) [ rospy.Subscriber("angles_raw_leg_{0}".format(i), numpy_msg(Floats), self.check_angles, i) for i in range(6) ] print "Initialization done. Running..." rospy.spin()
def __init__(self): self.image_pub = rospy.Publisher("contoured_image", Image, queue_size=10) self.lower = np.array([23,87,255], dtype = "uint8") self.upper = np.array([63,137,255], dtype = "uint8") self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/stereo/right/image_rect_color", Image, self.callback) self.image_thresholds = rospy.Subscriber("/threshold_values", numpy_msg(Floats), self.getThresholds)
def listener(): # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. pub = rospy.Publisher('/firefly_2/fov_adj', numpy_msg(Floats),queue_size=10) rospy.init_node('fov2', anonymous=True) #Subscribing to Experimental Topics rospy.Subscriber("/firefly_1/ground_truth/pose", Pose, pos_2) rospy.Subscriber("/firefly_3/ground_truth/pose", Pose, pos_3) rospy.Subscriber("/firefly_4/ground_truth/pose", Pose, pos_4) rospy.Subscriber("/firefly_2/ground_truth/pose", Pose, fov) ######################################################################################### ############################################################################################# rate = rospy.Rate(10) # 10hz, may need this to be 100HZ while not rospy.is_shutdown(): #a = np.array([2.10,3.10,2.14,2.34,2.34,3.14],dtype=np.float32) pub.publish(adj) rate.sleep() # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def __init__(self,name="slm_laser", udp_addr="128.2.0.201", port=30, speed=10, frame="/slm", vis_spot=False, lo_res=False ): self.udp_addr=udp_addr self.udp_port=port #ros rospy.init_node(name) self.pub=rospy.Publisher(name, numpy_msg(LaserScan)) self.ls.range_max=655 self.ls.range_min=0.01 self.ls.header.frame_id=frame self.seq=0 self.comms_fail=0 #Open UDP port self.sock = socket.socket( socket.AF_INET, # Internet socket.SOCK_DGRAM ) # UDP self.sock.settimeout(0.1) self.set_enable_reply(True) self.send_cmd("X1\n") # Set output to Binary self.set_speed(speed) self.set_visible(vis_spot) self.set_resolution(lo_res) self.send_cmd("S\n") # Start motor spinning self.set_laser_on(True)
def __init__(self): ''' Iniate self and subscribe to /scan topic ''' # which nexus? self.name = 'n_2' # Desired distance - used for sending if no z is found or if the dataprocessingnode is shutdown: # robot not influenced if one z not found self.d = np.float32(0.8) self.dd = np.float32(np.sqrt(np.square(self.d) + np.square(self.d))) # set min and max values for filtering ranges in meter during initiation self.min_range = 0.25 self.max_range = 1.2 # set variables for tracking # 0: init, 1: tracking, 2: self.k = 0 # prepare shutdown self.running = True rospy.on_shutdown(self.shutdown) # prepare publisher self.pub = rospy.Publisher( self.name + '/z_values', numpy_msg(Floats), queue_size=1) #Publish the distances and angles to agents in range self.PUB = rospy.Publisher( self.name + '/agents', Int32, queue_size=1) #Publish the amount of surrounding agents # subscribe to /scan topic with calculate_z as calback #rospy.Subscriber('/nexus1/scan', LaserScan, self.calculate_z) #############3 rospy.Subscriber('/n_2hokuyo_points', LaserScan, self.calculate_z) np.set_printoptions(precision=2)
def ref_generation(): xbox_ctrl = rospy.Publisher('xboxctrl', numpy_msg(Floats)) rospy.init_node('xboxbridge', anonymous=True) rate = rospy.Rate(100) # Run no faster than 100hz UDP_IP = "192.168.10.81" UDP_PORT = 27300 sock = socket.socket(socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP sock.bind((UDP_IP, UDP_PORT)) sock.setblocking(0) packer = struct.Struct('c c c c f f f f B B h') while not rospy.is_shutdown(): try: data = sock.recv(48) # buffer size is 48 bytes if len(data) == 24: if data[0] == "X" and data[1] == "B" and data[2] == "O" and data[3] == "X": output = packer.unpack(data) ctrl = numpy.array([output[4],output[5],output[6],output[7],output[8]], dtype=numpy.float32) xbox_ctrl.publish(ctrl) #print output except: # print "Nothing" if str(sys.exc_info()[1]) != '[Errno 11] Resource temporarily unavailable': rospy.logwarn("Unexpected error: %s", sys.exc_info()[1]) nothing = 0 rate.sleep()
def __init__(self): # Data options (change me) self.im_height = 720 self.im_width = 1280 self.tcp_host_ip = '127.0.0.1' #self.tcp_host_ip = '192.168.0.5' self.tcp_port = 50000 self.buffer_size = 4098 # 4 KiB self.color_img = np.empty((self.im_height, self.im_width, 3)) self.depth_img = np.empty((self.im_height, self.im_width)) # Connect to server self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.tcp_socket.connect((self.tcp_host_ip, self.tcp_port)) # ROS Interfacing self.bridge = CvBridge() self.image_pub = rospy.Publisher("image_topic", Image, queue_size=10) self.np_pub = rospy.Publisher('np_data', numpy_msg(Float32), queue_size=10) _thread = threading.Thread(target=self.imageCB) _thread.daemon = True _thread.start() # Connect to server self.intrinsics = None self.get_data() self.get_data_flg = False
def listener(): rospy.init_node('srp_phat_fd_gui', anonymous=True) rospy.Subscriber('/srp_phat_fd_value', numpy_msg(Float32MultiArray), callback, queue_size=1) rospy.spin()
def __init__(self): # initialize potential field variables self.charge_laser_particle = 0.055 self.charge_forward_boost = 25.0 self.boost_distance = 0.5 self.certainty = 0 self.p_speed = 0.05 self.p_steering = 1.0 self.p_d = 0.5 self.turn_right = False self.last_y = 0.0 self.turn_x_component = np.zeros(1) self.turn_y_component = np.zeros(1) # subscribe to turning signal rospy.Subscriber("/turnRight", Bool, self.bool_back) # subscribe to laserscans. Force output message data to be in numpy arrays. rospy.Subscriber("/scan", numpy_msg(LaserScan), self.scan_callback) self.angle_min = 0 self.angle_increment = 0 # output a pose of where we want to go self.pub_goal = rospy.Publisher("~potentialFieldGoal", PointStamped, queue_size=1) self.pub_nav = rospy.Publisher( "/vesc/ackermann_cmd_mux/input/navigation", AckermannDriveStamped, queue_size=1) self.speedHist = 1.0
def stream(kinectTopic): rate = rospy.Rate(10) depthSub = rospy.Subscriber(kinectTopic, PointCloud2, callback) rospy.Subscriber("vec", numpy_msg(vec), callbackVec) print("Starting grasp procedure. Press Ctrl-C to stop...") while not rospy.is_shutdown(): rate.sleep()
def main(args): #global x,y rospy.init_node('objectGrasp', anonymous=True) rospy.Subscriber("vec", numpy_msg(vec), callback) #print x,y rospy.spin() return 0
def __init__(self): self.node = rospy.init_node('qav250', anonymous=True) # publisher for RC commands self.rc_pub = rospy.Publisher('rcctrl', numpy_msg(Floats)) # subscriber for Vicon data self.vicon_sub = rospy.Subscriber('drone', TransformStamped, self.vicon_callback) # subscriber for reference point input self.point_sub = rospy.Subscriber('refpoint', Point, self.ref_callback) # subscriber for integrator freeze self.freeze_int = rospy.Subscriber('freeze_int', Bool, self.freeze_int_callback) # PID controller for each axis self.pitch_pid = rospidlib.Rospid(0.075,0.0,0.11,'~pitch') # pitch self.roll_pid = rospidlib.Rospid(0.075,0.0,0.11,'~roll') # roll self.thrust_pid = rospidlib.Rospid(0.2,0.03,0.15,'~thrust') # height self.yaw_pid = rospidlib.Rospid(0.25,0.0,0.0,'~yaw') # yaw # freeze the thrust integrator until flying self.thrust_pid.freeze_integrator() # point reference input self.ref_point = Point() # default is at origin, 1.8m off ground self.ref_point.z = rospy.get_param('init_z', 0.8) self.ref_point.x = rospy.get_param('init_x', 0.0) self.ref_point.y = rospy.get_param('init_y', 0.0) # get min/max points self.min_x = rospy.get_param('/min_x', -5.0) self.min_y = rospy.get_param('/min_y', -5.0) self.min_z = rospy.get_param('/min_z', 0.0) self.max_x = rospy.get_param('/max_x', 5.0) self.max_y = rospy.get_param('/max_y', 5.0) self.max_z = rospy.get_param('/max_z', 5.0) self.rel_threshold = rospy.get_param('/rel_threshold', 2.5) #broadcast target point to RViz self.target_br = tf.TransformBroadcaster()
def __init__(self): rospy.init_node('findbox', anonymous=True) self.physical_robot = False if self.physical_robot: rospy.Subscriber("/scan/long_range",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1) else: rospy.Subscriber("/scan",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1) rospy.Subscriber('/odometry/filtered',Odometry, self.cb_odom) self.bearing_pub = rospy.Publisher("/detection",numpy_msg(Floats), queue_size=1) self.bridge = CvBridge() self.dist_min = 0.75 self.dist_max = 1.5 # 1m is real target, 41in for height self.ylen_lim = 4 self.ang_min = -1.57 self.ang_max = 1.57 self.R = None self.arena_xpos = 100 #rospy.get_param('arena_xpos') self.arena_xneg = -100 #rospy.get_param('arena_xneg') self.arena_ypos = -100 #rospy.get_param('arena_ypos') self.arena_yneg = 100 #rospy.get_param('arena_yneg') self.rate = rospy.Rate(10) self.scan_dist_thresh = 5.0 # Distance threshold to split obj into 2 obj. self.plot_data = True self.image_output = rospy.Publisher("/output/keyevent_image",Image, queue_size=1)
def test_floats(self): if disable: return vals = [1.0, 2.1, 3.2, 4.3, 5.4, 6.5] b = cStringIO.StringIO() f = Floats( numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)) f.serialize(b) # deserialize twice, once with numpy wrappers, once without f2 = Floats() self.assert_(type(f2.data) == list) f2.deserialize(b.getvalue()) for x, y in zip(f2.data, vals): self.assertAlmostEquals(x, y, 2) from rospy.numpy_msg import numpy_msg f3 = numpy_msg(Floats)() if 0: # future self.assert_(isinstance(f3.data, numpy.ndarray), type(f3.data)) f3.deserialize(b.getvalue()) self.assert_(isinstance(f3.data, numpy.ndarray), type(f3.data)) v = numpy.equal( f3.data, numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)) self.assert_(v.all())
def __init__(self): print('AccelerationControllerNode: initializing node') self.vehicle_model = Vehicle() self.ready = False self.mass = 1. self.inertial_tensor = numpy.identity(3) self.mass_inertial_matrix = numpy.zeros((6, 6)) self.vel_ = numpy.zeros(6) self.acc_ = numpy.zeros(6) # ROS infrastructure self.pub_gen_force = rospy.Publisher('thruster_manager/input', Wrench, queue_size=1) self.sub_accel = rospy.Subscriber('cmd_accel', numpy_msg(Accel), self.accel_callback) self.sub_vel = rospy.Subscriber('/anahita/pose_gt', numpy_msg(Odometry), self.vel_callback) self.sub_force = rospy.Subscriber('cmd_force', numpy_msg(Accel), self.force_callback) """self.pub_gen_force = rospy.Publisher( 'thruster_manager/input', Wrench, queue_size=1)""" if not rospy.has_param("pid/mass"): raise rospy.ROSException("UUV's mass was not provided") if not rospy.has_param("pid/inertial"): raise rospy.ROSException("UUV's inertial was not provided") self.mass = rospy.get_param("pid/mass") self.inertial = rospy.get_param("pid/inertial") # update mass, moments of inertia self.inertial_tensor = numpy.array([ [self.inertial['ixx'], self.inertial['ixy'], self.inertial['ixz']], [self.inertial['ixy'], self.inertial['iyy'], self.inertial['iyz']], [self.inertial['ixz'], self.inertial['iyz'], self.inertial['izz']] ]) self.mass_inertial_matrix = numpy.vstack( (numpy.hstack((self.mass * numpy.identity(3), numpy.zeros( (3, 3)))), numpy.hstack((numpy.zeros((3, 3)), self.inertial_tensor)))) print self.mass_inertial_matrix self.ready = True
def __init__(self): super(KeyboardController,self).__init__() self.pitch = 0 self.roll = 0 self.yaw_velocity = 0 self.z_velocity = 0 # Subscribe to visp auto tracker self.vispStatusSub = rospy.Subscriber('/visp_auto_tracker/status',numpy_msg(Int8),self.ReceiveVispStatus) self.vispPositionSub = rospy.Subscriber('/visp_auto_tracker/object_position',numpy_msg(PoseStamped),self.ReceiveVispPosition) # Variables for visp auto tracker self.vispStatus = 0 self.vispPosition = None self.vispStatusActioned = 0
def talker(): pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10) rospy.init_node('talker', anonymous=True) r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32) pub.publish(a) r.sleep()
def __init__(self): print('VelocityControllerNode: initializing node') self.config = {} self.v_linear_des = numpy.zeros(3) self.v_angular_des = numpy.zeros(3) # Initialize pids with default parameters self.pid_angular = PIDRegulator(1, 0, 0, 1) self.pid_linear = PIDRegulator(1, 0, 0, 1) # ROS infrastructure self.sub_cmd_vel = rospy.Subscriber('cmd_vel', numpy_msg(geometry_msgs.Twist), self.cmd_vel_callback) self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry), self.odometry_callback) self.pub_cmd_accel = rospy.Publisher('cmd_accel', geometry_msgs.Accel, queue_size=10) self.srv_reconfigure = Server(VelocityControlConfig, self.config_callback)
def __init__(self): ''' Initiate self and subscribe to /n_1hokuyo_points topic ''' # which nexus? self.name = 'n_3' # Desired distance - used for sending if no z is found or if the dataprocessingnode is shutdown: # robot not influenced if one z not found self.d = np.float32(0.8) self.dd = np.float32(np.sqrt(np.square(self.d) + np.square(self.d))) # NOTE: not used anymore # set min and max values for filtering ranges. Will change during tracking self.min_range = 0.35 self.max_range = 1.5 # state of processing # 0: init, 1: tracking, 2: self.k = 0 # the robots self.robots = [] # the obstacles self.obstacles = [] # prepare shutdown self.running = True rospy.on_shutdown(self.shutdown) # prepare publisher self.pub_r = rospy.Publisher(self.name + '/r_values', numpy_msg(Floats), queue_size=1) self.pub_obs = rospy.Publisher(self.name + '/obstacles', numpy_msg(Floats), queue_size=1) # prepare publisher for modified laser scan data self.pub_scan = rospy.Publisher('/n_3scan', LaserScan, queue_size=50) self.num_readings = 360 self.laser_frequency = 80 # subscribe to /scan topic with calculate_z as calback rospy.Subscriber('/n_3hokuyo_points', LaserScan, self.calculate_r) np.set_printoptions(precision=2)
def __init__(self): self.p = pyaudio.PyAudio() self.stream = self.p.open(format=self.FORMAT, channels=self.CHANNELS, rate=self.FS, input=True, frames_per_buffer=self.CHUNK) self.rec_publisher = rospy.Publisher('/wav',numpy_msg(Floats),queue_size =20)
def __init__(self): self.frame = None self.colors = [] self.hasNewFrame = False self.thresholds = np.array([0,0,0,0,0,0], dtype= np.float32) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/stereo/right/image_raw", Image, self.callback) self.image_thresholds = rospy.Publisher("/threshold_values",numpy_msg(Floats), queue_size=10) client = dynamic_reconfigure.client.Client("vision_server", timeout=30, config_callback=callback)
def __init__(self): print('PositionControllerNode: initializing node') self.config = {} self.pos_des = numpy.zeros(3) self.quat_des = numpy.array([0, 0, 0, 1]) self.initialized = False # Initialize pids with default parameters self.pid_rot = PIDRegulator(1, 0, 0, 1) self.pid_pos = PIDRegulator(1, 0, 0, 1) # ROS infrastructure self.sub_cmd_pose = rospy.Subscriber('cmd_pose', numpy_msg(geometry_msgs.PoseStamped), self.cmd_pose_callback) self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry), self.odometry_callback) self.pub_cmd_vel = rospy.Publisher('cmd_vel', geometry_msgs.Twist, queue_size=10) self.srv_reconfigure = Server(PositionControlConfig, self.config_callback)
def __init__(self): self.backend = ModelInterface.load(self.INPUT_MODEL) try: fs, signal = read_wav(self.BG) self.backend.init_noise(fs, signal) except: print "file not found!" self.pub = rospy.Publisher('/speaker',String,queue_size = 10) self.sub = rospy.Subscriber('/wav',numpy_msg(Floats),self.task_predict)
def __init__(self): #Initialize ros publisher, subscriber self.pub1 = rospy.Publisher('prediction',PredictionMSG,queue_size=1) self.sub1 = rospy.Subscriber('cropped_box_image/numpy',numpy_msg(Floats),self.callback,queue_size=1) self.pub2 = rospy.Publisher('cropped_box_image/image',Image, queue_size=1) self.bridge = CvBridge() self.pt1x = -40.0 self.pt1y = 0.0 self.pt2x = 40.0 self.pt2y = 0.0 rospy.loginfo("Initialized!")
def test_cloud(self): cloud = rospy.Subscriber("/ldmrs/cloud", numpy_msg(PointCloud2), self.callback) self.get_msg() self.init_seq() self.send_ldmrs_msg() self.callback_event.wait(self.timeout) self.assertTrue(self.callback_event.isSet(), "Timeout waiting for cloud message") self.callback_event.clear() cloud.unregister() self.check_point_cloud()
def __init__(self, camera=None): if camera is None: camera = '' self.image_source = camera + "/image_raw" self.pub_name = camera + "/image_numpy" self.bridge = CvBridge() self.np_image_pub = rospy.Publisher(self.pub_name, numpy_msg(Uint16s)) rospy.init_node('image_converter', anonymous=True) self.image_sub = rospy.Subscriber(self.image_source,Image,self.image_callback)
def __init__(self): #Initialize ros publisher, subscriber self.pub1 = rospy.Publisher('prediction',PredictionMSG,queue_size=1) self.sub1 = rospy.Subscriber('box_image/numpy',numpy_msg(Floats),self.callback,queue_size=1) self.pub2 = rospy.Publisher('p_box_image/image',Image, queue_size=1) self.pub3 = rospy.Publisher('predicted_class', String, queue_size=1) self.bridge = CvBridge() self.pt1x = -15.0 self.pt1y = 0.0 self.pt2x = 15.0 self.pt2y = 0.0 rospy.loginfo("Evaluator Initialized!")
def __init__(self, topics): # self.bridge = CvBridge() # Flags self.need_rgb = True self.need_depth = True # RGB Subscribers sub_rgb = message_filters.Subscriber(topics['rgb'], numpy_msg(Image)) sub_rgb_info = message_filters.Subscriber(topics['rgb_info'], CameraInfo) ts_rgb = message_filters.TimeSynchronizer([sub_rgb, sub_rgb_info], 10) ts_rgb.registerCallback(self.rgb_callback) # Depth Subscribers sub_depth = message_filters.Subscriber(topics['depth'], numpy_msg(Image)) sub_depth_info = message_filters.Subscriber(topics['depth_info'], CameraInfo) ts_depth = message_filters.TimeSynchronizer([sub_depth, sub_depth_info], 10) ts_depth.registerCallback(self.depth_callback) self.pub_filtered = rospy.Publisher('/camera/rgb/image_color_filtered', numpy_msg(Image))
def __init__(self, limb): ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self._sub_path = rospy.Subscriber('joint_path', numpy_msg(Float32MultiArray), self.path_cb) self._sub_traj = rospy.Subscriber('joint_traj', numpy_msg(Float32MultiArray), self.traj_cb) self._limb_interface = baxter_interface.limb.Limb('right') self._q_start = np.array([-0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914, -1.16889336, -0.90888362]) self.clear(limb)
def __init__(self): limb = 'left' cv.NamedWindow("Image window0", 1) cv.NamedWindow("Image window1", 1) cv.NamedWindow("Image window2", 1) cv.NamedWindow("Image window3", 1) cv.NamedWindow("Image window4", 1) cv.NamedWindow("Image window5", 1) self.bridge = CvBridge() self.head_pub = rospy.Publisher('/sdk/xdisplay', Image, latch=True) ##OPEN BAG FILE FOR TASKS task_num = rospy.get_param("test_task_number") pre_path = "/opt/ros/groovy/stacks/baxterjd/tasks/left/" file_name = "kitting_task"+str(task_num)+".bag" bagfile = rosbag.Bag(pre_path+file_name,'r') # Read required information for topic, msg, time in bagfile.read_messages(): if topic == 'train_hsv_val': self.hsv_callback(msg) elif topic == 'train_mean_eigen_list': self.mean_eigen_callback(msg) elif topic == 'train_shape_topic': self.shape_callback(msg) bagfile.close() #Publishes the center pixel coordinate of the detected object self.mean_pub = rospy.Publisher('detected_object_error', numpy_msg(Floats)) self.image_sub = rospy.Subscriber("/cameras/"+limb+"_hand_camera/image",Image,self.callback) self.hessian_threshold = 10 self.image = Image() self.fingers_calibrated = False self.gripper = baxter_interface.Gripper(limb) open = numpy.asarray(cv2.cv.LoadImage("/opt/ros/groovy/stacks/baxterjd/table_detector/src/calibration_open.jpg")[:,:]) close = numpy.asarray(cv2.cv.LoadImage("/opt/ros/groovy/stacks/baxterjd/table_detector/src/calibration_close.jpg")[:,:]) self.calibration_open = cv2.cvtColor(open,cv2.COLOR_BGR2GRAY) self.calibration_close = cv2.cvtColor(close,cv2.COLOR_BGR2GRAY) self.subscribed_topic = False
def __init__(self): # initialize potential field variables self.charge_laser_particle = 0.07 self.charge_forward_boost = 25.0 self.boost_distance = 0.5 self.p_speed = 0.007 self.p_steering = 1.0 # subscribe to laserscans. Force output message data to be in numpy arrays. rospy.Subscriber("/scan", numpy_msg(LaserScan), self.scan_callback) # output a pose of where we want to go self.pub_goal = rospy.Publisher("~potentialFieldGoal", PointStamped, queue_size=1) self.pub_nav = rospy.Publisher("/vesc/ackermann_cmd_mux/input/navigation", AckermannDriveStamped, queue_size=1)
def __init__(self): # create subscribers, timers, clients, etc. try: rospy.wait_for_service("/check_state_validity", timeout=5) except ROSException: rospy.logwarn("[check_collisions_node] Done waiting for /check_state_validity service... service not found") rospy.logwarn("shutting down...") rospy.signal_shutdown("service unavailable") except ROSInterruptException: pass self.coll_client = rospy.ServiceProxy("check_state_validity", GetStateValidity) self.js_sub = rospy.Subscriber("joint_state_check", numpy_msg(Float32MultiArray), self.js_cb) self.js_pub = rospy.Publisher("collision_check", Int16, queue_size = 10) return
def __init__(self, joint_names): self.joint_names = joint_names self.num_dof = len(self.joint_names) self.init_work_variables() self.js_sub = rospy.Subscriber("~in_joint_states", numpy_msg(JointState), self.js_cb, queue_size=3, tcp_nodelay=True) self.out_js_pub = rospy.Publisher("~out_joint_states", numpy_msg(JointState), queue_size=3, tcp_nodelay=True, latch=False)