BaseNodeMain.java
Go to the documentation of this file.
00001 
00009 package org.rosbuilding.common;
00010 
00011 import java.util.ArrayList;
00012 import java.util.List;
00013 
00014 import org.ros.concurrent.CancellableLoop;
00015 import org.ros.dynamic_reconfigure.server.Server;
00016 import org.ros.dynamic_reconfigure.server.Server.ReconfigureListener;
00017 import org.ros.internal.message.Message;
00018 import org.ros.message.MessageListener;
00019 import org.ros.namespace.GraphName;
00020 import org.ros.node.AbstractNodeMain;
00021 import org.ros.node.ConnectedNode;
00022 import org.ros.node.Node;
00023 import org.ros.node.topic.Publisher;
00024 import org.ros.node.topic.Subscriber;
00025 import org.rosbuilding.zeroconf.DiscoveredService;
00026 import org.rosbuilding.zeroconf.NodeConfiguration;
00027 import org.rosbuilding.zeroconf.NodeConfiguration.NodeCapability;
00028 import org.rosbuilding.zeroconf.Zeroconf;
00029 
00030 import com.google.common.base.Strings;
00031 
00032 import smarthome_media_msgs.StateData;
00033 import smarthome_comm_msgs.Command;
00034 
00042 public abstract class BaseNodeMain<TConfiguration extends NodeConfig,
00043         TStateData extends Message, TMessage extends Message>
00044         extends AbstractNodeMain
00045         implements ReconfigureListener<TConfiguration>, INode<TStateData> {
00046 
00047     // Constants
00048     public static final String PUB_WOL      = "/wol";
00049     public static final String PUB_STATE    = "statedata";
00050     public static final String SUB_CMD      = "cmd_action";
00051     public static final String SUB_STATE_ROBOT    = "robotsay";
00052 
00053     // Fields
00054     private boolean isConnected = false;
00055     private ConnectedNode connectedNode;
00056     private TStateData stateData;
00057     private TStateData oldStateData;
00058 
00059     // Capabilities
00060     private List<IModule<TStateData, TMessage>> modules = new ArrayList<>();
00061 
00062     // Topics
00063     private Server<TConfiguration> serverReconfig;
00064     private Publisher<TStateData> pubStateData;
00065     private Publisher<std_msgs.String> pubWol;
00066 
00067     private final StateDataComparator<TStateData> comparator;
00068     private final MessageConverter<TMessage> converter;
00069     private final String messageType;
00070     private final String stateDataType;
00071     public TConfiguration configuration; //TODO make private
00072     private final String nodeName;
00073 
00074     private Thread threadZeroconf = null;
00075 
00076     protected BaseNodeMain(
00077             String nodeName,
00078             StateDataComparator<TStateData> comparator,
00079             MessageConverter<TMessage> converter,
00080             String messageType, String stateDataType) {
00081         this.nodeName = nodeName;
00082         this.comparator = comparator;
00083         this.converter = converter;
00084         this.messageType = messageType;
00085         this.stateDataType = stateDataType;
00086     }
00087 
00091     protected abstract boolean connect();
00092 
00096     protected void loadParameters() {
00097         this.serverReconfig = new Server<TConfiguration>(
00098                 this.getConnectedNode(),
00099                 this.configuration,
00100                 this);
00101     }
00102 
00103     protected abstract void onConnected();
00104     protected abstract void onDisconnected();
00105 
00106     public final TMessage getNewMessageInstance() {
00107         return getNewMessageInstance(this.messageType);
00108     }
00109 
00110     public <T extends Message> T getNewMessageInstance(String type) {
00111         return this.getConnectedNode().getTopicMessageFactory().newFromType(type);
00112     }
00113 
00114     public final TStateData getStateData() {
00115         return this.stateData;
00116     }
00117 
00118     protected final boolean isConnected() {
00119         return this.isConnected;
00120     }
00121 
00122     protected final void addModule(IModule<TStateData, TMessage> module) {
00123         if (module != null) {
00124             this.modules.add(module);
00125         }
00126     }
00127 
00132     protected void refreshStateData() throws InterruptedException {
00133 
00134         if (this.isConnected ) {
00135             this.onConnected();
00136 
00137             for (IModule<TStateData, TMessage> module : this.modules) {
00138                 module.load(this.stateData);
00139             }
00140         } else {
00141             this.isConnected = this.connect();
00142         }
00143 
00144         if (!this.comparator.isEquals(this.stateData, this.oldStateData)) {
00145             TStateData stateDataNew = this.comparator.makeNewCopy(
00146                     this.connectedNode,
00147                     this.configuration.getFixedFrame(),
00148                     this.stateData);
00149             this.oldStateData = stateDataNew;
00150             this.pubStateData.publish(stateDataNew);
00151         }
00152 
00153         Thread.sleep(1000 / this.configuration.getRate());
00154     }
00155 
00160     public void onNewMessage(TMessage message) {
00161         if (message != null) {
00162             try {
00163                 for (IModule<TStateData, TMessage> module : this.modules) {
00164                     module.callbackCmdAction(message, this.stateData);
00165                 }
00166             } catch (Exception e) {
00167                 this.logE(e);
00168 
00169                 this.isConnected = false;
00170                 this.onDisconnected();
00171             }
00172         }
00173     }
00174 
00179     protected void onNewMessage(Command command) {
00180         String[] wheres = command.getContext().getWhere().split(" ");
00181 
00182         for (String where : wheres) {
00183             if (this.configuration.getPrefix().contains(where)) {
00184                 this.onNewMessage(this.converter.toMessage(this.connectedNode, command));
00185             }
00186         }
00187     }
00188 
00192     protected void initTopics() {
00193         this.pubWol = this.connectedNode.newPublisher(
00194                 PUB_WOL,
00195                 std_msgs.String._TYPE);
00196 
00197         if (!Strings.isNullOrEmpty(this.stateDataType)) {
00198             this.pubStateData = this.connectedNode.newPublisher(
00199                     this.configuration.getPrefix() + PUB_STATE,
00200                     this.stateDataType);
00201             this.pubStateData.setLatchMode(true);
00202         }
00203 
00204         if (!Strings.isNullOrEmpty(this.messageType)) {
00205             // Local topic (mapped by prefix and namespace)
00206             Subscriber<TMessage> messageSubscriber = this.connectedNode.newSubscriber(
00207                     this.configuration.getPrefix() + SUB_CMD,
00208                     this.messageType);
00209 
00210             messageSubscriber.addMessageListener(new MessageListener<TMessage>() {
00211                 @Override
00212                 public void onNewMessage(TMessage msg) {
00213                     BaseNodeMain.this.onNewMessage(msg);
00214                 }
00215             });
00216         }
00217 
00218         if (this.converter != null) {
00219             // Global topic registration (no prefix...)
00220             Subscriber<Command> commandSubscriber = this.connectedNode.newSubscriber(
00221                     "/" + SUB_STATE_ROBOT,
00222                     Command._TYPE);
00223 
00224             commandSubscriber.addMessageListener(new MessageListener<Command>() {
00225                 @Override
00226                 public void onNewMessage(Command msg) {
00227                     BaseNodeMain.this.onNewMessage(msg);
00228                 }
00229             });
00230         }
00231 
00232         this.initSubscribers();
00233         this.initPublishers();
00234     }
00235 
00239     protected void initServices() { }
00240 
00241     protected void initSubscribers() { }
00242     protected void initPublishers() { }
00243 
00244     public final ConnectedNode getConnectedNode() {
00245         return this.connectedNode;
00246     }
00247 
00248     @Override
00249     public GraphName getDefaultNodeName() {
00250         return GraphName.of(this.nodeName);
00251     }
00252 
00253     @Override
00254     public void onStart(final ConnectedNode connectedNode) {
00255         super.onStart(connectedNode);
00256         this.connectedNode = connectedNode;
00257 
00258         this.configuration = this.getConfig();
00259         this.configuration.loadParameters();
00260 
00261         this.logI(String.format("Start %s node...", this.nodeName));
00262     }
00263 
00264     @Override
00265     public void onShutdown(Node node) {
00266         this.logI("Stop node !");
00267 
00268         if (this.serverReconfig != null)
00269             this.serverReconfig.close();
00270 
00271         if (this.threadZeroconf != null && this.threadZeroconf.isAlive()) {
00272             this.threadZeroconf.interrupt();
00273         }
00274 
00275         super.onShutdown(node);
00276         this.connectedNode = null;
00277     }
00278 
00282     @Override
00283     public void onError(Node node, Throwable throwable) {
00284         super.onError(node, throwable);
00285         this.logE(throwable.getMessage());
00286     }
00287 
00288     public void startFinal() {
00289         this.initialize();
00290 
00291         // This CancellableLoop will be canceled automatically when the node
00292         // shuts down.
00293         this.connectedNode.executeCancellableLoop(new CancellableLoop() {
00294             @Override
00295             protected void loop() throws InterruptedException {
00296                 refreshStateData();
00297             }
00298         });
00299     }
00300 
00304     protected void initialize() {
00305         this.logI("Start main loop.");
00306 
00307         this.loadParameters();
00308         this.isConnected = false;
00309 
00310         this.initTopics();
00311         this.initServices();
00312 
00313         this.publishZeroConf();
00314 
00315         this.stateData = this.pubStateData.newMessage();
00316     }
00317 
00318     public final void wakeOnLan() {
00319         std_msgs.String message = this.connectedNode.getTopicMessageFactory()
00320                 .newFromType(std_msgs.String._TYPE);
00321         message.setData(this.configuration.getMac());
00322 
00323         this.pubWol.publish(message);
00324     }
00325 
00326     private void publishZeroConf() {
00327         final Zeroconf publisher = new Zeroconf();
00328 
00329         final DiscoveredService service = this.getConfiguration().toDiscoveredService();
00330 
00331         service.name = this.nodeName;
00332         service.type = "_ros-node._tcp";
00333         service.domain = "local";
00334         service.port = 8888;
00335 
00336         this.threadZeroconf = new Thread(new Runnable() {
00337 
00338             @Override
00339             public void run() {
00340                 publisher.addService(service);
00341             }
00342         });
00343 
00344         this.threadZeroconf.start();
00345     }
00346 
00347     protected abstract TConfiguration getConfig();
00348 
00349     protected NodeConfiguration getConfiguration() {
00350         NodeConfiguration configuration = new NodeConfiguration();
00351         configuration.setMasterAddress(this.connectedNode.getMasterUri().getHost());
00352         configuration.setNodePath(this.configuration.getPrefix().substring(
00353                 0, this.configuration.getPrefix().length() - 1));
00354         configuration.setNodeType(this.getClass().getName());
00355 
00356         NodeConfiguration.NodePermission permission =
00357                 new NodeConfiguration.NodePermission();
00358         permission.setExclude(true);
00359         permission.setName("Permission");
00360         configuration.getPermissions().add(permission);
00361         configuration.getCapabilities().add(NodeCapability.ALL);
00362 
00363         return configuration;
00364     }
00365 
00366     @Override
00367     public TConfiguration onReconfigure(TConfiguration config, int level) {
00368         this.configuration.setRate(
00369                 config.getInteger(NodeConfig.RATE, this.configuration.getRate()));
00370 
00371         return config;
00372     }
00373 
00374     public StateDataComparator<TStateData> getComparator() {
00375         return this.comparator;
00376     }
00377 
00378     // Log assessors
00383     public void logD(final Object message) {
00384         this.connectedNode.getLog().debug(message);
00385     }
00386 
00391     public void logI(final Object message) {
00392         this.connectedNode.getLog().info(message);
00393     }
00394 
00399     public void logE(final Object message) {
00400         this.connectedNode.getLog().error(message);
00401     }
00402 
00407     public void logE(final Exception message) {
00408         this.connectedNode.getLog().error(message.getStackTrace());
00409     }
00410 }


smarthome_common_driver
Author(s): Mickael Gaillard , Erwan Le Huitouze
autogenerated on Sat Jun 8 2019 19:35:17