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)
