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 gen_layout(shape): layout = MultiArrayLayout() layout.dim = [ gen_dim('dim{}'.format(i), size, 1) for i, size in enumerate(shape) ] layout.data_offset = 0 return layout
def callback(msg, prevMarkers): detectedMarkers = msg.markers # The current time in seconds now = rospy.get_time() for detectedMarker in detectedMarkers: measuredTime = detectedMarker.header.stamp.secs markerID = detectedMarker.id prevMarkers[markerID] = measuredTime detected_markers = list() for marker in prevMarkers.keys(): if abs(prevMarkers[marker] - now) > 5: del prevMarkers[marker] else: detected_markers.append(marker) array = MultiArrayDimension() array.label = 'numMarkers' array.size = len(detected_markers) array.size = len(detected_markers) layout = MultiArrayLayout() layout.dim = [ array, ] layout.data_offset = 0 msg = Int16MultiArray() msg.layout = layout msg.data = detected_markers pub.publish(msg)
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 callback(msg,prevMarkers): detectedMarkers = msg.markers now = rospy.get_time() #the current time in seconds for detectedMarker in detectedMarkers: measuredTime = detectedMarker.header.stamp.secs markerID = detectedMarker.id prevMarkers[markerID] = measuredTime detected_markers = [] for marker in prevMarkers.keys(): if abs(prevMarkers[marker]-now) > 5: #if the measurement has been stale for 5 seconds del prevMarkers[marker] else: detected_markers.append(marker) array1 = MultiArrayDimension() array1.label = 'numMarkers' array1.size = len(detected_markers) array1.size = len(detected_markers) layout = MultiArrayLayout() layout.dim = [array1,] layout.data_offset = 0 msg = Int16MultiArray() msg.layout = layout msg.data = detected_markers pub.publish(msg)
def publish(self): # compose the multiarray message jointVelocities = Float32MultiArray() myLayout = MultiArrayLayout() myMultiArrayDimension = MultiArrayDimension() myMultiArrayDimension.label = "joint_velocities" myMultiArrayDimension.size = 1 myMultiArrayDimension.stride = self.numOfWheels myLayout.dim = [myMultiArrayDimension] myLayout.data_offset = 0 jointVelocities.layout = myLayout if rospy.get_time() - self.lastTwistTime < self.timeout: self.right = 1.0 * self.linearVelocity + self.angularVelocity * self.baseWidth / 2 self.left = 1.0 * self.linearVelocity - self.angularVelocity * self.baseWidth / 2 rospy.loginfo("wheels velocities vl, vr [{0:.3f},{1:.3f}]".format(self.left, self.right)) if self.numOfWheels == 2: # first item is left and second is right jointVelocities.data = [self.left, self.right] elif self.numOfWheels == 4: jointVelocities.data = [self.left, self.right, self.left, self.right] else: if self.numOfWheels == 2: jointVelocities.data = [0.0, 0.0] # first item is left and second is right elif self.numOfWheels == 4: jointVelocities.data = [0.0, 0.0, 0.0, 0.0] self.joint_velocities_Pub.publish(jointVelocities)
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 main(args): if len(sys.argv) < 2: print 'Please specify hokuyo_4m file' return 1 if len(sys.argv) < 3: print 'Please specify output rosbag file' return 1 # hokuyo_4m always has 726 hits num_hits = 726 f_bin = open(sys.argv[1], "r") bag = rosbag.Bag(sys.argv[2], 'w') try: while True: # check for eof data = f_bin.read(8) if data == '': break # Read timestamp utime = struct.unpack('<Q', data)[0] r = np.zeros(num_hits) for i in range(num_hits): s = struct.unpack('<H', f_bin.read(2))[0] r[i] = convert(s) # Now write out to rosbag timestamp = rospy.Time.from_sec(utime / 1e6) layout = MultiArrayLayout() layout.dim = [MultiArrayDimension()] layout.dim[0].label = "r" layout.dim[0].size = num_hits layout.dim[0].stride = 1 hits = Float64MultiArray() hits.data = r hits.layout = layout bag.write('hokuyo_4m_packet', hits, t=timestamp) except Exception: print 'End of File' finally: f_bin.close() bag.close() return 0
def sendCommand(self, ts_rostime): msg = Float64MultiArray() dim0 = MultiArrayDimension() dim0.label = 'ts_rostime, numOfsamples, dt' dim0.size = 3 layout_var = MultiArrayLayout() layout_var.dim = [dim0] msg.layout = layout_var msg.data = [ts_rostime, self.numOfSamples, self.dt] self.sampleParticalTrajectory_pub.publish(msg)
def init_multdata_NEW(self, data): msg = MultiArrayLayout() msg.data_offset = 0 msg.dim = [MultiArrayDimension()] msg.dim[0].label = "state_estimation" msg.dim[0].size = data.shape[0] msg.dim[0].stride = data.shape[0] return msg
def init_multdata(self): msg = MultiArrayLayout() msg.data_offset = 0 msg.dim = [MultiArrayDimension()] msg.dim[0].label= "Quat" msg.dim[0].size = 4 msg.dim[0].stride = 4 return msg
def create_depth_msg(depth_img): command = Float64MultiArray() layout = MultiArrayLayout() dimension = MultiArrayDimension() dimension.label = "depth_img" dimension.size = len(depth_img) dimension.stride = len(depth_img) layout.data_offset = 0 layout.dim = [dimension] command.layout = layout command.data = depth_img return command
def publish(vals): command = Float64MultiArray() layout = MultiArrayLayout() dimension = MultiArrayDimension() dimension.label = "keyboard_control" dimension.size = 4 dimension.stride = 4 layout.data_offset = 0 layout.dim = [dimension] command.layout = layout command.data = vals control_pub.publish(command)
def init_multdata(self): msg = MultiArrayLayout() msg.data_offset = 0 msg.dim = [MultiArrayDimension()] msg.dim[0].label = "state_estimation" msg.dim[0].size = 15 msg.dim[0].stride = 15 return 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 pub(): """ input: none output: none """ # Register a new node and initialization pub = rospy.Publisher("joint_angle_gz_mani", Float32MultiArray, queue_size=1) rospy.init_node('pub') # Initialize for multi dimension array # Each cell of list means the angle of gz_mani's joints dim = [MultiArrayDimension(label = 'Dimension1', size = 1, stride = 7)] data_offset = 1 layout = MultiArrayLayout(dim, data_offset) time = 0 data_flag = 2 data = [0, 0.1, 0, 0, 0, 0, 0] data_1 = [0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] data_2 = [0, 0, 1, 1, 1, -1, 1] # The super loop while not rospy.is_shutdown(): str = "ros time: %s"%rospy.get_time() rospy.loginfo(str) rospy.loginfo(data) topic = Float32MultiArray(layout, data) pub.publish(topic) rospy.sleep(0.1) print 'done'
def convert_np_vector_to_int16_multi_array(vector): assert len(vector.shape) == 1 N = vector.shape[0] msg = Int16MultiArray(layout=MultiArrayLayout( dim=[MultiArrayDimension(label="", size=N, stride=1)], data_offset=0), data=vector.astype(int).tolist()) return msg
def flatten(self): #tran_array = np.split(np.array(self.listener.data),240) for i in self.listener.image_data: tempa,tempb = self.max_array(i) while tempa <= tempb: self.flatten_array.append(i[tempa]) tempa += 1 finala,finalb = self.max_array(self.flatten_array) layout = MultiArrayLayout() layout.dim = [MultiArrayDimension('height', 1, 2*1), MultiArrayDimension('width', 2, 2)] index_data = [finala, finalb] self.maxsub_pub.publish(layout,index_data) print("in the show") rospy.loginfo("calculating the work") print(index_data) rospy.spin()
def __init__(self,robotPath, robotName, timeOut, stdOut, stdErr): self.robot_path = robotPath self.robot_name = robotName self.TIME_OUT = timeOut self.STDOUT = stdOut self.STDERR = stdErr #For internal use self.FINAL_RESULT=[] self.CHECK_ERROR_RESULT = False self.joint_error_subscriber = None self.START_TIME = None self.LAUNCH = None #Set the new goal position self.dim = MultiArrayDimension(label ="x", size = 3, stride = 3) self.layout = MultiArrayLayout(dim = [self.dim] ,data_offset= 0) self.mulitarray = Float64MultiArray(layout = self.layout, data = [0.0, 0.6, 0.8]) #Initialization self.STDOUT = self.Set_shell_output(self.STDOUT) self.STDERR = self.Set_shell_output(self.STDERR) self.ROSCORE = subprocess.Popen(["roscore"],stdin=None, stdout=self.STDOUT, stderr=self.STDERR, shell=False) rospy.init_node('Hello_Testing', anonymous=True)
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 __init__(self, node_name_in_twi="twist_to_motors"): rospy.init_node(node_name_in_twi) self.nodename = rospy.get_name() rospy.loginfo("%s started" % self.nodename) self.wheel_radii = rospy.get_param('wheel_radii', 0.254) self.base_width = rospy.get_param('base_width', 0.508) self.vals_vdes = MultiArrayDimension() self.layout_vdes = MultiArrayLayout() self.data_vdes = [] self.vals_vdes.label = "vdes" self.vals_vdes.size = 2 self.vals_vdes.stride = 2 self.layout_vdes.data_offset = 2 self.layout_vdes.dim.append(self.vals_vdes) self.vdesPub = rospy.Publisher('vdes', Float32MultiArray) rospy.Subscriber('twist', Twist, self.twistCallback) self.rate = rospy.get_param("twist_to_motors_rate", 20) self.timeout_ticks = rospy.get_param("~timeout_ticks", 2) self.left = 0 self.right = 0
def listener(): global pub #create the node rospy.init_node('read_sensors', anonymous=True) #create the subscribers rospy.Subscriber("/robot0/left_wheel", Float64, lw_callback) rospy.Subscriber("/robot0/right_wheel", Float64, rw_callback) rospy.Subscriber("/robot0/beacon", Pose2D, beacon_callback) rospy.Subscriber("/robot0/laser_0", LaserScan, lidar_callback) # infinite loop while True: speed = 8 r = 0.1 L = 0.2 # while we haven't hit a wall, go straight if hit_wall() != True: w1 = speed w2 = speed # while we are hitting a wall, turn left else: w1 = speed w2 = -speed data = Float64MultiArray(data=[]) data.layout = MultiArrayLayout() data.layout.dim = [MultiArrayDimension()] data.layout.dim[0].label = "Parameters" data.layout.dim[0].size = 4 data.layout.dim[0].stride = 1 data.data = [w1, w2, r, L] pub.publish(data) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
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 init_ros_topics(self): # ROS topic: retrieve frames rospy.Subscriber(self.cam1_topic, Image, self.cam1_callback) rospy.Subscriber(self.cam2_topic, Image, self.cam2_callback) # ROS topic: to publish image result self.pub1 = rospy.Publisher(self.pub1_topic, Image, queue_size=10) self.pub2 = rospy.Publisher(self.pub2_topic, Image, queue_size=10) # ROS topic: to publish 3D object location estimation and its dimensions self.markerPub = rospy.Publisher('estimatedObject', PointStamped, queue_size=10) self.dimPub = rospy.Publisher('estimatedDimensions', Float64MultiArray, queue_size=10) self.state = PointStamped() self.state.header.frame_id = "/board" self.dimensions = Float64MultiArray() self.dimensions.layout = MultiArrayLayout() self.dimensions.layout.data_offset = 0 self.dimensions.layout.dim = [MultiArrayDimension()] self.dimensions.layout.dim[0].size = 4 self.dimensions.data = [0, 0, 0, 0]
def callback(config, level): # rospy.loginfo("""Reconfigure Request: {Motor_Master_Switch}, {motor_1}, {motor_2}, {motor_3}, {motor_4},\ # {Controller_ON_OFF}, {Roll_SetPoint}, {Pitch_SetPoint}, {Yaw_SetPoint}, {Pitch_Kp}, {Pitch_Kd}, {Pitch_Ki},\ # {Roll_Kp}, {Roll_Kd}, {Roll_Ki}, {Yaw_Kp}, {Yaw_Kd}, {Yaw_Ki},""".format(**config)) # return config global pub if not rospy.is_shutdown(): multfloatlayout = MultiArrayLayout([MultiArrayDimension('parameters',18,18)],0) multfloat = Float32MultiArray(multfloatlayout, [float(config.Motor_Master_Switch),\ config.motor_1,\ config.motor_2,\ config.motor_3,\ config.motor_4,\ float(config.Controller_ON_OFF),\ config.Roll_SetPoint,\ config.Pitch_SetPoint,\ config.Yaw_SetPoint,\ config.Pitch_Kp,\ config.Pitch_Kd,\ config.Pitch_Ki,\ config.Roll_Kp,\ config.Roll_Kd,\ config.Roll_Ki,\ config.Yaw_Kp,\ config.Yaw_Kd,\ config.Yaw_Ki]) # array_float = [1.0, 2.0, 3.0, 2.0, 1.0, 1.0, 2.0, 3.0, 2.0, 1.0, 1.0, 2.0, 3.0, 2.0, 1.0, 1.0, 2.0, 3.0] rospy.loginfo(multfloat) pub.publish(multfloat) return config
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from std_msgs.msg import MultiArrayLayout self.layout = kwargs.get('layout', MultiArrayLayout()) self.data = array.array('d', kwargs.get('data', []))
def sensor_node(): pub = rospy.Publisher('/sensor_values', Int32MultiArray, queue_size=1) rospy.init_node('sensor_node') rate = rospy.Rate(50) while not rospy.is_shutdown(): with serial.Serial(PORT_NAME, BAUD_RATE, timeout=0.1) as ser: data_canditate = ser.readline().decode('utf-8') # print (data_candidate) data_candidate = ser.readline().decode('utf-8') data_candidate = np.fromstring(data_candidate,sep=' ',dtype=np.uint32) if data_candidate.shape[0] == NUM_SENSORS: values = data_candidate msg = Int32MultiArray( MultiArrayLayout( [MultiArrayDimension('sensor data', NUM_SENSORS, 1)],1),values) # CHANGE the second arg to no. of sensor values read values) pub.publish(msg) else: rospy.signal_shutdown("\n"+"No data available on serial port. Shutting down!"+"\n") print ("\n"+"No data available on serial port. Shutting down!"+"\n") 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 rosRGBDCallBack(rgb_data, depth_data): try: cv_image = cv_bridge.imgmsg_to_cv2(rgb_data, "bgr8") cv_depthimage = cv_bridge.imgmsg_to_cv2(depth_data, "16SC1") except CvBridgeError as e: print(e) # Convert the image to HSV hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) color_masks = { color: cv2.inRange(hsv_image, colors_lower[color], colors_upper[color]) for color in colors } kwidth = 41 kheight = 65 color_scores = { color: cv2.blur(color_masks[color], (kwidth, kheight)) + np.random.randn(len(color_masks[color]), len(color_masks[color][0])) for color in colors } color_maxima = { color: maximum_filter(color_scores[color], kheight) == color_scores[color] for color in colors } threshold = 80 color_thresholded = { color: color_scores[color] > threshold for color in colors } color_object_map = { color: np.logical_and(color_maxima[color], color_thresholded[color]) for color in colors } color_object_locations = { color: np.argwhere(color_object_map[color]) for color in colors } for color in colors: locs = color_object_locations[color] values = [] for i in range(min(len(locs), 5)): loc = locs[i, :] center_x = loc[1] center_y = loc[0] values.extend([(center_x - cx) / fx, (center_y - cy) / fy]) pt1 = (center_x - (kwidth / 2), center_y - (kheight / 2)) pt2 = (center_x + (kwidth / 2), center_y + (kheight / 2)) cv2.rectangle(cv_image, pt1, pt2, colors_bgr[color]) depth = cv_depthimage[loc[0], loc[1]] cv2.putText(cv_image, str(depth), (loc[1], loc[0]), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255)) dim = MultiArrayDimension("coordinates", len(values), 1) layout = MultiArrayLayout((dim, ), 0) color_publishers[color].publish(layout, values)
def write_hokuyo_4m_packet(hok_4m, utime, bag): # hokuyo_4m always has 726 hits num_hits = 726 timestamp = microseconds_to_ros_timestamp(utime) layout = MultiArrayLayout() layout.dim = [MultiArrayDimension()] layout.dim[0].label = "r" layout.dim[0].size = num_hits layout.dim[0].stride = 1 hits = Float64MultiArray() hits.data = hok_4m hits.layout = layout bag.write('/hokuyo_4m_packet', hits, t=timestamp)