Feature/add map server

navigation_setup/launch/tangobot_demo_visualizer.launch

@@ -6,9 +6,6 @@
   <arg name="stacks"     default="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons -->
   <arg name="3d_sensor"  default="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro -->
 
-  <!-- Name of the map to use (without path nor extension) and initial position -->
-  <arg name="map_file"       default="$(find navigation_setup)/map/empty.yaml"/>
-
   <param name="/use_sim_time" value="false"/>   <!-- False if we are not using simulation-->
 
   <!--  ***************** Robot Model *****************  -->
@@ -18,11 +15,6 @@
     <arg name="3d_sensor" value="$(arg 3d_sensor)" />
   </include>
 
-  <!--  ****** Maps *****  -->
-  <include file="$(find navigation_setup)/launch/tools/map.launch">
-    <arg name="map_file" value="$(arg map_file)"/>
-  </include>
-
   <!-- publish device pose -->
   <node pkg="dynamic_reconfigure" name="dynparam" type="dynparam" args="set /tango publish_device_pose true"/>
 

tangobot_app/app/src/main/java/com/ekumen/tangobot/application/MainActivity.java

@@ -43,7 +43,9 @@
 import com.ekumen.tangobot.nodes.DefaultMapTfPublisherNode;
 import com.ekumen.tangobot.nodes.DefaultRobotTfPublisherNode;
 import com.ekumen.tangobot.nodes.ExtrinsicsTfPublisherNode;
+import com.ekumen.tangobot.nodes.EmptyMapGenerator;
 import com.ekumen.tangobot.nodes.MoveBaseNode;
+import com.ekumen.tangobot.nodes.OccupancyGridPublisherNode;
 
 import org.apache.commons.logging.Log;
 import org.apache.commons.logging.LogFactory;
@@ -94,6 +96,7 @@ public class MainActivity extends AppCompatRosActivity implements TangoRosNode.C
     private ParameterLoaderNode mParameterLoaderNode;
     private ExtrinsicsTfPublisherNode mRobotExtrinsicsTfPublisherNode;
     private ExtrinsicsTfPublisherNode mMapExtrinsicsTfPublisherNode;
+    private OccupancyGridPublisherNode mOccupancyGridPublisherNode;
 
     // Status
     private ModuleStatusIndicator mRosMasterConnection;
@@ -207,6 +210,7 @@ protected void init(NodeMainExecutor nodeMainExecutor) {
         configureParameterServer();
 
         startExtrinsicsPublisherNodes();
+        startMapServerNode();
 
         // Start Tango node and navigation stack.
         startTangoRosNode();
@@ -316,6 +320,16 @@ private void startExtrinsicsPublisherNodes() {
         mNodeMainExecutor.execute(mRobotExtrinsicsTfPublisherNode, nodeConfiguration);
     }
 
+    private void startMapServerNode() {
+        // Create ROS node to publish the map
+        mLog.info("Starting map server");
+        NodeConfiguration nodeConfiguration = NodeConfiguration.newPublic(mHostName);
+        nodeConfiguration.setMasterUri(mMasterUri);
+        nodeConfiguration.setNodeName(OccupancyGridPublisherNode.NODE_NAME);
+        mOccupancyGridPublisherNode = new OccupancyGridPublisherNode(new EmptyMapGenerator());
+        mNodeMainExecutor.execute(mOccupancyGridPublisherNode, nodeConfiguration);
+    }
+
     @Override
     public void onResume() {
         super.onResume();

tangobot_app/app/src/main/java/com/ekumen/tangobot/nodes/EmptyMapGenerator.java

@@ -0,0 +1,66 @@
+/*
+ * Copyright 2017 Ekumen, Inc. All Rights Reserved.
+ *
+ * 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 com.ekumen.tangobot.nodes;
+
+import org.jboss.netty.buffer.ChannelBuffer;
+import org.jboss.netty.buffer.ChannelBufferOutputStream;
+import org.ros.internal.message.MessageBuffers;
+import org.ros.message.Time;
+import org.ros.rosjava_geometry.Quaternion;
+import org.ros.rosjava_geometry.Vector3;
+
+import geometry_msgs.Pose;
+import nav_msgs.MapMetaData;
+import std_msgs.Header;
+
+/**
+ * Class that generates message for an empty map of 10x10 meters.
+ * TODO: This should be replaced by a Map Generator that could generate a map grid from an image with metadata.
+ */
+public class EmptyMapGenerator implements OccupancyGridGenerator {
+    private static final int WIDTH = 200;
+    private static final int HEIGHT = 200;
+    private static final float RESOLUTION = (float) 0.05;
+
+    @Override
+    public void fillHeader(Header header) {
+        header.setFrameId("map");
+        header.setStamp(Time.fromMillis(System.currentTimeMillis()));
+    }
+
+    @Override
+    public void fillInformation(MapMetaData information) {
+        information.setMapLoadTime(Time.fromMillis(System.currentTimeMillis()));
+        Pose origin = information.getOrigin();
+        Vector3.zero().toPointMessage(origin.getPosition());
+        Quaternion.identity().toQuaternionMessage(origin.getOrientation());
+        information.setWidth(WIDTH);
+        information.setHeight(HEIGHT);
+        information.setResolution(RESOLUTION);
+    }
+
+    @Override
+    public ChannelBuffer generateData() {
+        ChannelBufferOutputStream output = new ChannelBufferOutputStream(MessageBuffers.dynamicBuffer());
+        try {
+            output.write(new byte[WIDTH * HEIGHT]);
+        } catch (Exception e) {
+            throw new RuntimeException("Empty map generator generateData error: " + e.getMessage());
+        }
+        return output.buffer();
+    }
+}

tangobot_app/app/src/main/java/com/ekumen/tangobot/nodes/OccupancyGridGenerator.java

@@ -0,0 +1,32 @@
+/*
+ * Copyright 2017 Ekumen, Inc. All Rights Reserved.
+ *
+ * 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 com.ekumen.tangobot.nodes;
+
+import org.jboss.netty.buffer.ChannelBuffer;
+
+import nav_msgs.MapMetaData;
+import nav_msgs.OccupancyGrid;
+import std_msgs.Header;
+
+/**
+ * Interface to generate an {@link OccupancyGrid} to be published by {@link OccupancyGridPublisherNode}.
+ */
+public interface OccupancyGridGenerator {
+    void fillHeader(Header header);
+    void fillInformation(MapMetaData information);
+    ChannelBuffer generateData();
+}

tangobot_app/app/src/main/java/com/ekumen/tangobot/nodes/OccupancyGridPublisherNode.java

@@ -0,0 +1,57 @@
+/*
+ * Copyright 2017 Ekumen, Inc. All Rights Reserved.
+ *
+ * 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 com.ekumen.tangobot.nodes;
+
+import org.ros.namespace.GraphName;
+import org.ros.node.AbstractNodeMain;
+import org.ros.node.ConnectedNode;
+import org.ros.node.topic.Publisher;
+
+import nav_msgs.OccupancyGrid;
+
+/**
+ * Publishes an {@link OccupancyGrid} created by a {@link OccupancyGridGenerator}.
+ */
+public class OccupancyGridPublisherNode extends AbstractNodeMain {
+    public static final String NODE_NAME = "occupancy_grid_publisher";
+    private Publisher<OccupancyGrid> mPublisher;
+    private OccupancyGridGenerator gridGenerator;
+
+    public OccupancyGridPublisherNode(OccupancyGridGenerator generator) {
+        gridGenerator = generator;
+    }
+
+    @Override
+    public GraphName getDefaultNodeName() {
+        return GraphName.of(NODE_NAME);
+    }
+
+    @Override
+    public void onStart(ConnectedNode connectedNode) {
+        super.onStart(connectedNode);
+
+        mPublisher = connectedNode.newPublisher("map", OccupancyGrid._TYPE);
+        mPublisher.setLatchMode(true);
+
+        OccupancyGrid message = mPublisher.newMessage();
+
+        gridGenerator.fillHeader(message.getHeader());
+        gridGenerator.fillInformation(message.getInfo());
+        message.setData(gridGenerator.generateData());
+        mPublisher.publish(message);
+    }
+}