def statusInterpreter(status): # Variable Initialization gripperStatus = 0 gripperMovement = 0 # Gripper Reset if (status.gACT == 0) and (status.gIMC == 0): gripperStatus = 0 # Gripper Activating if (status.gACT == 1) and (status.gIMC == 1) and (status.gSTA == 0): gripperStatus = 1 # Gripper Activated if (status.gACT == 1) and (status.gIMC == 3): gripperStatus = 2 # Gripper Open if (status.gPOA < 110) and (status.gPOB < 110) and (status.gPOC < 110): handClosed = False # Gripper Closed if (status.gPOA >= 110) and (status.gPOB >= 110) and (status.gPOC >= 110): handClosed = True # Gripper Moving if (status.gDTA == 0) and (status.gDTA == 0) and (status.gDTA == 0): handMoving = True # Gripper Stopped if (status.gDTA == 3) and (status.gDTA == 3) and (status.gDTA == 3): handMoving = False # Hand Moving and Opening if handMoving and not handClosed: gripperMovement = 0 # Hand Moving and Closing if handMoving and handClosed: gripperMovement = 1 # Hand Stoppend and Open if not handMoving and not handClosed: gripperMovement = 2 # Hand Stopped and Closed if not handMoving and handClosed: gripperMovement = 3 output = HoldingRegister() output.data = [gripperStatus, gripperMovement] # rospy.loginfo("Updating Robot Registers") pub_robot.publish(output)
def setValues(self, address, value): """ Sets the requested values to the holding registers and publishes they new values on a rostopic :param address: The starting address :type address: int :param values: The new values to be set :type values: list[int] """ # print "address",address,"value",value ModbusSequentialDataBlock.setValues(self,address, value) if address >= self.reading_start: msg = HoldingRegister() msg.data = value self.pub.publish(msg)
def __init__(self): rospy.init_node('global_path_viz') #Publisher self.global_path_marker_pub = rospy.Publisher('/global_path/path/viz', MarkerArray, queue_size=1, latch=True) #Subscriber self.node_edge_map_sub = rospy.Subscriber('/node_edge_map/map', NodeEdgeMap, self.node_edge_map_callback) self.global_path_sub = rospy.Subscriber('/global_path/path', Int32MultiArray, self.global_path_callback) self.global_path_marker = MarkerArray() self.global_path = Int32MultiArray() self.node_edge_map = NodeEdgeMap() self.sub_map = False
def sensor_node(): c = collect_data() pub = rospy.Publisher('/sensor_values', Int32MultiArray, queue_size=1) rospy.init_node('sensor_node') rate = rospy.Rate(50) while not rospy.is_shutdown(): values = next(c) msg = Int32MultiArray( MultiArrayLayout( [MultiArrayDimension('sensor data', 6, 1)], 1), # CHANGE the second arg to no. of sensor values read values) # from serialport(just the values) pub.publish(msg) rate.sleep()
def talk_goal_pos(): global goalpos rospy.init_node('goal_detector', anonymous=True) pub = rospy.Publisher("goal_pos", Int32MultiArray, queue_size=10) rospy.Subscriber("VideoRaw", Image, callback) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): if goalpos == None: pass else: goal_positions = Int32MultiArray(data=goalpos) rospy.loginfo(goal_positions) pub.publish(goal_positions) rate.sleep()
def recive_frame_and_track(self, msg): if self.init_rect==None: return if True: cv2_img = self.bridge.imgmsg_to_cv2(msg, "bgr8") if self.init_rect is not None and self.tracker_manager is None: self.tracker_manager = tracker_manager(self.init_rect, cv2_img, self.regressor, self.tracker) return else: bbox = self.tracker_manager.track_frame(cv2_img) bbox_msg = Int32MultiArray() bbox_msg.data = [int(bbox.x1), int(bbox.y1), int(bbox.x2), int(bbox.y2)] print(bbox_msg) self.goturn_pub.publish(bbox_msg)
def Sub_Red(self, msg): if (msg is not None): scan = [999] * SCAN_NUM for i in range(len(msg.ranges)): if (msg.ranges[i] == math.inf): scan[i] = SCAN_LIMIT elif (msg.ranges[i] == math.isnan): scan[i] = 20 else: scan[i] = int(round(msg.ranges[i], 2) * 100) pub_msg = Int32MultiArray() pub_msg.data = scan self.pub_red.publish(pub_msg)
def callback(self, msg): data = msg.data numOfFaces = data[1] frameRate = data[0] pictureWidth = data[2] pictureHeight = data[3] filteredData = [] filteredData.append(data[0]) #frame rate filteredData.append(0) #number of faces filteredData.append(data[2]) #width filteredData.append(data[3]) #height faceCounter = 0 for i in range(numOfFaces): if data[i * 6 + 5] > frameRate / 2 and data[ i * 6 + 8] > pictureWidth / 15 and data[ i * 6 + 9] > pictureHeight / 15: filteredData.append(data[6 * i + 4]) #face index filteredData.append(data[6 * i + 5]) #face duration filteredData.append(data[6 * i + 6]) #face corner x filteredData.append(data[6 * i + 7]) #face corner y filteredData.append(data[6 * i + 8]) #face width filteredData.append(data[6 * i + 9]) #face height faceCounter += 1 #print filteredData filteredData[1] = faceCounter dataForPublish = Int32MultiArray(data=filteredData) self.faceCoord_pub.publish(dataForPublish) if not (self.SentTrigger) and faceCounter > 0: self.facePresentCounter += 1 if self.SentTrigger and faceCounter == 0: self.facePresentCounter += 1 if self.facePresentCounter > 2 * frameRate and not (self.SentTrigger): self.facePresent_pub.publish(True) self.SentTrigger = True self.facePresentCounter = 0 if self.facePresentCounter > 6 * frameRate and self.SentTrigger: self.facePresent_pub.publish(False) self.SentTrigger = False self.facePresentCounter = 0
def callback(self, data): cv_image = self._bridge.imgmsg_to_cv2(data, 'bgr8') gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) gray_image = gray_image > np.max(np.max(gray_image)) line1_data = gray_image[120:122, 1:640] line2_data = gray_image[240:242, 1:640] line3_data = gray_image[1:480, 480:482] line4_data = gray_image[1:480, 320:322] line1 = Int32MultiArray() line2 = Int32MultiArray() line3 = Int32MultiArray() line4 = Int32MultiArray() line1.data = np.sum(line1_data, axis=0) line2.data = np.sum(line2_data, axis=0) line3.data = np.sum(line3_data, axis=0) line4.data = np.sum(line4_data, axis=0) # tmpline = np.asarray(line1.data[1:638],dtype=float)-np.asarray(line1.data[2:639],dtype=float) # inds = indices(np.abs(tmpline),lambda x: x > 20) print np.array(line1.data, dtype=float) > np.max(line1.data) * 0.8 gray_image[120:122, 1:640] = 255 gray_image[240:242, 1:640] = 255 gray_image[1:480, 480:482] = 255 gray_image[1:480, 320:322] = 255 self._gray_pub.publish(self._bridge.cv2_to_imgmsg(gray_image, 'mono8')) self._line1_pub.publish(line1) self._line2_pub.publish(line2) self._line3_pub.publish(line3) self._line4_pub.publish(line4) self._csteeringline.data = 0.0 self._csteeringline_pub.publish(self._csteeringline) self._cstopline.data = False self._cstopline_pub.publish(self._cstopline)
def finding_path(ros_data): global flag global final_path np_arr = np.fromstring(ros_data.data, np.uint8) image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) frame = image_np #cv2.imwrite('/home/benlee/catkin_ws/src/find_path/script/map.png',frame, params=[cv2.IMWRITE_PNG_COMPRESSION,0]) #frame = cv2.imread('/home/benlee/catkin_ws/src/find_path/script/map.png',cv2.IMREAD_COLOR) start_point = Img_process.Basic_map() end_point = Img_process.Basic_map() obstacle_map = Img_process.Basic_map() ret_start,start_x, start_y, start_img, s_w, s_h = start_point.find(frame, 'BLUE') ret_end, end_x, end_y, end_img, e_w, e_h = end_point.find(frame, 'RED') #print ("start",ret_start,"end",ret_end) capstone_map = obstacle_map.obstacle_load(frame) cv2.imshow('end_img', end_img) cv2.imshow('img', frame) cv2.imshow('start_img', start_img) cv2.imshow('obstacle', capstone_map) if ret_start == 1 and ret_end == 1: start = (int(start_x), int(start_y)) end = (int(end_x), int(end_y)) # print("start:", start) # print("end", end) if flag == 0: path = astar(capstone_map, start, end) final_path = Int32MultiArray(data = path) pub.publish(final_path) #print ("path", path) if path is not None and flag == 0: if len(path)>0: for i in range(0, len(path), 2): # print ("i",i) # print("path:", (path[i],path[i+1])) cv2.circle(frame, (path[i],path[i+1]), 1, (0,0,255), 1) flag = 1 cv2.imwrite('/home/benlee/catkin_ws/src/find_path/script/path.png',frame, params=[cv2.IMWRITE_PNG_COMPRESSION,0]) else: print("no path bro") else: print(" no image, setting random start,end poinst") found_path = cv2.imread('/home/benlee/catkin_ws/src/find_path/script/path.png',cv2.IMREAD_COLOR) flag = 0 cv2.imshow('find path',found_path) cv2.waitKey(1) & 0xFF
def talker(): pub = rospy.Publisher('cmd_joints', Int32MultiArray, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(40) # 10hz i = 0 while not rospy.is_shutdown(): positions = Int32MultiArray() positions.data = [ abs(int(1023 * sin(i))), 160, 800, abs(int(800 * sin(i))), 500 ] i = i + 0.01 rospy.loginfo(positions) pub.publish(positions) rate.sleep()
def socket_talker(): ##創建Server node pub = rospy.Publisher('chatter', Int32MultiArray, queue_size=10) rospy.init_node(NAME) #a = rospy.Service('arm_mode',arm_mode, Arm_Mode) ##server arm mode data #s = rospy.Service('arm_pos',arm_data, point_data) ##server arm point data #b = rospy.Service('speed_mode',speed_mode, Speed_Mode) ##server speed mode data rate = rospy.Rate(100) # 10hz print("Ready to connect") while not rospy.is_shutdown(): # hello_str = "hello world %s" % rospy.get_time() state = Int32MultiArray() state.data = [state_feedback.ArmState, state_feedback.SentFlag] pub.publish(state) rate.sleep()
def _boxes_pub(self, boxes_in, scores_in, classes_in, width, height): ROI_msgs = [] i = 0 while i < len(boxes_in[0]): if scores_in[0][i] > 0.65: min_x = boxes_in[0][i][1] * 640 max_x = boxes_in[0][i][3] * 640 min_y = boxes_in[0][i][0] * 480 max_y = boxes_in[0][i][2] * 480 ROI = [classes_in[0][i], min_x, max_x, min_y, max_y] ROI_msgs = ROI_msgs + ROI i = i + 1 print(ROI_msgs) image_ROI = Int32MultiArray(data=ROI_msgs) self._pub_ROI.publish(image_ROI)
def box_publish(boxes): msg = Int32MultiArray() msg.layout.dim = [MultiArrayDimension(),MultiArrayDimension()] n = 7 #Gandalfs number msg.layout.dim[0].label = "n_boxes" msg.layout.dim[0].size = len(boxes)/n msg.layout.dim[0].stride = len(boxes) msg.layout.dim[1].label = "nums" msg.layout.dim[1].size = n msg.layout.dim[1].stride = n msg.layout.data_offset = 0 msg.data = boxes box_pub.publish(msg) return
def socket_talker(): ##創建Server node pub = rospy.Publisher('chatter', Int32MultiArray, queue_size=10) rospy.init_node(NAME) rate = rospy.Rate(10) # 10hz print("Ready to connect") while not rospy.is_shutdown(): # hello_str = "hello world %s" % rospy.get_time() state = Int32MultiArray() state.data = [state_feedback.ArmState, state_feedback.SentFlag] # rospy.loginfo(state) pub.publish(state) rate.sleep() rospy.spin
def main(): cam = CameraSubscriber() centroid_module = utils.centroid_module() best_params_path = "model/model_v2.pickle" model = utils.create_model(best_params_path) labels_path = "model/label_colors.json" labels_dict = utils.labels_colors_dicts(labels_path) #Sleep until the subscribers are ready. time.sleep(0.40) # frame = cam.right_frame # cv2.imshow("d", frame) # cv2.waitKey(0) count = 0 try: while not rospy.core.is_shutdown(): # rospy.rostime.wallsleep(0.25) frame = cam.right_frame final_frame, mask = utils.test_img(frame,model, labels_dict) print("prediction {:}".format(count)) count += 1 cv2.imshow("img", final_frame) k = cv2.waitKey(1) & 0xFF centroids = centroid_module.calculate_centroids(mask) # print(centroids) centroids = centroids.reshape(-1).tolist() arr_to_send = Int32MultiArray() arr_to_send.data = centroids cam.multi_arr_pub.publish(arr_to_send) time.sleep(2) if k == 27: cv2.imwrite('./masks/mask1.jpg', mask) break # break # cv2.waitKey(1) except KeyboardInterrupt: cv2.imwrite('./masks/mask1.jpg', mask) print("Shutting down") cv2.destroyAllWindows()
def main(): rospy.init_node('motion_controller', anonymous=True) rospy.Subscriber("/dxl_data/present_position_array", Int32MultiArray, dynamixel_position_callback) rospy.Subscriber("/dxl_data/present_velocity_array", Int32MultiArray, dynamixel_velocity_callback) rospy.Subscriber("/Angle", Float64, robot_position_callback) pub0 = rospy.Publisher('/program_state', Int32, queue_size=1) pub1 = rospy.Publisher('/dxl_target/Target_position', Int32MultiArray, queue_size=1) pub2 = rospy.Publisher('/dxl_target/Target_current', Int32, queue_size=1) sleep_rate = 100 # 100Hz rate = rospy.Rate(sleep_rate) dynamixel_goal = np.array([35, 0]) publish_target_position_array = Int32MultiArray(data=dynamixel_goal) previous_robot_position = 0. state = input() print("RUNNING") pub0.publish(state) while not rospy.is_shutdown(): robot_velocity = (previous_robot_position - present_robot_position) * sleep_rate #print(robot_velocity) desired_position = alpha * np.arctan(robot_velocity) publish_target_current = int((control_signal_definition( present_robot_position, -1 * present_dynamixel_position[1], desired_position, robot_velocity, -1 * present_dynamixel_velocity[1]) / torque_constant) * 10 / (current_scaling_factor)) pub1.publish(publish_target_position_array) pub2.publish(publish_target_current) previous_robot_position = present_robot_position rate.sleep()
def __init__(self, model_name): self.model_name = model_name self.wheel_vels = Int32MultiArray() self.wheel_vels.data = [0, 0] self.camera_img = None self.input_size = 8 self.output_size = 2 # move net out self.net = Net(self.input_size, self.output_size) self.inputs = torch.ones([1, 8], dtype=torch.float64) self.inputs = self.inputs.float() t = threading.Thread(target=self.start) t.daemon = True t.start()
def __init__(self): self.wheel_left = Encoder() self.wheel_right = Encoder() self.array_to_publish = Int32MultiArray(data=[0, 0]) GPIO.setmode(GPIO.BOARD) GPIO.setup(MOTORENCODERRIGHT, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) GPIO.add_event_detect(MOTORENCODERRIGHT, GPIO.RISING, callback=self.wheel_right.tick, bouncetime=2) GPIO.setup(MOTORENCODERLEFT, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) GPIO.add_event_detect(MOTORENCODERLEFT, GPIO.RISING, callback=self.wheel_left.tick, bouncetime=2)
def publish_array(): global explore, arrpub, borders points = [] for i in range(curr_point.x - range_, curr_point.x + range_): for j in range(curr_point.y - range_, curr_point.y + range_): if (((i - curr_point.x) * (i - curr_point.x) + (j - curr_point.y) * (j - curr_point.y) <= range_ * range_) and (i <= grid_size - 1 and i >= 0 and j <= grid_sizey - 1 and j >= 0)): points.append(i) points.append(j) else: continue a = Int32MultiArray() a.data = points arrpub.publish(a)
def netsend(msg, flag=-1, need_unpack=True): global pro_pub, gesture_id if msg: if flag != -1 and flag != -19 and flag != -18: rospy.loginfo("flag is {}. msg is {}".format(flag, msg)) if need_unpack: send = [] for i in range(len(msg)): send.append(int(msg[i][0])) send.append(int(msg[i][1])) a = deepcopy(send) a.append(flag) else: a = deepcopy(msg) a.append(flag) pro_pub.publish(Int32MultiArray(data=a))
def timer_cb(msg1,msg2,msg3,msg4): # planes_indices_l = msg1.indices # planes_indices_r = msg2.indices # global length_r,length_l,length_cover_r,length_cover_l length_l=len(msg1.indices) length_r=len(msg2.indices) length_cover_l=len(msg3.indices) length_cover_r=len(msg4.indices) flag_list=Int32MultiArray() if length_l < threshould_l: flag_l= 0 else: flag_l= 1 if length_r < threshould_r: flag_r= 0 else: flag_r= 1 if length_cover_l < threshould_cover_l: flag_cover_l= 0 else: flag_cover_l= 1 if length_cover_r < threshould_cover_r: flag_cover_r= 0 else: flag_cover_r= 1 # print('in_hand points [r , l] = {}'.format([length_r,length_l])) print('inhand R', length_r) print('inhand L', length_l) print('covered R', length_cover_r) print('covered L', length_cover_l) print('holding flag [r , l] = {}'.format([flag_r,flag_l])) # print('flag_l', flag_l) # print('flag_r', flag_r) # print('in_vision points [r , l] = {}'.format([in_vision_r,in_vision_l])) print('arm coverd flag [r , l] = {}'.format([flag_cover_r,flag_cover_l])) print('in_vision flag [r , l] = {}'.format([in_vision_r,in_vision_l])) pub_num_l.publish(length_l) pub_num_r.publish(length_r) # pub_drop_flag_l.publish(flag_l) # pub_drop_flag_r.publish(flag_r) flag_list.data=[flag_r,flag_l,flag_cover_r,flag_cover_l,in_vision_r,in_vision_l] pub_drop_flag.publish(flag_list)
def check_leader(): # check which robot is the leader desired_leader = 0 max = 0 for i in range(len(inrange_count)): if (inrange_count[i] > max): max = inrange_count[i] desired_leader = i # Notify the user with the highest count rospy.loginfo('Highest count is at Robot {}. Sees {} robots'.format( desired_leader + 1, inrange_count[desired_leader])) rospy.loginfo('All range counts are: {}'.format(inrange_count)) rangePub.publish(Int32MultiArray(data=inrange_count)) leaderPub.publish(desired_leader) rospy.loginfo('inrange_count is now published')
def motor_angle_publisher(): global counter_array pub = rospy.Publisher('motor_encoder', Int32MultiArray, queue_size=10) rospy.init_node('publisher', anonymous=True) rate = rospy.Rate(1000) counter_array = Int32MultiArray() counter_array.data = [] clk1, clk2, dt1, dt2, counter1, counter2, clkLastState1, clkLastState2 = init_motor_encoders( ) while not rospy.is_shutdown(): clkState1 = GPIO.input(clk1) clkState2 = GPIO.input(clk2) dtState1 = GPIO.input(dt1) dtState2 = GPIO.input(dt2) if clkState1 != clkLastState1: if dtState1 != clkState1: counter1 += 1 else: counter1 -= 1 print counter1 if clkState2 != clkLastState2: if dtState2 != clkState2: counter2 += 1 else: counter2 -= 1 print counter2 clkLastState1 = clkState1 clkLastState2 = clkState2 counter_array.data = [counter1, counter2] pub.publish(counter_array) sleep(0.01) #rate.sleep() GPIO.cleanup() return
def listener(): #global the_map the_map = Int32MultiArray() rospy.init_node('Tracking') rospy.Subscriber('robot1', Int32MultiArray, callback1) rospy.Subscriber('robot2', Int32MultiArray, callback2) rospy.Subscriber('robot3', Int32MultiArray, callback3) rospy.Subscriber('robot4', Int32MultiArray, callback4) rospy.Subscriber('obst1', Int32MultiArray, callback5) rospy.Subscriber('obst2', Int32MultiArray, callback6) rospy.Subscriber('rob1_CurrentPose', Int32MultiArray, callback7) rospy.Subscriber('rob2_CurrentPose', Int32MultiArray, callback8) rospy.Subscriber('rob3_CurrentPose', Int32MultiArray, callback9) rospy.Subscriber('rob4_CurrentPose', Int32MultiArray, callback10) while not rospy.is_shutdown(): rospy.sleep(1)
def path_planning_routine(): map_valid = create_map() if map_valid: route = pathPlanner(the_map,n,m,dirs,dx,dy,robot_position[0], robot_position[1],goal_position[0],goal_position[1]) update_map_w_route(route) path_squares = get_path_squares(the_map) print "Path is: \n", path_squares, "\n" print "Final Map is: \n", the_map, "\n" publisher_path = np.array(path_squares).flatten().tolist() if ( len(publisher_path) <= 0): publisher_path = robot_position pathPublisher.publish(Int32MultiArray(data=publisher_path)) return path_squares else: print "Map is invalid. No Planning will occur" return 0
def listener(): global pub pub_arr_key = Int32MultiArray() pub = rospy.Publisher('x', String, queue_size=10) rospy.init_node('converter', anonymous=True) rospy.Subscriber("cmd_vel", Twist, callback) rate = rospy.Rate(10) # spin() simply keeps python from exiting until this node is stopped rospy.spin() rate.sleep()
def joyPub(self): vl, vr = 200.0, 200.0 # define teleop velocity v, bytes = [0.0] * 2, [ 0 ] * 2 # allocate space for arrays that will be published velocity = Float32MultiArray() # objects for ros variables ros_bytes = Int32MultiArray() if self.name == 'Xbox Wireless Controller': joy = pygame.joystick.Joystick(self.joy_id) joy.init() # initialize object created above for i in range(joy.get_numaxes()): axis = joy.get_axis(i) print('buttons active at %d are %s\n' % (i, axis)) if joy.get_button( self.B) == 1: # If B is pressed, start sending commands hat = joy.get_hat(self.hat) # check hat controls for input if hat[0] == -1: # store desired velocity per input v = [-vl, vr] elif hat[0] == 1: v = [vl, -vr] elif hat[1] == -1: v = [-vl, -vr] elif hat[1] == 1: v = [vl, vr] velocity.data = v self.velPub.publish(velocity) if joy.get_button( self.rBumper ) == 1: # turn on vacuum and all brushes if right trigger is active bytes = [ 138, 7 ] # motor byte determined from bits available given in create specs ros_bytes.data = bytes self.bytePub.publish( ros_bytes) # publish bytes for writing to create if joy.get_button( self.lBumper ) == 1: # turn off vacuum if left trigger is activated bytes = [138, 0] ros_bytes.data = bytes self.bytePub.publish(ros_bytes)
def joyCallback(self, joy_msg): com_data = [0, 0] button_msg = Int32() steering_cmd = joy_msg.axes[self.get_parameter("LEFT_JOY_LR").value] com_data[0] = roundUp(int(100 * (steering_cmd)), 10) lt_val = joy_msg.axes[self.get_parameter("LT").value] rt_val = joy_msg.axes[self.get_parameter("RT").value] com_data[1] = roundUp(int(100 * (lt_val - rt_val)), 10) msg = Int32MultiArray(data=com_data) self.command_pub.publish(msg) breaking_cmd = joy_msg.buttons[self.get_parameter("B").value] button_msg.data = breaking_cmd #self.get_logger().info('Publishing: "%s"' % msg.data) self.breaking_pub.publish(button_msg)
def socket_talker(): ##創建Server node pub = rospy.Publisher('chatter', Int32MultiArray, queue_size=10) rospy.init_node(NAME) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): # hello_str = "hello world %s" % rospy.get_time() state = Int32MultiArray() state.data = [1 2] # rospy.loginfo(state) pub.publish(state) rate.sleep() # a = rospy.Service('arm_mode',arm_mode, Arm_Mode) ##server arm mode data # s = rospy.Service('arm_pos',arm_data, point_data) ##server arm point data # b = rospy.Service('speed_mode',speed_mode, Speed_Mode) ##server speed mode data #c = rospy.Service('grip_mode',grip_mode, Grip_Mode) ##server grip mode data print ("Ready to connect")
def numpy_to_multiarray(A): strides = np.cumprod( A.shape[::-1])[::-1] # unused, but conforms to the ROS spec layout = MultiArrayLayout(dim=[ MultiArrayDimension(size=d, stride=s) for d, s in zip(A.shape, strides) ]) if A.dtype == np.float32: return Float32MultiArray(layout, A.reshape(-1).tolist()) elif A.dtype == np.float64: return Float64MultiArray(layout, A.reshape(-1).tolist()) elif A.dtype == np.int32: return Int32MultiArray(layout, A.reshape(-1).tolist()) elif A.dtype == np.int64: return Int64MultiArray(layout, A.reshape(-1).tolist()) else: raise TypeError
def camera_depth_callback(msg): global processing if (processing == False): processing = True time_start = time.clock() result = Int32MultiArray() result.data = [] cloud = tr3_nav.transformer.transform(msg) for row in range(cloud.shape[0]): for col in range(cloud.shape[1]): n = 0 x = cloud[row][col][0] y = cloud[row][col][1] z = cloud[row][col][2] if math.isnan(x): n = 1 x = 0 else: x = int(x * 1000000) if math.isnan(y): n = 1 y = 0 else: y = int(y * 1000000) if math.isnan(z): n = 1 z = 0 else: z = int(z * 1000000) result.data.append(n) result.data.append(x) result.data.append(y) result.data.append(z) pub.publish(result) time_elapsed = (time.clock() - time_start) processing = False
############### # Example 3 # Listen for the writeable modbus registers in any node def callback(msg): rospy.loginfo("Modbus register have been written: %s",str(msg.data)) rospy.sleep(2) sub = rospy.Subscriber("modbus_server/read_from_registers",HoldingRegister,callback,queue_size=500) ############### ############### # Example 4 # Publisher to write first 20 modbus registers from any node pub = rospy.Publisher("modbus_server/write_to_registers",HoldingRegister,queue_size=500) rospy.sleep(1) msg = HoldingRegister() msg.data = range(20) msg2 = HoldingRegister() msg2.data = range(20,0,-1) while not rospy.is_shutdown(): pub.publish(msg) rospy.sleep(1) pub.publish(msg2) rospy.sleep(1) ################ rospy.spin() mws.stopServer()
################# # Example 2 # Create a listener that show us a message if anything on the readable modbus registers change rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c") def showUpdatedRegisters(msg): rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data)) sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500) ################# ################# # Example 3 # writing to modbus registers using a standard ros publisher pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500) output = HoldingRegister() output.data = xrange(20,40) output2 = HoldingRegister() output2.data = xrange(40,20,-1) rospy.loginfo("Sending arrays to the modbus server") while not rospy.is_shutdown(): rospy.sleep(1) pub.publish(output) rospy.sleep(1) pub.publish(output2) ################# # Stops the listener on the modbus modclient.stopListening()
################# # Example 2 # Create a listener that show us a message if anything on the readable modbus registers change rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c") def showUpdatedRegisters(msg): rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data)) sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500) ################# ################# # Example 3 # writing to modbus registers using a standard ros publisher pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500) output = HoldingRegister() output.data = [1 for x in xrange(0,6)] output2 = HoldingRegister() output2.data = [0 for x in xrange(0,6)] rospy.loginfo("Sending arrays to the modbus server") for i in xrange(5): rospy.sleep(1) pub.publish(output) rospy.sleep(1) pub.publish(output2) ################# rospy.loginfo("Outputs tests all done, just listening to inputs. Stop listening by pressing Ctrl+c") rospy.spin() # Stops the listener on the modbus