Initial commit

20 files changed, 1960 insertions, 0 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
new file mode 100644
index 0000000..ae311c6
--- /dev/null
+++ b/src/Makefile.am
@@ -0,0 +1,97 @@
+# Makefile.am -- automake script for the car plugin
+# Copyright (C) 1994, 1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002, 2003,
+#     2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011  Free Software Foundation,
+#     Inc.
+# Copyright (C) 2013  Galois, Inc.
+#
+# This program is free software: you can redistribute it and/or modify it under
+# the terms of the GNU General Public License as published by the Free Software
+# Foundation, either version 3 of the License, or (at your option) any later
+# version.
+#
+# This program is distributed in the hope that it will be useful, but WITHOUT
+# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+# FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+# details.
+#
+# You should have received a copy of the GNU General Public License along with
+# this program.  If not, see <http://www.gnu.org/licenses/>.
+#
+# To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+# or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+# Oregon, 97204-1622.
+
+# We're building a library that gets loaded by dlopen, not a standard .so.
+AM_LDFLAGS = \
+	-avoid-version -module -shared -export-dynamic
+
+lib_LTLIBRARIES = libv_repExtAutomobile.la
+libv_repExtAutomobile_la_SOURCES = \
+	$(srcdir)/automobile.cpp \
+	$(srcdir)/automobile.h \
+	$(srcdir)/csv.cpp \
+	$(srcdir)/csv.h \
+	$(srcdir)/csv-inl.h \
+	$(srcdir)/main.cpp \
+	$(srcdir)/main.h \
+	$(srcdir)/measurement.cpp \
+	$(srcdir)/measurement.h \
+	$(srcdir)/measurement-inl.h \
+	$(srcdir)/noise.cpp \
+	$(srcdir)/noise.h \
+	$(srcdir)/noise-inl.h \
+	$(srcdir)/vrep.cpp \
+	$(srcdir)/vrep.h \
+	$(srcdir)/vrep-inl.h \
+	$(srcdir)/vrepFfi.cpp \
+	$(srcdir)/vrepFfi.h \
+	$(srcdir)/vrepFfi-inl.h
+libv_repExtAutomobile_la_CPPFLAGS = \
+	$(BOOST_CPPFLAGS)
+libv_repExtAutomobile_la_CXXFLAGS = \
+	-Wall \
+	-Wextra \
+	-pedantic \
+	@VREP_CXXFLAGS@
+libv_repExtAutomobile_la_LDFLAGS = \
+	$(AM_LDFLAGS) \
+	-export-symbols-regex '^v_rep' \
+	$(BOOST_FILESYSTEM_LDFLAGS)
+libv_repExtAutomobile_la_LIBADD = \
+	$(BOOST_FILESYSTEM_LIBS)
+
+# In addition to these sources, we also need to bundle a special object built
+# from code in the V-REP distribution.  Ideally, I'd just add that source file
+# to 'libv_repExtAutomobile_la_SOURCES', but it's not exactly
+# standards-compliant and generates a ton of warnings.  To get around this, use
+# per-object flags emulation (see "Per-Object Flags Emulation" in the Automake
+# manual).
+libv_repExtAutomobile_la_LIBADD += libv_repLib.la
+noinst_LTLIBRARIES = libv_repLib.la
+libv_repLib_la_SOURCES = @VREP@/programming/common/v_repLib.cpp
+libv_repLib_la_CXXFLAGS = @VREP_CXXFLAGS@
+
+# Override install and uninstall targets to stick the libraries in the V-REP
+# directory.
+install-libLTLIBRARIES: $(lib_LTLIBRARIES)
+	@$(NORMAL_INSTALL)
+	@list='$(lib_LTLIBRARIES)'; test -n "$(libdir)" || list=; \
+	list2=; for p in $$list; do \
+	  if test -f $$p; then \
+	    list2="$$list2 $$p"; \
+	  else :; fi; \
+	done; \
+	test -z "$$list2" || { \
+	  echo " $(MKDIR_P) '$(VREP)'"; \
+	  $(MKDIR_P) "$(VREP)" || exit 1; \
+	  echo " $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=install --no-warn $(INSTALL) $(INSTALL_STRIP_FLAG) $$list2 '$(VREP)'"; \
+	  $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=install --no-warn $(INSTALL) $(INSTALL_STRIP_FLAG) $$list2 "$(VREP)"; \
+	}
+uninstall-libLTLIBRARIES:
+	@$(NORMAL_UNINSTALL)
+	@list='$(lib_LTLIBRARIES)'; test -n "$(libdir)" || list=; \
+	for p in $$list; do \
+	  $(am__strip_dir) \
+	  echo " $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=uninstall rm -f '$(VREP)/$$f'"; \
+	  $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=uninstall rm -f "$(VREP)/$$f"; \
+	done
diff --git a/src/automobile.cpp b/src/automobile.cpp
new file mode 100644
index 0000000..365c96b
--- /dev/null
+++ b/src/automobile.cpp
@@ -0,0 +1,403 @@
+/* automobile.cpp -- recording data about the car
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+/* This module assumes only one car is recording data at once.  Thus, it follows
+ * a singleton object pattern.  To keep everything a bit simpler, the state
+ * variables live in an anonymous namespace, and the functions simply mutate
+ * them as appropriate. */
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <cassert>
+#include <cstdarg>
+
+#include <array>
+#include <fstream>
+#include <functional>
+#include <string>
+#include <vector>
+
+#include <boost/filesystem.hpp>
+#include <boost/optional.hpp>
+#include <v_repLib.h>
+
+#include "automobile.h"
+#include "noise.h"
+#include "vrepFfi.h"
+
+#ifndef HAVE_CXX11_INITIALIZER_LISTS
+#   include <cassert>
+#   include <list>
+#endif
+
+
+namespace {
+
+    // Paths to the output files
+    namespace path {
+
+        std::string dataDir = "/tmp";
+
+        const std::string groundDir = "/ground";
+        const std::string noisyDir = "/noisy";
+
+        const std::string properties = "/properties.csv";
+        const std::string sensor = "/slam_sensor.csv";
+        const std::string pose = "/slam_gps.csv";
+        const std::string control = "/slam_control.csv";
+        const std::string laser = "/slam_laser.csv";
+
+    }
+
+    // Run specifications
+    struct Properties : public csv::Datum {
+        inline Properties(float L, float h, float a, float b, float theta0)
+            : L(L), h(h), a(a), b(b), theta0(theta0) {
+        }
+        inline virtual std::string csvHeader() const;
+        virtual std::string csv() const;
+        inline virtual unsigned int nCols() const;
+        // Distance between front and rear axles
+        float L;
+        // Distance between center of rear axle and encoder on left rear wheel
+        float h;
+        // Lengthwise distance between the rear axle and the lidar
+        float a;
+        // Widthwise distance between the rear axle and the lidar
+        float b;
+        // Initial angle of the car
+        float theta0;
+    };
+
+    // Individual sample records
+    struct Sample : public csv::Datum {
+        inline explicit Sample(const Pose &);
+        inline explicit Sample(const ControlSignals &);
+        inline explicit Sample(const LidarDatum &);
+        inline virtual std::string csvHeader() const;
+        inline virtual std::string csv() const;
+        inline virtual unsigned int nCols() const;
+        float time;
+        unsigned short sensorId;
+    };
+
+
+    // Lidar specifications //
+    namespace laser {
+
+        float MAX_DISTANCE = 10.;
+        float MAX_INTENSITY = 32768.;
+
+    }
+
+
+    // Noise //
+    namespace noise {
+
+        bool requested = false;
+
+        // Dummy noise source that provides no noise
+        GaussianNoiseSource<float> none(0., 0.);
+
+        // Noise sources for the various measurements (can be set via Lua)
+        boost::optional<GaussianNoiseSource<float>> position;
+        boost::optional<GaussianNoiseSource<float>> angle;
+        boost::optional<GaussianNoiseSource<float>> speed;
+        boost::optional<GaussianNoiseSource<float>> steeringAngle;
+        boost::optional<GaussianNoiseSource<float>> distance;
+        boost::optional<GaussianNoiseSource<float>> intensity;
+
+    }
+
+
+    // Prototypes //
+
+    // Lua callbacks
+    typedef simVoid LuaFunc(SLuaCallBack *);
+    LuaFunc init;
+    LuaFunc setNoiseParameters;
+    LuaFunc savePose;
+    LuaFunc saveControls;
+    LuaFunc saveLaser;
+
+    inline void savePropertiesFile(const Properties &);
+
+    template<typename CsvDatum>
+    inline void saveDatum(const std::string &subdir,
+                          const std::string &filename, const CsvDatum &);
+
+    void outputDatum(const std::string &filePath, const csv::Datum &);
+
+}
+
+
+// Registering //
+
+void registerLuaFunctions() {
+    vrep::exposeFunction<
+        std::string,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float>(
+        "simExtAutomobileInit",
+        "simExtAutomobileInit(string directoryName, number L, number h, number a, number b, number theta0, number max_distance, number max_intensity)",
+        init);
+    vrep::exposeFunction<
+        std::vector<float>,     // x and y
+        std::vector<float>,     // angle
+        std::vector<float>,     // speed
+        std::vector<float>,     // steering angle
+        std::vector<float>,     // distance
+        std::vector<float>>(    // intensity
+            "simExtAutomobileRequestNoise",
+            "simExtAutomobileRequestNoise(table2 xy, table2 angle, table2 speed, table2 steeringAngle, table2 distance, table2 intensity)",
+        setNoiseParameters);
+    vrep::exposeFunction<float, float, float, float>(
+        "simExtAutomobileSavePose",
+        "simExtAutomobileSavePose(number simulationTime, number x, number y, number theta)",
+        savePose);
+    vrep::exposeFunction<float, float, float>(
+        "simExtAutomobileSaveControls",
+        "simExtAutomobileSaveControls(number simulationTime, number speed, number steeringAngle)",
+        saveControls);
+    /* V-REP's depth sensor part has a maximum field of view narrower than 180
+     * degrees.  To compensate, we instead use two 90-degree depth sensors and
+     * combine the results here. */
+    vrep::exposeFunction<
+        float,
+        std::vector<float>, std::vector<float>,  // depths
+        std::vector<float>, std::vector<float>>( // grayscale images
+            "simExtAutomobileSaveLaserPair",
+            "simExtAutomobileSaveLaserPair(number simulationTime, table leftDepthBuffer, table rightDepthBuffer, table leftImage, table rightImage)",
+            saveLaser);
+}
+
+
+// Callbacks //
+namespace {
+
+    std::string Properties::csvHeader() const {
+#       ifdef HAVE_CXX11_INITIALIZER_LISTS
+            return csv::fromContainer(
+                std::vector<std::string> {"L", "h", "a", "b", "InitialAngle"});
+#       else
+            return csv::fromContainer(
+                makeList<const char *>(nCols(), "L", "h", "a", "b",
+                                       "InitialAngle"));
+#       endif
+    }
+
+    std::string Properties::csv() const {
+#       ifdef HAVE_CXX11_INITIALIZER_LISTS
+            return csv::fromContainer(
+                std::vector<float> {L, h, a, b, theta0});
+#       else
+            return csv::fromContainer(
+                makeList<float>(nCols(), L, h, a, b, theta0));
+#       endif
+    }
+
+    unsigned int Properties::nCols() const {
+        return 5;
+    }
+
+    Sample::Sample(const Pose &pose)
+        : time(pose.time), sensorId(1) {
+    }
+
+    Sample::Sample(const ControlSignals &signals)
+        : time(signals.time), sensorId(2) {
+    }
+
+    Sample::Sample(const LidarDatum &datum)
+        : time(datum.time), sensorId(3) {
+    }
+
+    std::string Sample::csvHeader() const {
+        return "Time,Sensor";
+    }
+
+    std::string Sample::csv() const {
+        return std::to_string(time) + "," + std::to_string(sensorId);
+    }
+
+    unsigned int Sample::nCols() const {
+        return 2;
+    }
+
+    simVoid init(SLuaCallBack *const simCall) {
+        // Clean data from previous runs.
+        boost::filesystem::remove_all(path::dataDir + path::groundDir);
+        boost::filesystem::remove_all(path::dataDir + path::noisyDir);
+        boost::filesystem::remove(path::dataDir + path::properties);
+        // Save the passed directory as the base directory for the output.
+        vrep::LuaCall call(simCall);
+        path::dataDir = call.expectAtom<std::string>();
+        // Save the passed properties in the properties file.
+        const float L = call.expectAtom<float>();
+        const float h = call.expectAtom<float>();
+        const float a = call.expectAtom<float>();
+        const float b = call.expectAtom<float>();
+        const float theta0 = call.expectAtom<float>();
+        savePropertiesFile(Properties(L, h, a, b, theta0));
+        // Save the maximum distance and intensity settings.
+        laser::MAX_DISTANCE = call.expectAtom<float>();
+        laser::MAX_INTENSITY = call.expectAtom<float>();
+        // Reset the noise settings.
+        noise::requested = false;
+        const std::array<boost::optional<GaussianNoiseSource<float>> *const, 6> sources =
+            {{&noise::position, &noise::angle, &noise::speed,
+              &noise::steeringAngle, &noise::intensity, &noise::distance}};
+        for (boost::optional<GaussianNoiseSource<float>> *const source : sources) {
+            *source = boost::none;
+        }
+    }
+
+    simVoid setNoiseParameters(SLuaCallBack *const simCall) {
+        vrep::LuaCall call(simCall);
+        const std::array<boost::optional<GaussianNoiseSource<float>> *const, 6> sources =
+            {{&noise::position, &noise::angle, &noise::speed,
+              &noise::steeringAngle, &noise::intensity, &noise::distance}};
+        for (boost::optional<GaussianNoiseSource<float>> *const source : sources) {
+            *source = gaussian(call.expectTable<float>());
+        }
+        noise::requested = true;
+    }
+
+    simVoid savePose(SLuaCallBack *const simCall) {
+        // Build the pose.
+        vrep::LuaCall call(simCall);
+        const float time = call.expectAtom<float>();
+        const float x = call.expectAtom<float>();
+        const float y = call.expectAtom<float>();
+        const float theta = call.expectAtom<float>();
+        const Pose pose(time, x, y, theta);
+        // Record it.
+        saveDatum(path::groundDir, path::pose, pose);
+        if (noise::requested) {
+            saveDatum(path::noisyDir, path::pose,
+                      addNoise(pose, noise::position.get_value_or(noise::none),
+                               noise::angle.get_value_or(noise::none)));
+        }
+    }
+
+    simVoid saveControls(SLuaCallBack *const simCall) {
+        // Build the control systems object.
+        vrep::LuaCall call(simCall);
+        const float time = call.expectAtom<float>();
+        const float speed = call.expectAtom<float>();
+        const float angle = call.expectAtom<float>();
+        const ControlSignals signals(time, speed, angle);
+        // Record it.
+        saveDatum(path::groundDir, path::control, signals);
+        if (noise::requested) {
+            saveDatum(path::noisyDir, path::control,
+                      addNoise(signals, noise::speed.get_value_or(noise::none),
+                               noise::steeringAngle.get_value_or(noise::none)));
+        }
+    }
+
+    simVoid saveLaser(SLuaCallBack *const simCall) {
+        // Get the arguments and reconstruct the full lidar measurements.
+        vrep::LuaCall call(simCall);
+        const float time = call.expectAtom<float>();
+        std::vector<float> distance = call.expectTable<float>();
+        const std::vector<float> distanceRight = call.expectTable<float>();
+        distance.insert(distance.end(),
+                        distanceRight.begin(), distanceRight.end());
+        std::vector<float> image = call.expectTable<float>();
+        const std::vector<float> imageRight = call.expectTable<float>();
+        image.insert(image.end(), imageRight.begin(), imageRight.end());
+        // Process the lidar measurements.
+#       ifdef HAVE_CXX11_CLOSURES
+            std::for_each(distance.begin(), distance.end(),
+                          [](float &d) { d *= laser::MAX_DISTANCE; });
+            std::for_each(image.begin(), image.end(),
+                          [](float &i) { i *= laser::MAX_INTENSITY; });
+#       else
+            for (float &d : distance) {
+                d *= laser::MAX_DISTANCE;
+            }
+            for (float &i : image) {
+                i *= laser::MAX_INTENSITY;
+            }
+#       endif
+        // Build the lidar datum and record it.
+        const LidarDatum datum(time, distance, image);
+        saveDatum(path::groundDir, path::laser, datum);
+        if (noise::requested) {
+            saveDatum(path::noisyDir, path::laser,
+                      addNoise(datum, noise::distance.get_value_or(noise::none),
+                               noise::intensity.get_value_or(noise::none)));
+        }
+    }
+
+    void savePropertiesFile(const Properties &properties) {
+        boost::filesystem::create_directories(path::dataDir);
+        outputDatum(path::dataDir + path::properties, properties);
+    }
+
+    template<typename CsvDatum>
+    void saveDatum(const std::string &subdir, const std::string &filename,
+                   const CsvDatum &datum) {
+        const std::string logDir = path::dataDir + subdir;
+        boost::filesystem::create_directories(logDir);
+        outputDatum(logDir + filename, datum);
+        outputDatum(logDir + path::sensor, Sample(datum));
+    }
+
+    void outputDatum(const std::string &filePath, const csv::Datum &datum) {
+        const std::string header = datum.csvHeader();
+        std::fstream file(filePath,
+                          std::ios::in | std::ios::out | std::ios::app);
+        // Ensure we're writing correctly-formatted data.
+        std::string firstLine;
+        std::getline(file, firstLine);
+        if (file.eof() && firstLine.empty()) {
+            // The file was empty, so write in the header.
+            file.clear();
+            file << header << std::endl;
+        } else if (firstLine != header) {
+            /* The file was nonempty, but the first line did not match the
+             * expected header.  This might occur if an external program
+             * modifies the file in between our writes, but more likely, we've
+             * got a bug somewhere that is causing us to write different data
+             * sets to the same file. */
+            throw std::logic_error(
+                std::string("CSV header mismatch: expected `")
+                + header
+                + "', but got `"
+                + firstLine
+                + "'");
+        } else {
+            /* The file was nonempty, and the headers matched what we expected.
+             * We're good to go. */
+        }
+        // Write the datum.
+        file << datum.csv() << std::endl;
+    }
+
+}
diff --git a/src/automobile.h b/src/automobile.h
new file mode 100644
index 0000000..0d3d2c1
--- /dev/null
+++ b/src/automobile.h
@@ -0,0 +1,30 @@
+/* automobile.h -- recording data about the car
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_AUTOMOBILE_H
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_AUTOMOBILE_H
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+void registerLuaFunctions();
+
+#endif
diff --git a/src/csv-inl.h b/src/csv-inl.h
new file mode 100644
index 0000000..c1b7cb9
--- /dev/null
+++ b/src/csv-inl.h
@@ -0,0 +1,58 @@
+/* csv-inl.h -- CSV utilities
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_CSV_INL_H
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_CSV_INL_H
+
+namespace csv {
+
+    template<typename SequenceContainer>
+    std::string fromContainer(const SequenceContainer &columns) {
+        std::ostringstream line;
+        for (typename SequenceContainer::const_iterator col = columns.begin();
+             col != columns.end();
+             col++) {
+            if (col != columns.begin()) {
+                line << ",";
+            }
+            line << *col;
+        }
+        return line.str();
+    }
+
+}
+
+#ifndef HAVE_CXX11_INITIALIZER_LISTS
+
+    template<typename T>
+    std::list<T> makeList(typename std::list<T>::size_type size, ...) {
+        std::list<T> result;
+        va_list args;
+        va_start(args, size);
+        for (typename std::list<T>::size_type i = 0; i < size; i++) {
+            result.push_back(va_arg(args, T));
+        }
+        va_end(args);
+        return result;
+    }
+
+#endif
+
+#endif
diff --git a/src/csv.cpp b/src/csv.cpp
new file mode 100644
index 0000000..88d08b3
--- /dev/null
+++ b/src/csv.cpp
@@ -0,0 +1,48 @@
+/* csv.h -- CSV utilities
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#ifndef HAVE_CXX11_INITIALIZER_LISTS
+#   include <cstdarg>
+#   include <list>
+#   include <sstream>
+#endif
+
+#include "csv.h"
+
+#ifndef HAVE_CXX11_INITIALIZER_LISTS
+
+    std::list<float> makeList(typename std::list<float>::size_type size, ...) {
+        std::list<float> result;
+        va_list args;
+        va_start(args, size);
+        for (typename std::list<double>::size_type i = 0;
+             i < static_cast<std::list<double>::size_type>(size);
+             i++) {
+            result.push_back(va_arg(args, double));
+        }
+        va_end(args);
+        return result;
+    }
+
+#endif
diff --git a/src/csv.h b/src/csv.h
new file mode 100644
index 0000000..fd46646
--- /dev/null
+++ b/src/csv.h
@@ -0,0 +1,71 @@
+/* csv.h -- CSV utilities
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_CSV_H
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_CSV_H
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <string>
+#include <sstream>
+
+#ifndef HAVE_CXX11_INITIALIZER_LISTS
+#   include <cstdarg>
+#   include <list>
+#endif
+
+namespace csv {
+
+    // Interface: Any datum which can be converted to a CSV string.
+    struct Datum {
+        virtual std::string csvHeader() const = 0;
+        virtual std::string csv() const = 0;
+        virtual unsigned int nCols() const = 0;
+    };
+
+    /* Converts a sequence of objects to a CSV string.  It uses stringstreams
+     * internally, so all data in the sequence will get converted to strings
+     * using operator<<; however, no escaping will occur. */
+    template<typename SequenceContainer>
+    std::string fromContainer(const SequenceContainer &);
+
+}
+
+#ifndef HAVE_CXX11_INITIALIZER_LISTS
+
+    /* Without C++11 initializer list syntax, creating containers to pass to
+     * csv::fromContainer is really ugly.  This function makes it substantially
+     * nicer, at the cost of forcing you to manually specify the length of the
+     * container (an unfortunate varargs limitation).  */
+    template<typename T>
+    std::list<T> makeList(typename std::list<T>::size_type, ...);
+
+    /* floats get promoted to doubles in a va_list, so they need special
+     * handling. */
+    template<>
+    std::list<float> makeList(typename std::list<float>::size_type, ...);
+
+#endif
+
+#include "csv-inl.h"
+
+#endif
diff --git a/src/main.cpp b/src/main.cpp
new file mode 100644
index 0000000..6deb436
--- /dev/null
+++ b/src/main.cpp
@@ -0,0 +1,137 @@
+/* main.cpp -- plugin entry points
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <cstddef>
+
+#include <exception>
+#include <stdexcept>
+#include <string>
+
+#include <boost/filesystem.hpp>
+#include <v_repLib.h>
+
+#include "automobile.h"
+#include "main.h"
+#include "vrep.h"
+#include "vrepFfi.h"
+
+namespace {
+
+    const unsigned char PLUGIN_VERSION = 1;
+
+    // The V-REP library that will get loaded at runtime
+    const std::string VREP_LIBRARY_NAME = "libv_rep.so";
+    LIBRARY vrepLibrary;
+
+
+    // Prototypes //
+
+    /* The library we think we're loading might not actually be the right
+     * library. */
+    class BadLibraryError : public std::runtime_error {
+    public:
+        explicit inline BadLibraryError();
+    };
+
+    // We might be using a too-old V-REP.
+    class TooOldVrepError : public std::runtime_error {
+    public:
+        explicit inline TooOldVrepError(const simInt version);
+    };
+
+    /* Loads the V-REP library and ensures it has the correct set of symbols.
+     * Throws 'std::exception' if an error occurs. */
+    // This function is declared inline because it is only used once.
+    inline void loadAndValidateVrepLibrary();
+
+    /* Throws 'std::exception' if the current version of V-REP is strictly less
+       than the 'simInt' passed. */
+    // This function is declared inline because it is only used once.
+    inline void requireVrepVersion(const simInt);
+}
+
+unsigned char v_repStart(void *, int) {
+    try {
+        loadAndValidateVrepLibrary();
+        // If we needed a specific version of V-REP, we'd check it here with
+        // 'requireVrepVersion'.
+        vrep::InterfaceLockGuard lockInterface;
+        // Register functions.
+        registerLuaFunctions();
+        // Returning zero indicates a failure condition.
+        static_assert(PLUGIN_VERSION != 0, "Plugin version must be nonzero");
+        return PLUGIN_VERSION;
+    } catch (const std::exception& error) {
+        // Loading the plugin failed.  Return 0 to indicate failure.
+        return 0;
+    }
+}
+
+void v_repEnd() {
+    unloadVrepLibrary(vrepLibrary);
+}
+
+void *v_repMessage(int, int *, void *, int *) {
+    return nullptr;
+}
+
+namespace {
+
+    void loadAndValidateVrepLibrary() {
+        try {
+            // Load the V-REP library out of the V-REP directory.
+            const std::string vrepLibraryPath =
+                boost::filesystem::current_path().string()
+                + "/" + VREP_LIBRARY_NAME;
+            vrepLibrary = loadVrepLibrary(vrepLibraryPath.c_str());
+            // Make sure we can actually get code out of the V-REP library.
+            if (getVrepProcAddresses(vrepLibrary) == 0) {
+                throw BadLibraryError();
+            }
+        } catch (const std::exception& e) {
+            // Problems occurred loading the library; unload it before leaving.
+            unloadVrepLibrary(vrepLibrary);
+            throw e;
+        }
+    }
+
+    void requireVrepVersion(const simInt expected) {
+        simInt actual;
+        VREP(simGetIntegerParameter(sim_intparam_program_version, &actual));
+        if (actual < expected) {
+            throw TooOldVrepError(expected);
+        }
+    }
+
+    BadLibraryError::BadLibraryError()
+        : std::runtime_error("could not find all required functions in "
+                             + VREP_LIBRARY_NAME) {
+    }
+
+    TooOldVrepError::TooOldVrepError(const simInt version)
+        : std::runtime_error("plugin requires V-REP >= "
+                             + std::to_string(version)) {
+    }
+
+}
diff --git a/src/main.h b/src/main.h
new file mode 100644
index 0000000..4960210
--- /dev/null
+++ b/src/main.h
@@ -0,0 +1,44 @@
+/* main.h -- main plugin interface
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_MAIN_H_
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_MAIN_H_
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+/* This interface is defined by V-REP itself.  See
+ * <http://www.coppeliarobotics.com/helpFiles/en/plugins.htm>. */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    unsigned char v_repStart(void *pointer, int integer);
+    void v_repEnd();
+    void *v_repMessage(int message, int *auxiliary_data, void *custom_data,
+                       int *reply_data);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/measurement-inl.h b/src/measurement-inl.h
new file mode 100644
index 0000000..9762cd1
--- /dev/null
+++ b/src/measurement-inl.h
@@ -0,0 +1,62 @@
+/* measurement-inl.h -- data sets we gather
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_MEASUREMENT_INL_H
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_MEASUREMENT_INL_H
+
+std::string Pose::csvHeader() const {
+    return csvHeader_;
+}
+
+std::string Pose::csv() const {
+#   ifdef HAVE_CXX11_INITIALIZER_LISTS
+        return csv::fromContainer(std::vector<float> {time, y, x, theta});
+#   else
+        return csv::fromContainer(makeList<float>(nCols(), time, y, x, theta));
+#   endif
+}
+
+unsigned int Pose::nCols() const {
+    return 4;
+}
+
+std::string ControlSignals::csvHeader() const {
+    return csvHeader_;
+}
+
+std::string ControlSignals::csv() const {
+#   ifdef HAVE_CXX11_INITIALIZER_LISTS
+        return csv::fromContainer(
+            std::vector<float> {time, speed, steeringAngle});
+#   else
+        return csv::fromContainer(
+            makeList<float>(nCols(), time, speed, steeringAngle));
+#   endif
+}
+
+unsigned int ControlSignals::nCols() const {
+    return 3;
+}
+
+unsigned int LidarDatum::nCols() const {
+    return 1 + distance.size() + intensity.size();
+}
+
+#endif
diff --git a/src/measurement.cpp b/src/measurement.cpp
new file mode 100644
index 0000000..bcf20c3
--- /dev/null
+++ b/src/measurement.cpp
@@ -0,0 +1,68 @@
+/* measurement.cpp -- data sets we gather
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <array>
+#include <string>
+#include <vector>
+
+#include "csv.h"
+#include "measurement.h"
+
+const std::string Pose::csvHeader_ = "TimeGPS,GPSLat,GPSLon,Orientation";
+
+const std::string ControlSignals::csvHeader_ = "Time_VS,Velocity,Steering";
+
+std::string LidarDatum::csvHeader() const {
+    std::string result;
+    const std::string timeLaserHeader = "TimeLaser";
+    const std::string distanceHeader = "Laser";
+    const std::string intensityHeader = "Intensity";
+    result.reserve(timeLaserHeader.size()
+                   + distance.size() * (1 + distanceHeader.size())
+                   + intensity.size() * (1 + intensityHeader.size()));
+    result.append(timeLaserHeader);
+    for (std::vector<float>::size_type i = 0; i < distance.size(); i++) {
+        result.append(",");
+        result.append(distanceHeader);
+    }
+    for (std::vector<float>::size_type i = 0; i < intensity.size(); i++) {
+        result.append(",");
+        result.append(intensityHeader);
+    }
+    return result;
+}
+
+std::string LidarDatum::csv() const {
+    std::string result = std::to_string(time);
+    /* Yes, there are double braces in the initializer on the next line.  See
+     * <http://stackoverflow.com/questions/11400090>. */
+    const std::array<std::vector<float>, 2> dataSets = {{distance, intensity}};
+    for (const std::vector<float> &data : dataSets) {
+        if (! data.empty()) {
+            result.append(",");
+            result.append(csv::fromContainer(data));
+        }
+    }
+    return result;
+}
diff --git a/src/measurement.h b/src/measurement.h
new file mode 100644
index 0000000..bad199c
--- /dev/null
+++ b/src/measurement.h
@@ -0,0 +1,93 @@
+/* measurement.h -- data sets we gather
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_MEASUREMENT_H
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_MEASUREMENT_H
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <string>
+#include <vector>
+
+#include "csv.h"
+
+// Base class for time-series data
+struct Datum {
+    inline Datum(float time)
+        : time(time) {
+    }
+    float time;
+};
+
+
+// Datum definitions //
+
+class Pose : public Datum, public csv::Datum {
+public:
+    inline Pose(float time, float x, float y, float theta)
+        : ::Datum(time), x(x), y(y), theta(theta) {
+    }
+    float x;
+    float y;
+    float theta;
+    inline virtual std::string csvHeader() const;
+    inline virtual std::string csv() const;
+    inline virtual unsigned int nCols() const;
+
+protected:
+    static const std::string csvHeader_;
+};
+
+
+class ControlSignals : public Datum, public csv::Datum {
+public:
+    inline ControlSignals(float time, float speed, float steeringAngle)
+        : ::Datum(time), speed(speed), steeringAngle(steeringAngle) {
+    }
+    float speed;
+    float steeringAngle;
+    inline virtual std::string csvHeader() const;
+    inline virtual std::string csv() const;
+    inline virtual unsigned int nCols() const;
+
+protected:
+    static const std::string csvHeader_;
+};
+
+
+class LidarDatum : public Datum, public csv::Datum {
+public:
+    inline LidarDatum(float time, std::vector<float> distance,
+                      std::vector<float> intensity)
+        : ::Datum(time), distance(distance), intensity(intensity) {
+    }
+    std::vector<float> distance;
+    std::vector<float> intensity;
+    virtual std::string csvHeader() const;
+    inline virtual std::string csv() const;
+    inline virtual unsigned int nCols() const;
+};
+
+
+#include "measurement-inl.h"
+
+#endif
diff --git a/src/noise-inl.h b/src/noise-inl.h
new file mode 100644
index 0000000..5013a3c
--- /dev/null
+++ b/src/noise-inl.h
@@ -0,0 +1,51 @@
+/* noise-inl.h -- noisy data
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_NOISE_INL_H
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_NOISE_INL_H
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+template<typename RealType>
+GaussianNoiseSource<RealType>::GaussianNoiseSource(RealType mean,
+                                                   RealType stddev)
+	: generator(), distribution(mean, stddev) {
+    generator.seed(std::random_device()());
+}
+
+template<typename RealType>
+RealType GaussianNoiseSource<RealType>::get() {
+    return distribution(generator);
+}
+
+GaussianNoiseSource<float> gaussian(const std::vector<float> params) {
+    if (params.size() != 2) {
+        throw std::invalid_argument(std::string()
+            + "expected two-element vector of distribution parameters "
+            + "(got " + std::to_string(params.size()) + "-element "
+            + "vector instead");
+    } else {
+        return GaussianNoiseSource<float>(params[0], params[1]);
+    }
+}
+
+#endif
diff --git a/src/noise.cpp b/src/noise.cpp
new file mode 100644
index 0000000..bee0224
--- /dev/null
+++ b/src/noise.cpp
@@ -0,0 +1,73 @@
+/* noise.h -- noisy data
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#ifdef HAVE_CXX11_CLOSURES
+#   include <algorithm>
+#endif
+
+#include "measurement.h"
+#include "noise.h"
+
+Pose addNoise(const Pose &pose,
+              GaussianNoiseSource<float> &positionNoise,
+              GaussianNoiseSource<float> &angleNoise) {
+    Pose result = pose;
+    result.x += positionNoise.get();
+    result.y += positionNoise.get();
+    result.theta += angleNoise.get();
+    return result;
+}
+
+ControlSignals addNoise(const ControlSignals &signals,
+                        GaussianNoiseSource<float> &speedNoise,
+                        GaussianNoiseSource<float> &steeringAngleNoise) {
+    ControlSignals result = signals;
+    result.speed += speedNoise.get();
+    result.steeringAngle += steeringAngleNoise.get();
+    return result;
+}
+
+LidarDatum addNoise(const LidarDatum &datum,
+                    GaussianNoiseSource<float> &distanceNoise,
+                    GaussianNoiseSource<float> &intensityNoise) {
+    LidarDatum result = datum;
+#   ifdef HAVE_CXX11_CLOSURES
+        std::for_each(result.distance.begin(), result.distance.end(),
+                      [&distanceNoise](float &d) {
+                          d += distanceNoise.get();
+                      });
+        std::for_each(result.intensity.begin(), result.intensity.end(),
+                      [&intensityNoise](float &i) {
+                          i += intensityNoise.get();
+                      });
+#   else
+        for (float &d : result.distance) {
+            d += distanceNoise.get();
+        }
+        for (float &i : result.intensity) {
+            i += intensityNoise.get();
+        }
+#   endif
+    return result;
+}
diff --git a/src/noise.h b/src/noise.h
new file mode 100644
index 0000000..e0b9980
--- /dev/null
+++ b/src/noise.h
@@ -0,0 +1,72 @@
+/* noise.h -- noisy data
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_NOISE_H
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_NOISE_H
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <random>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "csv.h"
+#include "measurement.h"
+
+
+// Noise sources //
+
+template<typename RealType = double>
+class GaussianNoiseSource {
+public:
+    inline GaussianNoiseSource(RealType mean, RealType stddev);
+    inline RealType get();
+
+private:
+    std::mt19937 generator;
+    std::normal_distribution<RealType> distribution;
+};
+
+/* Convenience function to construct a GaussianNoiseSource<float> from a
+ * two-element float vector representing mean and standard deviation.
+ * Throws a std::invalid_argument if the vector is of the wrong
+ * length. */
+inline GaussianNoiseSource<float> gaussian(const std::vector<float>);
+
+
+// Noisy values //
+
+// Adds noise to a value.
+Pose addNoise(const Pose &, GaussianNoiseSource<float> &positionNoise,
+              GaussianNoiseSource<float> &angleNoise);
+ControlSignals addNoise(const ControlSignals &,
+                        GaussianNoiseSource<float> &speedNoise,
+                        GaussianNoiseSource<float> &steeringAngleNoise);
+LidarDatum addNoise(const LidarDatum &,
+                    GaussianNoiseSource<float> &distanceNoise,
+                    GaussianNoiseSource<float> &intensityNoise);
+
+
+#include "noise-inl.h"
+
+#endif
diff --git a/src/vrep-inl.h b/src/vrep-inl.h
new file mode 100644
index 0000000..84d8ef0
--- /dev/null
+++ b/src/vrep-inl.h
@@ -0,0 +1,118 @@
+/* vrep-inl.h -- convenience code to interface with V-REP
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_VREP_INL_H_
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_VREP_INL_H_
+
+namespace vrep {
+
+    // class InterfaceLockGuard
+
+    InterfaceLockGuard::InterfaceLockGuard() {
+        lock();
+    }
+
+    InterfaceLockGuard::InterfaceLockGuard(const InterfaceLockGuard &other) {
+        copyState(other);
+    }
+
+    InterfaceLockGuard &InterfaceLockGuard::operator=(
+        const InterfaceLockGuard &other) {
+        copyState(other);
+        return *this;
+    }
+
+    InterfaceLockGuard::InterfaceLockGuard(InterfaceLockGuard &&other) {
+        moveState(other);
+    }
+
+    InterfaceLockGuard &InterfaceLockGuard::operator=(
+        InterfaceLockGuard &&other) {
+        moveState(other);
+        return *this;
+    }
+
+    void InterfaceLockGuard::lock() {
+        VREP(simLockInterface(true));
+        isLocked = true;
+    }
+
+    void InterfaceLockGuard::unlock() {
+        VREP(simLockInterface(false));
+        isLocked = false;
+    }
+
+
+    // class Error
+
+    Error::Error(const std::string &whatArg)
+        : std::runtime_error(whatArg) {
+    }
+
+    Error::Error(const char *whatArg)
+        : Error(std::string(whatArg)) {
+    }
+
+
+    // template<typename T> class FunctionError<T>
+
+    template<typename T>
+    FunctionError<T>::FunctionError(const std::string &invocation,
+                                        const T result)
+        : Error("V-REP function invocation "
+                + invocation
+                + " failed, returning "
+                + std::to_string(result)) {
+    }
+
+    template<typename T>
+    FunctionError<T>::FunctionError(const char *invocation, const T result)
+        : FunctionError(std::string(invocation), result) {
+    }
+
+
+    // template<typename T> T checkFunction(T, const char[])
+
+    template<typename T>
+    T checkFunction(T result, const char invocation[]) {
+        if (isSuccess(result)) {
+            return result;
+        } else {
+            throw vrep::FunctionError<T>(invocation, result);
+        }
+    }
+
+
+    // template<typename T> bool isSuccess(const T)
+
+    template<>
+    bool isSuccess(const simInt val) {
+        return val != -1;
+    }
+
+    template<>
+    bool isSuccess(const simVoid *const val) {
+        return val != nullptr;
+    }
+
+}
+
+#endif
+
diff --git a/src/vrep.cpp b/src/vrep.cpp
new file mode 100644
index 0000000..2043b45
--- /dev/null
+++ b/src/vrep.cpp
@@ -0,0 +1,72 @@
+/* vrep.cpp -- convenience code to interface with V-REP
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <stdexcept>
+#include <string>
+
+#include <v_repLib.h>
+
+#include "vrep.h"
+
+namespace vrep {
+
+    InterfaceLockGuard::~InterfaceLockGuard() noexcept {
+        if (isLocked) {
+            /* Throwing an exception from a destructor is a bad, bad idea.  See
+             * /More Effective C++/ #14. */
+            try {
+                unlock();
+            } catch (const Error& error) {
+                /* Unlocking failed, so we're kind of screwed at this point.  We
+                 * can't even display a dialog to the user noting the failure,
+                 * because the interface is locked! *sigh*  Just give up and do
+                 * nothing. */
+            }
+        }
+    }
+
+    void InterfaceLockGuard::copyState(const InterfaceLockGuard &other) {
+        if (isLocked) {
+            unlock();
+        }
+        if (other.isLocked) {
+            lock();
+        } else {
+            isLocked = false;
+        }
+    }
+
+    void InterfaceLockGuard::moveState(InterfaceLockGuard &other) {
+        if (isLocked) {
+            unlock();
+        }
+        if (other.isLocked) {
+            lock();
+            other.unlock();
+        } else {
+            isLocked = false;
+        }
+    }
+
+}
diff --git a/src/vrep.h b/src/vrep.h
new file mode 100644
index 0000000..7a1bac0
--- /dev/null
+++ b/src/vrep.h
@@ -0,0 +1,104 @@
+/* vrep.h -- convenience code to interface with V-REP
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_VREP_H_
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_VREP_H_
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <cstddef>
+
+#include <stdexcept>
+#include <string>
+
+#include <v_repLib.h>
+
+/* All the convenience code lives in the 'vrep' namespace to distinguish it from
+ * the actual V-REP functions, which all are prefixed with 'sim'. */
+namespace vrep {
+
+    // A RAII wrapper for 'simLockInterface'.
+    class InterfaceLockGuard {
+    public:
+        inline InterfaceLockGuard();
+        ~InterfaceLockGuard() noexcept;
+        // Copying
+        inline InterfaceLockGuard(const InterfaceLockGuard &);
+        inline InterfaceLockGuard &operator=(const InterfaceLockGuard &);
+        // Moving
+        inline InterfaceLockGuard(InterfaceLockGuard &&);
+        inline InterfaceLockGuard &operator=(InterfaceLockGuard &&);
+
+    private:
+        inline void lock();
+        inline void unlock();
+        void copyState(const InterfaceLockGuard &);
+        void moveState(InterfaceLockGuard &);
+
+        bool isLocked;
+    };
+
+
+    // Error handling //
+
+    // The class for all errors which occur when interfacing with V-REP.
+    class Error : public std::runtime_error {
+    public:
+        explicit inline Error(const std::string &whatArg);
+        explicit inline Error(const char *whatArg);
+    };
+
+    // An error signaling a failed V-REP function invocation.
+    template<typename T>
+    class FunctionError : public Error {
+    public:
+        FunctionError(const std::string &invocation, const T result);
+        inline FunctionError(const char *invocation, const T result);
+    };
+
+/* A nice macro to convert failed V-REP function invocations to
+ * 'FunctionError's.  Simply wrap your V-REP function interface--e.g.,
+ *
+ *     simInt x = VREP(simGetIntegerParameter(foo, &bar));
+ *
+ * --and you'll be off to the races.  'x' will be assigned the result of the
+ * invocation if and only if the invocation succeeded; otherwise, a
+ * 'FunctionError' describing the failed invocation will be thrown.
+ *
+ * This needs to be a macro so that it can include the code of the failed
+ * invocation itself. */
+#define VREP(invocation) vrep::checkFunction((invocation), #invocation)
+
+    template<typename T>
+    T checkFunction(T result, const char invocation[]);
+
+    /* Returns 'true' iff the passed value signifies a successful V-REP function
+     * invocation. */
+    template<typename T>
+    inline bool isSuccess(const T);
+    template<> inline bool isSuccess(const simInt);
+    template<> inline bool isSuccess(const simVoid *);
+}
+
+#include "vrep-inl.h"
+
+#endif
diff --git a/src/vrepFfi-inl.h b/src/vrepFfi-inl.h
new file mode 100644
index 0000000..554d954
--- /dev/null
+++ b/src/vrepFfi-inl.h
@@ -0,0 +1,133 @@
+/* vrepFfi-inl.h -- exposing functions to V-REP
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_VREPFFI_INL_H_
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_VREPFFI_INL_H_
+
+namespace vrep {
+
+    namespace {
+
+        // Converting a variadic template type to a Lua type list //
+
+        std::forward_list<int> toLuaTypes() {
+            return std::forward_list<int>();
+        }
+
+        template<typename Car>
+        std::forward_list<int> toLuaTypes() {
+            std::forward_list<int> luaTypes = toLuaTypes();
+            luaTypes.push_front(LuaType<Car>::id);
+            return luaTypes;
+        }
+
+        template<typename Car, typename Cadr, typename ...Cddr>
+        std::forward_list<int> toLuaTypes() {
+            std::forward_list<int> luaTypes = toLuaTypes<Cadr, Cddr...>();
+            luaTypes.push_front(LuaType<Car>::id);
+            return luaTypes;
+        }
+
+    }
+
+
+    // Exposing C++ functions to Lua //
+
+    template<typename ...Args>
+    void exposeFunction(const std::string name, const std::string callTips,
+                        simVoid (*const f)(SLuaCallBack *)) {
+        // Compute the Lua signature.
+        std::vector<int> luaTypes;
+        luaTypes.push_back(0);
+        for (int type : toLuaTypes<Args...>()) {
+            luaTypes.push_back(type);
+            // We just added an argument.
+            luaTypes[0]++;
+        }
+        // Register the function with Lua.
+        VREP(simRegisterCustomLuaFunction(name.c_str(), callTips.c_str(),
+                                          luaTypes.data(), f));
+    }
+
+
+    // Extracting Lua arguments from the callback structure //
+
+    template<typename T>
+    T LuaCall::expectAtom() {
+        ensureNextArgType(LuaType<T>::id);
+        const T result = unsafeGetAtom<T>();
+        argIdx++;
+        return result;
+    }
+
+    template<typename T>
+    std::vector<T> LuaCall::expectTable() {
+        ensureNextArgType(LuaType<std::vector<T>>::id);
+        const std::vector<T> result = unsafeGetTable<T>();
+        argIdx++;
+        return result;
+    }
+
+    template<typename T>
+    std::vector<T> LuaCall::unsafeGetTable() {
+        // Determine how long the table is.
+        size_t tableLen;
+        if (simCall->inputArgTypeAndSize[2 * argIdx] & sim_lua_arg_table) {
+            tableLen = simCall->inputArgTypeAndSize[2 * argIdx + 1];
+        } else {
+            tableLen = 1;
+        }
+        // Build the result.
+        std::vector<T> result(tableLen);
+        for (size_t i = 0; i < tableLen; i++) {
+            result[i] = unsafeGetAtom<T>();
+        }
+        return result;
+    }
+
+    template<>
+    bool LuaCall::unsafeGetAtom() {
+        return *cursorBool++;
+    }
+
+    template<>
+    int LuaCall::unsafeGetAtom() {
+        return *cursorInt++;
+    }
+
+    template<>
+    float LuaCall::unsafeGetAtom() {
+        return *cursorFloat++;
+    }
+
+
+    // Error handling //
+
+    MarshalingError::MarshalingError(const std::string &whatArg)
+        : Error(whatArg) {
+    }
+
+    MarshalingError::MarshalingError(const char *whatArg)
+        : MarshalingError(std::string(whatArg)) {
+    }
+
+}
+
+#endif
diff --git a/src/vrepFfi.cpp b/src/vrepFfi.cpp
new file mode 100644
index 0000000..4641bbf
--- /dev/null
+++ b/src/vrepFfi.cpp
@@ -0,0 +1,68 @@
+/* vrepFfi.cpp -- exposing functions to V-REP
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <cstring>
+
+#include <string>
+
+#include "vrepFfi.h"
+
+namespace vrep {
+
+    // Extracting Lua arguments from the callback structure //
+
+    LuaCall::LuaCall(SLuaCallBack *simCall)
+        : simCall(simCall), argIdx(0), cursorBool(simCall->inputBool),
+          cursorInt(simCall->inputInt), cursorFloat(simCall->inputFloat),
+          cursorChar(simCall->inputChar) {
+    }
+
+    void LuaCall::ensureNextArgType(const int expected) {
+        const int nextType = simCall->inputArgTypeAndSize[2 * argIdx];
+        if (nextType != expected) {
+            throw MarshalingError("unexpected Lua argument (expected type "
+                                  + std::to_string(expected)
+                                  + ", got type "
+                                  + std::to_string(nextType)
+                                  + ")");
+        }
+    }
+
+    template<>
+    std::string LuaCall::unsafeGetAtom() {
+        // Determine how long the string is.  Lua uses '@' as its sentinel.
+        size_t len;
+        simChar *sentinel = std::strchr(cursorChar, '@');
+        if (sentinel) {
+            len = sentinel - cursorChar;
+        } else {
+            len = std::strlen(cursorChar);
+        }
+        // Build the 'std::string'.
+        const std::string result = std::string(cursorChar, len);
+        cursorChar += len + (sentinel ? 1 : 0);
+        return result;
+    }
+
+}
diff --git a/src/vrepFfi.h b/src/vrepFfi.h
new file mode 100644
index 0000000..0dbab1e
--- /dev/null
+++ b/src/vrepFfi.h
@@ -0,0 +1,158 @@
+/* vrepFfi.h -- exposing functions to V-REP
+ * Copyright (C) 2013  Galois, Inc.
+ *
+ * This library is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * To contact Galois, complete the Web form at <http://corp.galois.com/contact/>
+ * or write to Galois, Inc., 421 Southwest 6th Avenue, Suite 300, Portland,
+ * Oregon, 97204-1622. */
+
+#ifndef PPAML_VREP_AUTOMOBILE_PLUGIN_VREPFFI_H_
+#define PPAML_VREP_AUTOMOBILE_PLUGIN_VREPFFI_H_
+
+#ifdef HAVE_CONFIG_H
+#   include <config.h>
+#endif
+
+#include <cstddef>
+#include <cstring>
+
+#include <forward_list>
+#include <string>
+#include <vector>
+
+#include <v_repLib.h>
+
+#include "vrep.h"
+
+namespace vrep {
+
+    namespace {
+
+        // C-Lua type equivalences //
+
+        template<typename T, int idVal>
+        struct LuaTypeWrapper {
+            static const int id = idVal;
+            static inline T isNextArg(const SLuaCallBack *);
+        };
+
+        template<typename T>
+        struct LuaType;
+
+        template<>
+        struct LuaType<std::nullptr_t>
+            : public LuaTypeWrapper<std::nullptr_t, sim_lua_arg_nil> {
+        };
+
+        template<>
+        struct LuaType<bool> : public LuaTypeWrapper<bool, sim_lua_arg_bool> {
+            typedef simBool LuaT;
+        };
+
+        template<>
+        struct LuaType<int> : public LuaTypeWrapper<int, sim_lua_arg_int> {
+            typedef simInt LuaT;
+        };
+
+        template<>
+        struct LuaType<float>
+            : public LuaTypeWrapper<float, sim_lua_arg_float> {
+            typedef simFloat LuaT;
+        };
+
+        template<>
+        struct LuaType<std::string>
+            : public LuaTypeWrapper<std::string, sim_lua_arg_string> {
+            typedef simChar LuaT;
+       };
+
+        template<typename U>
+        struct LuaType<std::vector<U>>
+            : public LuaTypeWrapper<std::vector<U>,
+                                    sim_lua_arg_table | LuaType<U>::id> {
+            typedef typename LuaType<U>::LuaT LuaT;
+        };
+
+
+        // Converting a variadic template type to a Lua type list //
+
+        std::forward_list<int> toLuaTypes();
+
+        template<typename Car>
+        std::forward_list<int> toLuaTypes();
+
+        template<typename Car, typename Cadr, typename ...Cddr>
+        std::forward_list<int> toLuaTypes();
+    }
+
+
+    // Exposing C++ functions to Lua //
+
+    template<typename ...Args>
+    void exposeFunction(const std::string name, const std::string callTips,
+                        simVoid (*const f)(SLuaCallBack *));
+
+
+    // Extracting Lua arguments from the callback structure //
+
+    /* A class layered on 'SLuaCallBack' for handling calls from Lua into C++.
+     * Watch out--interacting with this class at all outside of a C++ function
+     * exposed to Lua will almost certainly cause undefined behavior! */
+    class LuaCall {
+    public:
+        LuaCall(SLuaCallBack *);
+
+        template<typename T>
+        T expectAtom();
+
+        template<typename T>
+        std::vector<T> expectTable();
+
+    private:
+        void ensureNextArgType(const int expected);
+
+        template<typename T>
+        T unsafeGetAtom();
+
+        template<typename T>
+        std::vector<T> unsafeGetTable();
+
+        SLuaCallBack *const simCall;
+        size_t argIdx;
+        simBool *cursorBool;
+        simInt *cursorInt;
+        simFloat *cursorFloat;
+        simChar *cursorChar;
+    };
+    template<> inline bool LuaCall::unsafeGetAtom();
+    template<> inline int LuaCall::unsafeGetAtom();
+    template<> inline float LuaCall::unsafeGetAtom();
+    template<> std::string LuaCall::unsafeGetAtom();
+
+
+    // Error handling //
+
+    // An error signaling a failed marshal from Lua to C++.
+    class MarshalingError : public Error {
+    public:
+        explicit inline MarshalingError(const std::string &whatArg);
+        explicit inline MarshalingError(const char *whatArg);
+    };
+
+}
+
+#include "vrepFfi-inl.h"
+
+#endif