Skip to content
Snippets Groups Projects
Commit 25969881 authored by Damon Kohler's avatar Damon Kohler
Browse files

Improves performance of MessageIdentifier and MessageFields.

Adds a new message creation benchmark.
parent 92733ce4
No related branches found
No related tags found
No related merge requests found
/*
* Copyright (C) 2012 Google Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License"); you may not
* use this file except in compliance with the License. You may obtain a copy of
* the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations under
* the License.
*/
package org.ros.rosjava_benchmarks;
import org.ros.concurrent.CancellableLoop;
import org.ros.concurrent.Rate;
import org.ros.concurrent.WallTimeRate;
import org.ros.message.Duration;
import org.ros.message.Time;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.topic.Publisher;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.atomic.AtomicInteger;
/**
* @author damonkohler@google.com (Damon Kohler)
*/
public class MessagesBenchmark extends AbstractNodeMain {
private final AtomicInteger counter;
private Time time;
public MessagesBenchmark() {
counter = new AtomicInteger();
}
@Override
public GraphName getDefaultNodeName() {
return GraphName.of("messages_benchmark");
}
@Override
public void onStart(final ConnectedNode connectedNode) {
connectedNode.executeCancellableLoop(new CancellableLoop() {
@Override
protected void loop() throws InterruptedException {
geometry_msgs.TransformStamped message =
connectedNode.getTopicMessageFactory()
.newFromType(geometry_msgs.TransformStamped._TYPE);
message.getHeader().setFrameId("world");
message.setChildFrameId("turtle");
message.getHeader().setStamp(connectedNode.getCurrentTime());
message.getTransform().getRotation().setW(Math.random());
message.getTransform().getRotation().setX(Math.random());
message.getTransform().getRotation().setY(Math.random());
message.getTransform().getRotation().setZ(Math.random());
message.getTransform().getTranslation().setX(Math.random());
message.getTransform().getTranslation().setY(Math.random());
message.getTransform().getTranslation().setZ(Math.random());
counter.incrementAndGet();
}
});
time = connectedNode.getCurrentTime();
final Publisher<std_msgs.String> statusPublisher =
connectedNode.newPublisher("status", std_msgs.String._TYPE);
final Rate rate = new WallTimeRate(1);
final std_msgs.String status = statusPublisher.newMessage();
connectedNode.executeCancellableLoop(new CancellableLoop() {
@Override
protected void loop() throws InterruptedException {
Time now = connectedNode.getCurrentTime();
Duration delta = now.subtract(time);
if (delta.totalNsecs() > TimeUnit.NANOSECONDS.convert(5, TimeUnit.SECONDS)) {
double hz = counter.getAndSet(0) * 1e9 / delta.totalNsecs();
status.setData(String.format("%.2f Hz", hz));
statusPublisher.publish(status);
time = now;
}
rate.sleep();
}
});
}
}
......@@ -67,15 +67,17 @@ public class MessageFields {
}
public Object getFieldValue(String name) {
if (fields.containsKey(name)) {
return fields.get(name).getValue();
Field field = fields.get(name);
if (field != null) {
return field.getValue();
}
throw new RosRuntimeException("Uknown field: " + name);
}
public void setFieldValue(String name, Object value) {
if (fields.containsKey(name)) {
fields.get(name).setValue(value);
Field field = fields.get(name);
if (field != null) {
field.setValue(value);
} else {
throw new RosRuntimeException("Uknown field: " + name);
}
......
......@@ -25,40 +25,67 @@ import com.google.common.base.Preconditions;
*/
public class MessageIdentifier {
private final String type;
private final String pkg;
private final String name;
private String type;
private String pkg;
private String name;
public static MessageIdentifier of(String pkg, String name) {
Preconditions.checkNotNull(pkg);
Preconditions.checkNotNull(name);
return of(pkg + "/" + name);
return new MessageIdentifier(pkg, name);
}
public static MessageIdentifier of(String type) {
Preconditions.checkNotNull(type);
Preconditions.checkArgument(type.contains("/"), "Type must be fully qualified: " + type);
// We're not using Preconditions.checkArgument() here because we want a
// useful error message without paying the performance penalty of
// constructing it every time.
if (!type.contains("/")) {
throw new IllegalArgumentException(String.format(
"Type name is invalid or not fully qualified: \"%s\"", type));
}
return new MessageIdentifier(type);
}
public MessageIdentifier(String type) {
Preconditions.checkNotNull(type);
Preconditions.checkArgument(type.contains("/"), "Type must be fully qualified: " + type);
private MessageIdentifier(String type) {
this.type = type;
String[] packageAndName = type.split("/", 2);
pkg = packageAndName[0];
name = packageAndName[1];
}
private MessageIdentifier(String pkg, String name) {
this.pkg = pkg;
this.name = name;
}
public String getType() {
if (type == null) {
// Using StringBuilder like this is about 40% faster than using the +
// operator.
StringBuilder stringBuilder = new StringBuilder(pkg.length() + name.length() + 1);
stringBuilder.append(pkg);
stringBuilder.append("/");
stringBuilder.append(name);
type = stringBuilder.toString();
}
return type;
}
private void splitType() {
String[] packageAndName = type.split("/", 2);
pkg = packageAndName[0];
name = packageAndName[1];
}
public String getPackage() {
if (pkg == null) {
splitType();
}
return pkg;
}
public String getName() {
if (name == null) {
splitType();
}
return name;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment