def talker(): pub = rospy.Publisher('path', Float32MultiArray, queue_size=10) rospy.init_node('path_planner', anonymous=True) rate = rospy.Rate(1) # 1hz waypoints = Float32MultiArray() waypoints.layout.dim.append(MultiArrayDimension()) waypoints.layout.dim.append(MultiArrayDimension()) waypoints.layout.dim[0].label = "Length" #waypoints.layout.dim[0].size = 18 length = waypoints.layout.dim[0].size waypoints.layout.dim[0].stride = length * 3 waypoints.layout.dim[1].label = "Joints" waypoints.layout.dim[1].size = 3 waypoints.layout.dim[1].stride = 3 waypoints.layout.data_offset = 0 while not rospy.is_shutdown(): waypoints.data = [0] * (length) #for i in range(0, length): # waypoints.data[i] = i + 1 #waypoints.data = [-1.44, 1.4, 0.6, -1.57, 0, -1.57, -1.0, 0.7, 0, -0.5, 0.7, -1.0, -0.3, 0.5, -0.5, 0, 0, 0, -0.3, 0.5, -0.5, 0, 0, 0] waypoints.data = [-1.57, 0, 0] waypoints.layout.dim[0].size = len(waypoints.data) rospy.loginfo(waypoints) pub.publish(waypoints) rate.sleep() print("\nTask completed\n") rospy.signal_shutdown("Task completed")
def main(): global pub rospy.init_node('go_to_point') pub = rospy.Publisher('/diff_drive_controller/cmd_vel', Twist, queue_size=1) status_pub = rospy.Publisher('/move_bot/status', Bool, queue_size=1) sub_odom = rospy.Subscriber('/odom',Odometry, clbk_odom) target_sub = rospy.Subscriber('/move_bot/goal',Pose, clbk_goal) mat = Float64MultiArray() mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim.append(MultiArrayDimension()) gripper_pub = rospy.Publisher('/gripper_controller/command', Float64MultiArray, queue_size=1) rate =rospy.Rate(200) extend = 0 gripper_extend = 0 global message_arrived while not rospy.is_shutdown(): if message_arrived: if state_ == 0: fix_yaw(desired_position_) elif state_ == 1: go_straight_ahead(desired_position_) elif state_ == 2: orient_yaw(yaw_required) elif state_ ==3: done() change_state(0) message_arrived = False status = True status_msg = Bool() status_msg.data = status status_pub.publish(status_msg) print("done")
def __init__(self): # Define the dimensions of the message rightHandGoalDim = MultiArrayDimension() rightHandGoalDim.size = NUM_DOFS rightHandGoalDim.label = "rightHandGoal" rightHandGoalDim.stride = 1 # Define the goal messages self.rightHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.rightHandGoalMsg.data.append(0) self.rightHandGoalMsg.layout.dim.append(rightHandGoalDim) self.rightHandGoalMsg.layout.data_offset = 0 leftHandGoalDim = MultiArrayDimension() leftHandGoalDim.size = NUM_DOFS leftHandGoalDim.label = "leftHandGoal" leftHandGoalDim.stride = 1 self.leftHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.leftHandGoalMsg.data.append(0) self.leftHandGoalMsg.layout.dim.append(leftHandGoalDim) self.leftHandGoalMsg.layout.data_offset = 0
def callback(self, joint_state): if len(self.joint_position) == 0: self.joint_position = np.array(joint_state.position) else: if np.all(self.joint_position == joint_state.position): return else: self.joint_position = copy.deepcopy(joint_state.position) print self.joint_position mat = UInt16MultiArray() mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim[0].label = "servo_number" mat.layout.dim[0].size = 6 mat.layout.dim[0].stride = 3 * 6 mat.layout.dim[1].label = "single_command" mat.layout.dim[1].size = 3 mat.layout.dim[1].stride = 3 mat.layout.data_offset = 0 mat.data = [ 1, 1500, 1000, 2, 1500, 1000, 3, 1500, 1000, 4, 1500, 1000, 5, 1500, 1000, 6, 1500, 1000 ] dstride1 = mat.layout.dim[1].stride offset = mat.layout.data_offset for i in range(6): if i == 1 or i == 2: PMW_value = -self.joint_position[i] / 1.5708 * 1000 + 1500 else: PMW_value = self.joint_position[i] / 1.5708 * 1000 + 1500 mat.data[offset + 1 + i * dstride1] = PMW_value print mat self.PMW_value_pub.publish(mat)
def image_to_saliency(t, saliency, saliency_pub, saliency_image_pub, bridge, image, last_time, elapsed): if t < 1.0: return if image.value is None: return if last_time.value is None: last_time.value = t current_time = t dt = current_time - last_time.value last_time.value = current_time elapsed.value = elapsed.value + dt if elapsed.value < 0.01: return else: elapsed.value = 0. image = bridge.value.imgmsg_to_cv2(image.value, "bgr8") saliency_map = saliency.value.compute_saliency_map(image) saliency_map_image = bridge.value.cv2_to_imgmsg( np.uint8(saliency_map * 255.), "mono8") saliency_image_pub.value.publish(saliency_map_image) from std_msgs.msg import Float32MultiArray, MultiArrayDimension, MultiArrayLayout height = MultiArrayDimension(size=len(saliency_map)) width = MultiArrayDimension(size=len(saliency_map[0])) lo = MultiArrayLayout([height, width], 0) saliency_pub.value.publish( Float32MultiArray(layout=lo, data=saliency_map.flatten()))
def image_to_saliency(t, image, bridge, saliency, saliency_pub, saliency_image_pub, points, camera_model, camera_info_left, last_time, elapsed, pan, tilt, saliency_map): if image.value is None or camera_info_left.value is None: return if last_time.value is None: last_time.value = t current_time = t dt = current_time - last_time.value last_time.value = current_time elapsed.value = elapsed.value + dt if elapsed.value < 0.01: return else: elapsed.value = 0. if saliency_map.value is None: image = bridge.value.imgmsg_to_cv2(image.value, "bgr8") image = np.zeros(image.shape) saliency_map.value = saliency.value.compute_saliency_map(image) saliency_map_current = saliency_map.value.copy() # apply curiosity camera_model.value.fromCameraInfo(camera_info_left.value) for point in points.value: # call service pixel = camera_model.value.project3dToPixel( (point.point.x - pan.value, point.point.y - tilt.value, point.point.z)) x = int(pixel[0] * (len(saliency_map_current[0]) / float(camera_info_left.value.width))) x = x + 6 # correction, bug in opencv? y = int( pixel[1] * (len(saliency_map_current) / float(camera_info_left.value.height))) if x >= 0 and x < len(saliency_map_current[0]) and y >= 0 and y < len( saliency_map_current): from skimage.draw import circle rr, cc = circle( y, x, 25, (len(saliency_map_current), len(saliency_map_current[0]))) saliency_map[rr, cc] = saliency_map[rr, cc] * min( 1, (t - point.header.stamp.to_sec())) saliency_map_image = bridge.value.cv2_to_imgmsg( np.uint8(saliency_map_current * 255.), "mono8") saliency_image_pub.value.publish(saliency_map_image) from std_msgs.msg import Float32MultiArray, MultiArrayDimension, MultiArrayLayout height = MultiArrayDimension(size=len(saliency_map_current)) width = MultiArrayDimension(size=len(saliency_map_current[0])) lo = MultiArrayLayout([height, width], 0) saliency_pub.value.publish( Float32MultiArray(layout=lo, data=saliency_map_current.flatten())) return
def think(msgs): global curx, cury, positions, r print(msgs.data) data = [int(x) for x in msgs.data] idxs = sorted(range(9), key=lambda k:data[k]) mat = Float32MultiArray() mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim[0].label = "height" mat.layout.dim[1].label = "width" mat.layout.dim[0].size = 2 mat.layout.dim[1].size = 9 mat.data = [0]*18 print(idxs) for i in range(len(idxs)): pos = positions[idxs[i]] print(pos) tx = pos[0] - curx ty = pos[1] - cury curx = pos[0] cury = pos[1] mat.data[i * 2] = tx mat.data[i * 2 + 1] = ty print(mat) pub.publish(mat) r.sleep()
def demo(): ''' Publish sample data to ROS ''' # Read demo binvox as (64*64*64) array with open('demo/occupy.binvox', 'rb') as f: occ = binvox_rw.read_as_3d_array(f).data with open('demo/non_occupy.binvox', 'rb') as f: non = binvox_rw.read_as_3d_array(f).data msg = ByteMultiArray() msg.data = (occ.astype(int) - non.astype(int)).flatten().tolist() msg.layout.dim.append( MultiArrayDimension(label='x', size=DIM, stride=DIM * DIM * DIM)) msg.layout.dim.append( MultiArrayDimension(label='y', size=DIM, stride=DIM * DIM)) msg.layout.dim.append(MultiArrayDimension(label='z', size=DIM, stride=DIM)) rospy.init_node('shape_demo_loader') pub = rospy.Publisher('local_occupancy', numpy_msg(ByteMultiArray), queue_size=10) rospy.Subscriber("local_occupancy_predicted", numpy_msg(ByteMultiArray), callback) time.sleep(5) pub.publish(msg) rospy.spin()
def im_callback(self, data): outputs, im = self.objsegmenter.segment(data, y_crop_idx=650) labels = self.objsegmenter.get_labels(outputs) box_list = np.asarray( self.objsegmenter.get_boxes(outputs).tensor.tolist()) if self.visualize: out = self.objsegmenter.draw_output(outputs, im) # Publish segmented image self.image_pub.publish(cv2_to_imgmsg(out.get_image(), 'bgr8')) # Publish segmentation data as Segmentation message seg_msg = Segmentation() # Add labels seg_msg.classes = labels # Flatten data into 1d array seg_msg.boxes.data = list(box_list.flatten()) # Populate layout info dims = box_list.shape if len(dims) > 1: seg_msg.boxes.layout.dim = [ MultiArrayDimension(), MultiArrayDimension() ] seg_msg.boxes.layout.dim[0].label = 'classes' seg_msg.boxes.layout.dim[0].size = dims[0] seg_msg.boxes.layout.dim[0].stride = dims[0] * dims[1] seg_msg.boxes.layout.dim[1].label = 'bbox coordinates' seg_msg.boxes.layout.dim[1].size = dims[1] seg_msg.boxes.layout.dim[1].stride = dims[1] print(seg_msg.classes) print(seg_msg.boxes.data) else: print('No objects found') self.segment_pub.publish(seg_msg)
def vox_to_float_array(voxel_grid, dim): out_msg = Float32MultiArray() out_msg.data = voxel_grid.astype(np.float32).flatten().tolist() out_msg.layout.dim.append(MultiArrayDimension(label='x', size=dim, stride=dim*dim*dim)) out_msg.layout.dim.append(MultiArrayDimension(label='y', size=dim, stride=dim*dim)) out_msg.layout.dim.append(MultiArrayDimension(label='z', size=dim, stride=dim)) return out_msg
def determine_vision_pattern(self): resolution = self.occ_grid.info.resolution sensor_range = self.sensor.range_max dimension = 2 * int( np.round(math.ceil((sensor_range) / resolution - 0.5))) + 1 self.vision_pattern.data = [] self.vision_pattern.layout.dim.append(MultiArrayDimension()) self.vision_pattern.layout.dim.append(MultiArrayDimension()) self.vision_pattern.layout.dim[0].size = dimension self.vision_pattern.layout.dim[1].size = dimension self.vision_pattern.layout.dim[0].label = "rows" self.vision_pattern.layout.dim[1].label = "cols" self.vision_pattern.layout.data_offset = 0 centre_index = (dimension + 1) / 2 - 1 # -1 since index starts at 0 for i in range(0, dimension): for j in range(0, dimension): distance = resolution * np.sqrt((i - centre_index)**2 + (j - centre_index)**2) if distance <= sensor_range: self.vision_pattern.data.append(1) else: self.vision_pattern.data.append(0)
def handle_p2b(req): global theta print("here") theta = list(req.configs.angles) delta = 0.0001 f_original = computeH(0, 0, 0, 0) f_theta1 = computeH(delta, 0, 0, 0) f_theta2 = computeH(0, delta, 0, 0) f_theta3 = computeH(0, 0, delta, 0) f_theta4 = computeH(0, 0, 0, delta) J1 = (f_theta1 - f_original) / delta J2 = (f_theta2 - f_original) / delta J3 = (f_theta3 - f_original) / delta J4 = (f_theta4 - f_original) / delta jacobian = Float64MultiArray() jacobian.layout.dim = [ MultiArrayDimension('height', 4, 3 * 4), MultiArrayDimension('width', 3, 3) ] jacobian.data = [ J1[0], J2[0], J3[0], J4[0], J1[1], J2[1], J3[1], J4[1], J1[2], J2[2], J3[2], J4[2] ] return jacobian
def callback(data): #rospy.loginfo(data) rownum=data.layout.dim[0].size columnnum=data.layout.dim[1].size dataset=data.data subarrayset=[] #count=0 for i in range(0,rownum-1): temp=[] for k in range(0,columnnum-1): temp.append(dataset[rownum*i+k]) (x,y,z)=findsubarray(temp) #rospy.loginfo((x,y,z)) for j in range(x,y): subarrayset.append(dataset[rownum*i+j]) #count=count+y-x+1 #rospy.loginfo(count) #rospy.loginfo(subarrayset) (begin,end,summary)=findsubarray(subarrayset) rospy.loginfo((begin,end,summary)) outputdata=[begin,end] pub=rospy.Publisher('/hw1/subarray', UInt32MultiArray, queue_size=10) layout = MultiArrayLayout() layout.dim = [MultiArrayDimension('height', 1, 2 * 1), MultiArrayDimension('width', 2, 2)] pub.publish(layout, outputdata)
def omni_talker(): pub = rospy.Publisher('Omni_data', Float32MultiArray, queue_size=10) rospy.init_node('Omni_talker', anonymous=False) rate = rospy.Rate(19) # 11hz while not rospy.is_shutdown(): # random data for debug and test _Omni_data = Float32MultiArray() _Omni_data.data = np.random.random((3,3)).astype(np.float32).reshape([9]) _Omni_data.layout.data_offset = 0 # create two dimensions in the dim array _Omni_data.layout.dim = [MultiArrayDimension(), MultiArrayDimension()] ''' The stride in these dimensions refers to how many samples along the next higher dimension is. So in the case of the samples, there is a stride of 4096 to the next sample. In the case of dimension zero there is no higher dimension so the stride is the overall size of the message data. ''' # dim[0] is the vertical dimension of your matrix _Omni_data.layout.dim[0].label = "vertical" _Omni_data.layout.dim[0].size = 3 _Omni_data.layout.dim[0].stride = 3 # dim[1] is the horizontal dimension of your matrix _Omni_data.layout.dim[1].label = "horizontal" _Omni_data.layout.dim[1].size = 3 _Omni_data.layout.dim[1].stride = 3 rospy.loginfo("python: Omni_data sent") # rospy.loginfo(_Omni_data) # for test only pub.publish(_Omni_data) rate.sleep()
def test_strify_message(self): from genpy.message import Message, strify_message, fill_message_args def roundtrip(m): yaml_text = strify_message(m) print(yaml_text) loaded = yaml.load(yaml_text) print("loaded", loaded) new_inst = m.__class__() if loaded is not None: fill_message_args(new_inst, [loaded]) else: fill_message_args(new_inst, []) return new_inst # The following tests have not been ported to genpy yet # test array of Messages field. We can't use M4 or M5 because fill_message_args has to instantiate the embedded type from test_roslib_comm.msg import ArrayOfMsgs from std_msgs.msg import String, Time, MultiArrayLayout, MultiArrayDimension dims1 = [MultiArrayDimension(*args) for args in [('', 0, 0), ('x', 1, 2), ('y of z', 3, 4)]] dims2 = [MultiArrayDimension('hello world', 91280, 1983274)] times = [Time(genpy.Time(*args)) for args in [(0,), (12345, 6789), (1, 1)]] val = ArrayOfMsgs([String(''), String('foo'), String('bar of soap')], times, [MultiArrayLayout(dims1, 0), MultiArrayLayout(dims2, 12354)], ) self.assertEquals(val, roundtrip(val))
def publish_bounding_boxes(self, bbs, image_width, image_height): # Given a list of bounding boxes, publish same. # The detector returns an array of dlib.rectangle's. if len(bbs) > 0: msg = Int32MultiArray() msg.layout.data_offset = 0 msg.layout.dim.append( MultiArrayDimension(label='height', size=len(bbs) + 1, stride=4 * (len(bbs) + 1))) msg.layout.dim.append( MultiArrayDimension(label='width', size=4, stride=1)) msg.layout.dim.append( MultiArrayDimension(label='channel', size=1, stride=1)) data = [] # The first entry is the size of the source image data.append(0) data.append(0) data.append(image_width) data.append(image_height) for rect in bbs: data.append(rect.left()) data.append(rect.top()) data.append(rect.right()) data.append(rect.bottom()) msg.data = data self.bounding_box_publisher.publish(msg) return
def publish(self, rgb, temp_array): # Publish RGB image rgb_img_msg = self.bridge.cv2_to_imgmsg(rgb) rgb_img_msg.header.frame_id = self.evo_thermal_frame rgb_img_msg.header.stamp = rospy.Time.now() self.rgb_publisher.publish(rgb_img_msg) # Publish temperatures array raw_temp_array = Float64MultiArray() raw_temp_array.layout.dim.append(MultiArrayDimension()) raw_temp_array.layout.dim[0].size = 32 raw_temp_array.layout.dim[0].stride = 1 raw_temp_array.layout.dim[0].label = "x-axis" raw_temp_array.layout.dim.append(MultiArrayDimension()) raw_temp_array.layout.dim[1].size = 32 raw_temp_array.layout.dim[1].stride = 1 raw_temp_array.layout.dim[1].label = "y-axis" data = np.reshape(temp_array, (1024)) np.round(data, 2) raw_temp_array.data = data self.raw_publisher.publish(raw_temp_array) # Publish internal sensor temperature self.ptat_publisher.publish(self.ptat / self.scaling - self.celsius_offset)
def __init__(self): #Initialize arbotix comunications print"\nArbotix initialization, wait 10 seconds..." ArbotiX.__init__(self) for x in xrange(1,21): time.sleep(0.5) print(str(x*0.5) + "/10s") #Setupr velocities and positions vectors and messages self.joints_poses = [0,0,0,0,0,0] self.joints_vels = [0,0,0,0,0] self.ee_closed = 0 self.vels_to_pub = Float32MultiArray() self.poses_to_pub = Float32MultiArray() self.poses_layout = MultiArrayDimension('joints_poses', 6, 0) self.vels_layout = MultiArrayDimension('joints_vels', 5, 0) self.poses_to_pub.layout.dim = [self.poses_layout] self.poses_to_pub.layout.data_offset = 0 self.vels_to_pub.layout.dim = [self.vels_layout] self.vels_to_pub.layout.data_offset = 0 #ROS pubblisher for joint velocities and positions self.pos_pub = rospy.Publisher('/windowx_3links/joints_poses', Float32MultiArray, queue_size=1) self.vel_pub = rospy.Publisher('/windowx_3links/joints_vels', Float32MultiArray, queue_size=1) print"Windowx_3link node created, whaiting for messages in:" print" windowx_2links/torque" print"Publishing joints' positions and velocities in:" print" /windowx_3links/joints_poses" print" /windowx_3links/joints_vels" #Start publisher self.publish()
def to_multiarray(img_shape, boxes, scores, classes): # image_shape : the shape of the image used by yolo # boxes: An `array` of shape (num_boxes, 4) containing box corners as # (y_min, x_min, y_max, x_max). # `scores`: A `list` of scores for each box. # classes: A `list` of indicies into `class_names`. # return: multiarray data = [] for i in range(len(boxes)): y_min = boxes[i][0] / img_shape[0] x_min = boxes[i][1] / img_shape[1] y_max = boxes[i][2] / img_shape[0] x_max = boxes[i][3] / img_shape[1] confidence = scores[i] _class = classes[i] data.append(float(_class)) data.append(float(y_min)) data.append(float(x_min)) data.append(float(y_max)) data.append(float(x_max)) data.append(float(confidence)) message = Float32MultiArray() message.layout.dim.append(MultiArrayDimension("boxes", len(boxes), 1)) message.layout.dim.append(MultiArrayDimension("box_data", 6, 1)) message.data = data return message
def bno055_pub(): rospy.init_node('bno055_pub', anonymous=True) pub = rospy.Publisher('bno055_state', Float32MultiArray, queue_size=1) rate = rospy.Rate(10) # 10hz bno_controller = AdafruitBno055Controller() mat = Float32MultiArray() mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim[0].label = "height" mat.layout.dim[1].label = "width" mat.layout.dim[0].size = 3 mat.layout.dim[1].size = 1 mat.layout.dim[0].stride = 3 * 1 #note dim[0] stride is just size of image mat.layout.dim[1].stride = 1 mat.layout.data_offset = 0 mat.data = [0] * 3 while not rospy.is_shutdown(): x, y, z = bno_controller.gravity() gravity_str = "gravity is{}".format([x, y, z]) rospy.loginfo(gravity_str) mat.data[0] = x mat.data[1] = y mat.data[2] = z pub.publish(mat) rate.sleep()
def __init__(self, name): self.name = name self.position_stddev = 0.50 # [m] self.ref_frame = "/map" rospy.init_node('kf_example') self.position_stddev = rospy.get_param("~position_stddev_m", self.position_stddev) rospy.loginfo("Starting KF Example") self.array = Float64MultiArray() self.array.layout = MultiArrayLayout() self.array.layout.data_offset = 0 self.array.layout.dim = [MultiArrayDimension('data', 3, 3)] self.array.data = [0.0] * 3 self.array2 = Float64MultiArray() self.array2.layout = MultiArrayLayout() self.array2.layout.data_offset = 0 self.array2.layout.dim = [MultiArrayDimension('measure', 2, 2)] self.array2.data = [0.0] * 2 # Initialisation of the matrices self.A = numpy.mat([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) self.P = numpy.mat([[1, 0, 0], [0, 1, 0], [0, 0, 3.5]]) self.Q = numpy.mat([[pow(0.5, 2), 0, 0], [0, pow(0.5, 2), 0], [0, 0, 0.001]]) self.H = numpy.mat([[1, 0, 0], [0, 1, 0]]) self.R = numpy.mat([[0.1, 0], [0, 0.1]]) self.U = numpy.vstack([0.0, 0.0]) # State is x, y, theta self.state = numpy.vstack([0.0, 0.0, 0.0]) self.last_update = rospy.Time.now()
def default(self, ci='unused'): arrayList = [] arrayList.append(self.data['w_fl']) arrayList.append(self.data['w_fr']) arrayList.append(self.data['w_rl']) arrayList.append(self.data['w_rr']) dim_fl = MultiArrayDimension() dim_fl.size = sys.getsizeof(self.data['w_fl']) dim_fl.label = 'FrontLeftWheelSpeed' dim_fl.stride = sys.getsizeof(self.data['w_fl']) dim_fr = MultiArrayDimension() dim_fr.size = sys.getsizeof(self.data['w_fr']) dim_fr.label = 'FrontRightWheelSpeed' dim_fr.stride = sys.getsizeof(self.data['w_fr']) dim_rl = MultiArrayDimension() dim_rl.size = sys.getsizeof(self.data['w_rl']) dim_rl.label = 'RearLeftWheelSpeed' dim_rl.stride = sys.getsizeof(self.data['w_rl']) dim_rr = MultiArrayDimension() dim_rr.size = sys.getsizeof(self.data['w_rr']) dim_rr.label = 'RearRightWheelSpeed' dim_rr.stride = sys.getsizeof(self.data['w_rr']) wheelspeed = Float64MultiArray() wheelspeed.data = arrayList wheelspeed.layout.dim.append(dim_fl) wheelspeed.layout.dim.append(dim_fr) wheelspeed.layout.dim.append(dim_rl) wheelspeed.layout.dim.append(dim_rr) wheelspeed.layout.data_offset = 0 self.publish(wheelspeed)
def reconstruction(data): arduino_data = data.data # recosntruction #just to test dummy reconstruction #have to delete when the actual reconstruction is implemented arduino_data = np.array(arduino_data) arduino_data = arduino_data + 0.12 #reconstruction end here # send message mat_size = m * n reconstruction_data = Float32MultiArray() reconstruction_data.layout.dim.append(MultiArrayDimension()) reconstruction_data.layout.dim.append(MultiArrayDimension()) reconstruction_data.layout.dim[0].label = "W" reconstruction_data.layout.dim[1].label = "H" reconstruction_data.layout.dim[0].size = m reconstruction_data.layout.dim[1].size = n reconstruction_data.layout.dim[0].stride = m * n reconstruction_data.layout.dim[1].stride = m reconstruction_data.layout.data_offset = 0 reconstruction_data.data = arduino_data #[0]* mat_size print("--------Start of Reconstruction data------") rospy.loginfo(reconstruction_data) print("---------End of Reconstruction data ------") pub.publish(reconstruction_data)
def publish_depth(self, frames): depth_frame = frames.get_depth_frame() if self.DISPLAY: depth_image = np.asanyarray( self.colorizer.colorize(depth_frame).get_data()) cv2.imshow("Original Depth", depth_image) cv2.waitKey(2) depth = np.asanyarray(depth_frame.get_data(), dtype=np.float64) depth = self.crop_depth(depth) # Create depth Float64MultiArray message to publish depth_data = Float64MultiArray() dim1 = MultiArrayDimension(label="height", size=depth.shape[0]) dim2 = MultiArrayDimension(label="width", size=depth.shape[1]) depth_data.layout.dim.extend([dim1, dim2]) depth_data.data = depth.flatten() # Display Cropped Image if self.DANIEL_DEBUG and not self.DISPLAY: displayed_depth = np.asanyarray( self.colorizer.colorize(depth_frame).get_data()) cv2.imshow("Published Depth", self.crop_depth(displayed_depth)) cv2.waitKey(2) self.safe_publish(self.depth_pub, depth_data)
def talker(mymode): if mymode=='float64': pub = rospy.Publisher('codio', array64, queue_size=1) # mydata=([[0, 0, 1, 250],[0, 1, 0, 15],[-1, 0, 0, -21]]) mydata=([0, 0, 1, 250,0, 1, 0, 15,-1, 0, 0, -21]) mymsg = array64() # mymsg.data=mydata.reshape([12]) mymsg.data=mydata # mymsg.data = ([1.0, 2.0],[3.0, 4.0]) mymsg.layout.data_offset = 0 mymsg.layout.dim = [MultiArrayDimension(), MultiArrayDimension()] # dim[1] is the vertical dimension of your matrix mymsg.layout.dim[0].label = "row" mymsg.layout.dim[0].size = 3 mymsg.layout.dim[0].stride = 12 # dim[1] is the horizontal dimension of your matrix mymsg.layout.dim[1].label = "col" mymsg.layout.dim[1].size = 4 mymsg.layout.dim[1].stride = 4 else: pub = rospy.Publisher('codio', Pose, queue_size=1) mymsg=Pose() mymsg.position.x=1 mymsg.position.y=1 mymsg.position.z=1 mymsg.orientation.x=1 mymsg.orientation.y=1 mymsg.orientation.z=1 mymsg.orientation.w=1 rospy.init_node('talker', anonymous=True) rate = rospy.Rate(1) # 1hz while not rospy.is_shutdown(): pub.publish(mymsg) rate.sleep()
def test_dictionary_with_child_message(self): from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension expected_message = Float64MultiArray(layout=MultiArrayLayout( dim=[ MultiArrayDimension(label='Dimension1', size=12, stride=7), MultiArrayDimension(label='Dimension2', size=90, stride=8) ], data_offset=1), data=[1.1, 2.2, 3.3]) dictionary = { 'layout': { 'dim': [{ 'label': expected_message.layout.dim[0].label, 'size': expected_message.layout.dim[0].size, 'stride': expected_message.layout.dim[0].stride }, { 'label': expected_message.layout.dim[1].label, 'size': expected_message.layout.dim[1].size, 'stride': expected_message.layout.dim[1].stride }], 'data_offset': expected_message.layout.data_offset }, 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Float64MultiArray', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_ros_message_with_child_message(self): from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension expected_dictionary = { 'layout': { 'dim': [{ 'label': 'Dimension1', 'size': 12, 'stride': 7 }, { 'label': 'Dimension2', 'size': 24, 'stride': 14 }], 'data_offset': 0 }, 'data': [1.1, 2.2, 3.3] } dimension1 = MultiArrayDimension( label=expected_dictionary['layout']['dim'][0]['label'], size=expected_dictionary['layout']['dim'][0]['size'], stride=expected_dictionary['layout']['dim'][0]['stride']) dimension2 = MultiArrayDimension( label=expected_dictionary['layout']['dim'][1]['label'], size=expected_dictionary['layout']['dim'][1]['size'], stride=expected_dictionary['layout']['dim'][1]['stride']) layout = MultiArrayLayout( dim=[dimension1, dimension2], data_offset=expected_dictionary['layout']['data_offset']) message = Float64MultiArray(layout=layout, data=expected_dictionary['data']) message = serialize_deserialize(message) dictionary = message_converter.convert_ros_message_to_dictionary( message) self.assertEqual(dictionary, expected_dictionary)
def take_pictures_test(self): #input: None #output: None #Description: Used for validating, AI is working with camera or not #Getting the bird eye view of the image #persImg = perspective(cv_image) persImg = skimage.io.imread(INPUT_TEST_IMAGE_PATH) objCnt,objCord = self.weedLocation(persImg) #Making up the object location data in format for publishing if objCnt !=0: objCord = np.reshape(objCord,(objCnt*3,)) #reshaping in the form of single column objCord = np.ndarray.tolist(objCord) else: objCord = np.reshape(objCord,(3,)) #reshaping in the form of single column objCord = np.ndarray.tolist(objCord) msg = Float32MultiArray() msg.layout.dim.append(MultiArrayDimension()) msg.layout.dim.append(MultiArrayDimension()) msg.layout.dim[0].label = "row" msg.layout.dim[1].label = "col" msg.layout.dim[0].size = objCnt msg.layout.dim[1].size = 1 msg.layout.dim[0].stride = 3 msg.layout.dim[1].stride = 1 msg.layout.data_offset = 0 msg.data = objCord self.publisher.publish(msg)
def get_example_cov(example_id=0): """ Creates an example covariance matrix. The data in the matrix was acquired from the maven-1.bag file. :param example_id: A constant id of the example in the list of stored examples. Minimum value is 0. Values above the max value will be set to 0 instead. Current maximum value is: 1 :return: The covariance matrix in format std_msgs/Float64MultiArray """ MAX_SAMPLES = 1 if example_id > MAX_SAMPLES: example_id = 0 nan = float("NaN") examples = [] # Here, just copy-paste some more data from maven-1.bag so that you can choose which example you want examples.append([0.2256878004660412, 0.004743161046803848, nan, nan, nan, 0.4799785723084568, 6.694665570936009e-05, 0.004743161046804125, 0.03333087258142175, nan, nan, nan, 0.0064891656361629295, 0.08962944598353237, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 0.47997857230845664, 0.006489165636162725, nan, nan, nan, 2.4285886983849885, -0.06696549234373295, 6.694665570941213e-05, 0.08962944598353195, nan, nan, nan, -0.06696549234373322, 1.424665892676366]) examples.append([0.5758368975539181, -0.10477581457466455, nan, nan, nan, 3.003595362397707, -0.39924730334819275, -0.10477581457466437, 0.24081097327513568, nan, nan, nan, -0.4539729065847597, 1.439606811146181, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 3.003595362397709, -0.45397290658476147, nan, nan, nan, 18.613535338951223, -1.9899796692884495, -0.39924730334819236, 1.4396068111461806, nan, nan, nan, -1.9899796692884442, 10.681110693507879]) # ----------------- cov_list = examples[example_id] # Create Float64MultiArray similar to those in the bag files dim_0 = MultiArrayDimension(label="", size=7, stride=49) dim_1 = MultiArrayDimension(label="", size=7, stride=7) cov_dim = [dim_0, dim_1] cov_layout = MultiArrayLayout(dim=cov_dim, data_offset=0) return Float64MultiArray(layout=cov_layout, data=cov_list)
def __request_callback(self, msg): ''' Callback if image is received, faces are detected and added to the global array of people A message is sent with the ids, locations and face widths in a multiarray. ''' cv_image = self.bridge.imgmsg_to_cv2(msg, 'rgb8') if SHOW_RECEIVED_IMAGE: cv2.imwrite('rec_img.png', cv_image) ret = self.fr.recognize_people(cv_image) simple_msg = Float32MultiArray() if ret: print('new face detected') locs, ids = ret for i, loc in zip(ids, locs): if i in self.recognized_ids: index = self.recognized_ids.index(i) self.recognized_locs[index] = loc self.recognized_times[index] = datetime.datetime.now() else: self.recognized_ids.append(i) self.recognized_locs.append(loc) self.recognized_times.append(datetime.datetime.now()) else: print('No faces detected') # Publish the computed info on the simple_people topic # as a multiarray. The structure is defined as [id, xpos, ypos, width] # in a repeated fashion for multiple faces. # The width might be used as a metric for how far the face is away dim = MultiArrayDimension() dim.label = 'id_and_pos' dim.size = 3 dim.stride = 0 dim2 = MultiArrayDimension() dim2.label = 'length_faces' dim2.size = len(self.recognized_ids) dim2.stride = 0 simple_msg.layout.dim = [dim, dim2] simple_msg.layout.data_offset = 0 data = [] # Add the recognized faces around the table to the message for i, loc in zip(self.recognized_ids, self.recognized_locs): width = self.__get_width(loc) x, y, valid = self.__calculate_people_positions(width) data = np.concatenate((data, [i, x, y, width]), axis=0) simple_msg.data = data print(simple_msg) # Publish the message self.recognition_pub.publish(simple_msg) # Check if person is seen in the last TIME_BEFORE_FORGOTTEN seconds and otherwise remove for index, time in enumerate(self.recognized_times): if abs((time - datetime.datetime.now() ).total_seconds()) > TIME_BEFORE_FORGOTTEN: del self.recognized_ids[index] del self.recognized_locs[index] del self.recognized_times[index]