Monday, December 21, 2015

FFT-based cosine similarity for fast template matching

A common task in computer vision applications is template matching, the search for a comparatively small image patch – or more likely, its closest approximation – in a larger image. In its simplest form, this implies solving the following optimization:
\begin{equation}
(i^*, j^*) = \arg \max_{i,j} s(T, P_{i,j})
\label{eq:tm}
\end{equation}
Where the similarity metric \(s(T, P_{i,j})\) is defined between a template \(T^{m_t \times n_t}\) and all patches \(P^{m_t \times n_t}_{i,j}\) such that:
\begin{equation}
P^{m_t \times n_t}_{i,j} = I[i:i+m_t,j:j+n_t]
\label{eq:p}
\end{equation}
For an image \(I^{m \times n}\) with \(m_t \ll m\) and \(n_t \ll n\) (i.e. the image is much larger than the template). A common choice of similarity metric is cosine similarity, which is defined as:
\begin{equation}
cos(A, B) = \frac{A \cdot B}{\|A\| \|B\|} = \frac{\displaystyle\sum^{m,n}_{i,j} A[i,j]B[i,j]}{\sqrt{\displaystyle\sum^{m,n}_{i,j} A[i,j]^2 \displaystyle\sum^{m,n}_{i,j} B[i,j]^2}}
\label{eq:cos}
\end{equation}
Cosine similarity is a reliable metric, but as defined above its evaluation is rather costly. Combining formulas \eqref{eq:tm}, \eqref{eq:p} and \eqref{eq:cos} gives:
\begin{equation}
(i^*, j^*) = \arg \max_{i,j} \frac{\displaystyle\sum^{m_t,n_t}_{i_t,j_t} T[i_t,j_t] I[i + i_t, j + j_t]}{\sqrt{\displaystyle\sum^{m_t,n_t}_{i_t,j_t} T[i_t,j_t]^2 \displaystyle\sum^{m_t,n_t}_{i_t,j_t} I[i + i_t, j + j_t]^2}}
\label{eq:tm_cos}
\end{equation}
Which is of time complexity \(\mathrm{O}(m \, n \, m_tn_t)\) – i.e. the size of the template times the size of the image. Depending on image and template sizes, the intended use case (e.g. batch vs. real-time) or scale of the template matching task (e.g. a couple dozen vs. thousands of images), this may incur in prohibitive processing costs.

Cosine similarity template matching can be sped up with application of the convolution theorem. First, let's redefine formula \eqref{eq:tm_cos} as:
\begin{equation}
(i^*, j^*) = \arg \max_{i,j} \frac{D_{T, I}[i,j]}{\sqrt{m_T M_I[i,j]}}
\label{eq:tm_cos_2}
\end{equation}
Where:
\begin{equation}
D_{T, I}[i,j] = \displaystyle\sum^{m_t,n_t}_{i_t,j_t} T[i_t,j_t] I[i + i_t, j + j_t]
\label{eq:D_TI}
\end{equation}
\begin{equation}
m_T = \displaystyle\sum^{m_t,n_t}_{i_t,j_t} T[i_t,j_t]^2
\label{eq:m_T}
\end{equation}
\begin{equation}
M_I[i,j] = \displaystyle\sum^{m_t,n_t}_{i_t,j_t} I[i + i_t, j + j_t]^2
\label{eq:M_I}
\end{equation}
Looking into the three terms above, it's clear that \eqref{eq:m_T} is constant for any given \(T\) and can therefore be left out of the computation. On the other hand, \eqref{eq:D_TI} is just the cross-correlation between \(T\) and \(I\), which by the convolution theorem can also be computed as:
\begin{equation}
D_{T, I} = \mathcal{F}^{-1}(\mathcal{F}(T)^{*} \circ \mathcal{F}(I))
\end{equation}
Where \(\mathcal{F}\) is the fourier transform operator, \(\mathcal{F}^{-1}\) the inverse transform, the asterisk denotes the complex conjugate, and \(\circ\) is the Hadamard product (element-wise multiplication) of the two transforms. Likewise, \eqref{eq:M_I} can be computed as the cross-correlation between \(I^2\) and a window filter \(W^{m_t \times n_t} = [w_{ij} = 1]\):
\begin{equation}
M_I = \mathcal{F}^{-1}(\mathcal{F}(W)^{*} \circ \mathcal{F}(I^2))
\end{equation}
The advantage of this approach is that algorithms such as the Fast Fourier Transform (FFT) are of time complexity \(\mathrm{O}(m \, n \, log \, m \, n)\), which depending on the relative sizes of \(T\) and \(I\) may be faster than the \(\mathrm{O}(m \, n \, m_t n_t)\) of the direct computation. Also the Fourier transforms of \(W\) and (if the same template will be applied to several images) \(T\) can be cached, further saving up computation time.

Implementation

The C++ code below provides a basic implementation of the method outlined above. It uses the popular OpenCV library, with some further optimizations particular to its implementation of the Fourier transform, explained in the comments.
#include <opencv2/opencv.hpp>

static const cv::Scalar ONE(1);

static const cv::Scalar ZERO(0);

static const cv::Scalar WHITE(255, 255, 255);

// Fourier transform performance is not a monotonic function of a vector
// size - matrices whose dimensions are powers of two are the fastest to
// process, and multiples of 2, 3 and 5 (for example, 300 = 5*5*3*2*2) are
// also processed quite efficiently. Therefore it makes sense to pad input
// data with zeros to get a bit larger matrix that can be transformed much
// faster than the original one.
cv::Size fit(const cv::Size &size)
{
  return cv::Size(cv::getOptimalDFTSize(size.width),
                  cv::getOptimalDFTSize(size.height));
}

cv::Mat F_fwd(const cv::Mat &I, const cv::Size &size)
{
  // Pad input matrix to given size.
  cv::Mat P;
  int m = size.height - I.rows;
  int n = size.width - I.cols;
  cv::copyMakeBorder(I, P, 0, m, 0, n, cv::BORDER_CONSTANT, ZERO);

  // Compute Fourier transform for input data. The last argument
  // informs the dft() function of how many non-zero rows are there,
  // so it can handle the rest of the rows more efficiently and save
  // some time.
  cv::Mat F;
  cv::dft(P, F, 0, I.rows);

  return F;
}

cv::Mat F_inv(const cv::Mat &F, const cv::Size &size)
{
  // Compute inverse Fourier transform for input data. The last
  // argument informs the dft() function of how many non-zero
  // rows are expected in the output, so it can handle the rest
  // of the rows more efficiently and save some time.
  cv::Mat I;
  cv::dft(F, I, cv::DFT_INVERSE + cv::DFT_SCALE, size.height);

  return I(cv::Rect(0, 0, size.width, size.height));
}

cv::Mat C(const cv::Mat &T, const cv::Mat &I, const cv::Size &size)
{
  // Compute the Fourier transforms of template and image.
  cv::Mat F_T = F_fwd(T, size);
  cv::Mat F_I = F_fwd(I, size);

  // Compute the cross correlation in the frequency domain.
  cv::Mat F_TI;
  cv::mulSpectrums(F_I, F_T, F_TI, 0, true);

  // Compute the inverse Fourier transform of the cross-correlation,
  // dismissing those rows and columns of the cross-correlation
  // matrix that would require the template to "roll over" the image.
  cv::Size clipped;
  clipped.width = I.cols - T.cols;
  clipped.height = I.rows - T.rows;
  return F_inv(F_TI, clipped);
}

cv::Mat W(const cv::Mat &T)
{
  return cv::Mat(T.size(), CV_64F, ONE);
}

cv::Point3f matchTemplate(const cv::Mat &T, const cv::Mat &I)
{
  // Compute the optimal size for DFT computing.
  cv::Size size = fit(I.size());

  //Compute the cross-correlation and normalizing matrix.
  cv::Mat C_TI = C(T, I, size);
  cv::Mat M_I = C(W(T), I.mul(I), size);

  int i_s, j_s;
  float r = 0;
  int rows = C_TI.rows;
  int cols = C_TI.cols;
  for (int i = 0; i < rows; i++)
  {
    for (int j = 0; j < cols; j++)
    {
      float v = C_TI.at(i, j) / sqrt(M_I.at(i, j));
      if (r < v)
      {
        r = v;
        i_s = i;
        j_s = j;
      }
    }
  }

  return cv::Point3f(j_s, i_s, r);
}

cv::Mat L(const cv::Mat &I)
{
  cv::Mat L_I, L_F;
  cv::cvtColor(I, L_I, CV_BGR2GRAY);
  L_I.convertTo(L_F, CV_64F);
  return L_F;
}

int main(int argc, char *argv[])
{
  cv::Mat T = cv::imread(argv[1]);
  cv::Mat I = cv::imread(argv[2]);

  cv::Point3f match = matchTemplate(L(T), L(I));
  cv::rectangle(I, cv::Rect(match.x, match.y, T.cols, T.rows), WHITE);

  cv::imshow("Template", T);
  cv::imshow("Image", I);
  cv::waitKey();

  return 0;
}

Thursday, May 21, 2015

Controlling a differential drive robot in Gazebo from ROS

Gazebo is a simulation suite targeted at robotics projects, used by DARPA to implement the virtual part of its Robotics Challenge. It's a great companion to the Robot Operating System, supporting several ways to interface with it.

One recurrent question regarding Gazebo is how to control robots described in Simulation Description Format (SDF) models from ROS. The more involved case of porting a URDF model to Gazebo and manipulating it through ROS Control interfaces is well documented, but what about a robot that already has a full SDF model available? Isn't it possible to just paste it into a Gazebo workspace and get some ROS topics to manipulate it?

The tutorial below covers this simpler case. It is based on the SDF model for the Pioneer 2DX differential drive robot, which is available from Gazebo's default library. It also assumes ROS and Gazebo are already installed, as well as the necessary integration packages and plugins.

First, save the SDF world model below to your local filesystem as pioneer2dx_ros.world:
<?xml version="1.0" ?>
<sdf version='1.4'>
  <world name='default'>
    <light name='sun' type='directional'>
      <cast_shadows>1</cast_shadows>
      <pose>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>
    <model name='ground_plane'>
      <static>1</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>50</mu2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <cast_shadows>0</cast_shadows>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
    </model>
    <physics type='ode'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
      <gravity>0 0 -9.8</gravity>
    </physics>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>1</shadows>
    </scene>
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>0</latitude_deg>
      <longitude_deg>0</longitude_deg>
      <elevation>0</elevation>
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
    <model name='pioneer2dx'>
      <link name='chassis'>
        <pose>0 0 0.16 0 -0 0</pose>
        <inertial>
          <mass>5.67</mass>
          <inertia>
            <ixx>0.07</ixx>
            <iyy>0.08</iyy>
            <izz>0.1</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.445 0.277 0.17</size>
            </box>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <bounce/>
            <friction>
              <ode/>
            </friction>
            <contact>
              <ode/>
            </contact>
          </surface>
        </collision>
        <collision name='castor_collision'>
          <pose>-0.2 0 -0.12 0 -0 0</pose>
          <geometry>
            <sphere>
              <radius>0.04</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0</mu>
                <mu2>0</mu2>
                <slip1>1</slip1>
                <slip2>1</slip2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <pose>0 0 0.04 0 -0 0</pose>
          <geometry>
            <mesh>
              <uri>model://pioneer2dx/meshes/chassis.dae</uri>
            </mesh>
          </geometry>
        </visual>
        <visual name='castor_visual'>
          <pose>-0.2 0 -0.12 0 -0 0</pose>
          <geometry>
            <sphere>
              <radius>0.04</radius>
            </sphere>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/FlatBlack</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
      <link name='right_wheel'>
        <pose>0.1 -0.17 0.11 0 1.5707 1.5707</pose>
        <inertial>
          <mass>1.5</mass>
          <inertia>
            <ixx>0.0051</ixx>
            <iyy>0.0051</iyy>
            <izz>0.009</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <cylinder>
              <radius>0.11</radius>
              <length>0.05</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100000</mu>
                <mu2>100000</mu2>
                <slip1>0</slip1>
                <slip2>0</slip2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <geometry>
            <cylinder>
              <radius>0.11</radius>
              <length>0.05</length>
            </cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/FlatBlack</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
      <link name='left_wheel'>
        <pose>0.1 0.17 0.11 0 1.5707 1.5707</pose>
        <inertial>
          <mass>1.5</mass>
          <inertia>
            <ixx>0.0051</ixx>
            <iyy>0.0051</iyy>
            <izz>0.009</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <cylinder>
              <radius>0.11</radius>
              <length>0.05</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100000</mu>
                <mu2>100000</mu2>
                <slip1>0</slip1>
                <slip2>0</slip2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <geometry>
            <cylinder>
              <radius>0.11</radius>
              <length>0.05</length>
            </cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/FlatBlack</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
      <joint name='left_wheel_hinge' type='revolute'>
        <pose>0 0 -0.03 0 -0 0</pose>
        <child>left_wheel</child>
        <parent>chassis</parent>
        <axis>
          <xyz>0 1 0</xyz>
          <limit>
            <lower>-1e+16</lower>
            <upper>1e+16</upper>
          </limit>
        </axis>
      </joint>
      <joint name='right_wheel_hinge' type='revolute'>
        <pose>0 0 0.03 0 -0 0</pose>
        <child>right_wheel</child>
        <parent>chassis</parent>
        <axis>
          <xyz>0 1 0</xyz>
          <limit>
            <lower>-1e+16</lower>
            <upper>1e+16</upper>
          </limit>
        </axis>
      </joint>
      <!-- Replaced Gazebo's differential drive plugin with the
           ROS-friendly variant -->
      <!--
      <plugin filename="libDiffDrivePlugin.so" name="diff_drive">
        <left_joint>left_wheel_hinge</left_joint>
        <right_joint>right_wheel_hinge</right_joint>
        <torque>5</torque>
      </plugin>
      -->
      <plugin name='differential_drive_controller'
              filename='libgazebo_ros_diff_drive.so'>
        <alwaysOn>true</alwaysOn>
        <updateRate>100</updateRate>
        <leftJoint>left_wheel_hinge</leftJoint>
        <rightJoint>right_wheel_hinge</rightJoint>
        <wheelSeparation>0.39</wheelSeparation>
        <wheelDiameter>0.15</wheelDiameter>
        <torque>5</torque>
        <commandTopic>cmd_vel</commandTopic>
        <odometryTopic>odom</odometryTopic>
        <odometryFrame>odom</odometryFrame>
        <robotBaseFrame>chassis</robotBaseFrame>
      </plugin>
      <pose>0 0 0 0 -0 0</pose>
      <static>0</static>
    </model>
    <state world_name='default'>
      <sim_time>85 304000000</sim_time>
      <real_time>85 849190220</real_time>
      <wall_time>1432260579 736436496</wall_time>
      <model name='ground_plane'>
        <pose>0 0 0 0 -0 0</pose>
        <link name='link'>
          <pose>0 0 0 0 -0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
      <model name='pioneer2dx'>
        <pose>-0.103826 0.027961 2e-06 -2e-06 -8e-06 -0.094162</pose>
        <link name='chassis'>
          <pose>-0.103827 0.027961 0.160002 -2e-06 -8e-06 -0.094162</pose>
          <velocity>
            -0.000797 0.002229 0.003363 -0.024657 -0.007301 0.0023
          </velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
        <link name='left_wheel'>
          <pose>0.011714 0.187806 0.110002 1.38258 1.57033 2.85912</pose>
          <velocity>
            -0.000816 0.001565 0.000275 -0.014307 -0.006987 0.002012
          </velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
        <link name='right_wheel'>
          <pose>
            -0.020254 -0.150688 0.110002 -1.56886 1.51434 -0.092322
          </pose>
          <velocity>
            -0.000347 0.001559 0.008433 -0.014387 -0.003243 0.000126
          </velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
    </state>
    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose>5 -5 2 0 0.275643 2.35619</pose>
        <view_controller>orbit</view_controller>
      </camera>
    </gui>
  </world>
</sdf>
Open a terminal window and start the ROS middleware by entering:
roscore
Open another terminal window, cd to the folder containing pioneer2dx_ros.world and enter:
rosrun gazebo_ros gazebo -file pioneer2dx_ros.world
You should now have a Gazebo window opened with a Pioneer 2DX placed in the middle of an empty world.

Finally, open a third terminal window (it's the last one, I promise) and check if the differential drive topics have been published:
rostopic list
The following topics should be visible, among others:
/pioneer2dx/cmd_vel
/pioneer2dx/odom
You should now be able to get the robot moving by publishing messages to the /pioneer2dx/cmd_vel topic, e.g.
rostopic pub -1 /pioneer2dx/cmd_vel geometry_msgs/Twist \
'{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}'
Should get the robot running in a loop.

Sunday, May 17, 2015

How to download all Gazebo models


Gazebo is a wonderful simulation tool widely used in robotics projects. The project includes a library of 3D models that are automatically downloaded on demand. However this makes the application pause each time a model is used for the first time, which can take a considerable time depending on connection speed and interfere with the flow of work.

In order to avoid this issue I wrote the below script to do a bulk download of all models in the Gazebo library, then copy them to the local model folder. Simply copy the lines below to a script file on a convenient local folder and run it from a command prompt.
#!/bin/sh

# Download all model archive files
wget -l 2 -nc -r "http://models.gazebosim.org/" --accept gz

# This is the folder into which wget downloads the model archives
cd "models.gazebosim.org"

# Extract all model archives
for i in *
do
  tar -zvxf "$i/model.tar.gz"
done

# Copy extracted files to the local model folder
cp -vfR * "$HOME/.gazebo/models/"

Monday, April 27, 2015

Building OpenCV's HTML documentation

OpenCV has become the de facto standard vision processing library, but the project remains surprisingly obscure in several areas, particularly in relation to building and installation tasks. For example, it took me a couple hours to discover how to build the project's HTML documentation (though to be fair, I was looking into it concurrently with other work).

For those also struggling with this, see this question in the OpenCV Q&A site. In short, it gives the following instructions:
  1. Install Sphinx;
  2. Run cmake on your OpenCV source base as explained in the installation instructions, perhaps adding the -D BUILD_DOCS=ON option for good measure;
  3. Run make html_docs to build the documentation in HTML format.
If everything goes well the HTML documentation will be built to the subfolder doc/_html/ inside the build output folder. If not, check if the output of the cmake call reported items Build Documentation and Sphinx both set to YES. You may also check the cmake script cmake/OpenCVDetectPython.cmake inside your OpenCV source base for potential problems.

Wednesday, April 8, 2015

Internet sharing for Intel Edison on Linux

Recently I got hold of an Intel Edison with accompanying Arduino Expansion Board. The awkward development setup process (which requires two separate USB cable connections to a host computer) didn't make for a stellar first impression, but the possibilities of a full x86-64 computer that fits in a matchbox are intriguing. Of course, the fact both Linux and ROS are more-or-less readily available for the platform made the whole proposition a lot more interesting.

After flashing Edison with ubilinux the first order of business was getting the thing connected to the Internet. Edison includes an on-board wi-fi card and can simulate an ethernet port through its USB connections to the host computer, but bureaucratic questions would make a direct connection to my lab's network difficult. So I set up my notebook (Edison's host computer) as a local gateway, enabling Internet sharing through the virtual usb0 interface.

The procedure documented below was developed on a Kubuntu 14.04 host and ubilinux Edison unit, but it should be adaptable to other Linux distributions. It is largely based on a previous post on wireless Internet sharing; in fact that may be a better option, if unlike me you have a wireless interface to spare.

First open a command prompt and start a root session with su, then follow the steps below:

Step 1: Install Dnsmasq

Dnsmasq is a tool for setting up small networks. Install it from the command line by typing:
apt-get install dnsmasq
As it is the dnsmasq service will be automatically started when the computer boots, but we need better control than that. So change the default settings to manual start:
update-rc.d -f dnsmasq remove

Step 2: Configuration Files

Create or update the following config file with the contents below:

/etc/dnsmasq.conf
# disables dnsmasq reading any other files like /etc/resolv.conf for nameservers
no-resolv
# Interface to bind to
interface=usb0
# Specify starting_range,end_range,lease_time
dhcp-range=usb0,192.168.100.100,192.168.100.199,12h
# dns addresses to send to the clients
server=8.8.8.8
server=8.8.4.4

Step 3: Interface Scripts

Add the scripts below to your system:

/usr/local/bin/usb0start.sh
#!/bin/sh

ETH=eth0
USB=usb0
IP=192.168.100.99
MASK=255.255.255.0

# Enable IP forwarding
echo 1 > /proc/sys/net/ipv4/ip_forward

# Setup USB interface
ifconfig $USB up $IP netmask $MASK

# Start dnsmasq
/etc/init.d/dnsmasq start

# Enable NAT
iptables --flush
iptables --table nat --flush
iptables --delete-chain
iptables --table nat --delete-chain
iptables --table nat --append POSTROUTING --out-interface $ETH -j MASQUERADE
iptables --append FORWARD --in-interface $USB -j ACCEPT

/usr/local/bin/usb0stop.sh
#!/bin/bash

USB=usb0

# Stops DNS making service
/etc/init.d/dnsmasq stop

# Asked nice the first time...
killall dnsmasq

ifconfig $USB down
Make sure to create the scripts from the root account, so they'll have the right ownership. Also don't forget to give them running permission:
chmod +x /usr/local/bin/usb0start.sh
chmod +x /usr/local/bin/usb0stop.sh

Step 4: Edison Configuration

Connect Edison to the host computer and start a remote session. Change the following configuration file as below:

/etc/network/interfaces
# Disable auto connection, enable DHCP
iface usb0 inet dhcp
#auto usb0
#iface usb0 inet static
#    address 192.168.2.15
#    netmask 255.255.255.0

Usage

After performing the steps above, you should be able to start the local gateway by running the usb0start.sh script:
sudo /usr/local/bin/usb0start.sh

Then on the Edison environment, start the usb0 interface:
ifup usb0

After you're done with Edison, the gateway can be deactivated by running the usb0stop.sh script:
sudo /usr/local/bin/usb0stop.sh

Troubleshooting

If Edison cannot lease an IP from the host computer or see IP addresses beyond the gateway, you can enable logging on Dnsmasq by adding the following lines to the /etc/dnsmasq.conf file:
log-queries
log-dhcp

Then use tail -f /var/log/syslog to check for messages.

On Kubuntu the network manager tried to setup the usb0 interface using eth0's configuration every time it was turned on, messing the DHCP service. This was solved by explicitly binding the configuration to eth0 on the network manager's GUI.

Monday, April 6, 2015

ROS and my first pull request

I had been putting it off for a long time, but a couple weeks ago I finally decided to bite the bullet and port my research code to ROS. After a bit of a rough start reinstalling my OS from scratch – I had recently upgraded my notebook to Kubuntu 14.10, but the latest ROS distribution (Indigo) wouldn't install on anything past 14.04 – and coming to terms with the catkin build system, I quickly fell in love with its modular approach to software development.

One of the many little conveniences ROS offers are parameters, which among other things greatly simplify management of program arguments. The ROS C++ client library provides several ways to retrieve parameter values, and optionally provide defaults for when they are not defined. One of those ways is the ros::param::param template function, which can be used to write initialization code like this:
std::string path;
std::string format;
double fps;

param("~path", path, "video.mpg");
param("~format", format, "MPEG");
param("~fps", fps, 30.0);

VideoRecorder recorder(path, format, fps);
However I disliked having to declare variables to get to the parameter values, just to pass them straight to a constructor or function call and then disregard them. So I wrote an alternative version of the template function that retrieved the value as a return instead of an output argument:
template<class T> T param(const std::string& name, const T& fallback)
{
  T value;
  param(name, value, fallback);
  return value;
}
So I could rewrite the code above as:
VideoRecorder recorder(param("~path", "video.mpg"),
                       param("~format", "MPEG"),
                       param("~fps", 30.0));
At first I had separate copies of this function on each of my ROS projects, but soon I realized it would be great if I could contribute it to the official roscpp code base. After a bit of stumbling about looking for the right place to propose it, I ended up submitting the change myself as a pull request. Overall the process went very smoothly, though there are some tips I wish I had been given before I started:
  • You'll want to compile and run tests on the code you intend to submit (believe me, even a change as small as the one above was not without a couple coding errors), so remember to create a catkin workspace into which to clone the forked repository;
  • You may think that your change is trivial and will be done in a single commit, but it's likely your submission will go through several revisions before it's accepted. To keep things organized, create a new branch before starting your work;
  • Beware of line-ending whitespace – there are a few lurking around in source files, and if your editor automatically removes them you'll end with an unnecessarily large (and likely unacceptable) diff;
  • That said if you do mess up, don't bother trying to amend the initial commit – simply submit additional commits with the required changes or reversals (this is where having a separate branch may come in handy).
Open communities are not always noted for being welcoming to beginners, even less so in technical circles. But I have nothing but praise for the ros_comm community and the fairly friendly and constructive exchange we had. Keep up the good work people!

Saturday, July 13, 2013

Setup a Wireless Access Point with Linux Mint 15 (Olivia)

In these days of tablets, smartphones and other "connected" devices, the ability to share a fast wired Internet connection via wi-fi can be very useful, specially at home or small offices. Wireless routers aren't very expensive and are simple enough to install – yet I have always thought they're kind of wasteful, considering most computers today ship with wi-fi interfaces, or can be fitted with USB cards that sell for pennies.

With Linux, wi-fi interfaces can be configured to operate in master mode, so they can accept client connections just like a dedicated router would. This is a huge improvement over "ad-hoc" wi-fi network sharing, because:
  1. Many mobile devices do not connect to "ad-hoc" access points;
  2. The WEP authentication standard used on those connections is known to be flawed, in contrast to the newer WPA standard used in "master mode" connections.
Today I updated my desktop to the latest Linux Mint 15 (Olivia), and as I went about configuring a Wireless Access Point (WAP) on it for my other devices, I took the time to document my changes to the system, so I can more easily reproduce them in future upgrades – and hopefully, also help others to setup a WAP with Mint.

The procedure I came up with was largely inspired by this blog post. It assumes the existence of a wired interface eth0 (which provides access to the Internet) and a mac80211-compatible wireless interface wlan0 (which will be configured to accept client connections in "master mode").

First open a command prompt and start a root session with su, then follow the steps below:

Step 1: Install Applications

Type the command below to install the required services:
apt-get install hostapd dnsmasq
As it is the services will be automatically started when the computer boots, but we need better control than that. So change the default settings to manual start:
update-rc.d -f hostapd remove
update-rc.d -f dnsmasq remove

Step 2: Configuration Files

Create or update the following config files with the contents below:

/etc/hostapd/hostapd.conf
interface=wlan0
driver=nl80211
ssid=hotspot # Your WAP name, change it to something more unique
hw_mode=g
channel=6 # You may want to change this if the channel is too crowded
wpa=1
wpa_passphrase=hotspot_password # Password for clients
wpa_key_mgmt=WPA-PSK
wpa_pairwise=TKIP CCMP
wpa_ptk_rekey=600

/etc/dnsmasq.conf
# disables dnsmasq reading any other files like /etc/resolv.conf for nameservers
no-resolv
# Interface to bind to
interface=wlan0
# Specify starting_range,end_range,lease_time
dhcp-range=10.0.0.3,10.0.0.20,12h
# dns addresses to send to the clients
server=8.8.8.8
server=8.8.4.4

Step 3: Interface Scripts

Add the scripts below to your system:

/etc/network/if-up.d/wapstart
#!/bin/sh

WIRE=eth0
WIFI=wlan0

# Only run script for wired interface
if [ ! "$IFACE" = "$WIRE" ]
then
    exit 0
fi

# Setup wireless interface
ifconfig $WIFI up 10.0.0.1 netmask 255.255.255.0

# Start dnsmasq
/etc/init.d/dnsmasq start

#Enable NAT
iptables --flush
iptables --table nat --flush
iptables --delete-chain
iptables --table nat --delete-chain
iptables --table nat --append POSTROUTING --out-interface $WIRE -j MASQUERADE
iptables --append FORWARD --in-interface $WIFI -j ACCEPT

# Start the Wireless Access Point service
/etc/init.d/hostapd start

exit 0

/etc/network/if-post-down.d/wapstop
#!/bin/bash

WIRE=eth0
WIFI=wlan0

# Only run script for wired interface
if [ ! "$IFACE" = "$WIRE" ]
then
    exit 0
fi

# Stops Wireless Access Point services
/etc/init.d/hostapd stop
/etc/init.d/dnsmasq stop

# Asked nice the first time...
killall dnsmasq
killall hostapd

ifconfig $WIFI down
Make sure to create the scripts from the root account, so they'll have the right ownership. Also don't forget to give them running permission:
chmod +x /etc/network/if-up.d/wapstart
chmod +x /etc/network/if-post-down.d/wapstop

Usage

After performing the steps above, you should be able to start the WAP by stopping and restarting the wired interface:
ifconfig eth0 down
ifconfig eth0 up
From now on it will also be automatically started at boot time.

Troubleshooting

If your devices can see and connet to the wireless hotspot, but they cannot access the Internet, enter the command below:
sysctl -w net.ipv4.ip_forward=1
This will update kernel settings to enable IP forwarding, and only has to be issued once.

Depending on your distribution, hostapd may require that the wireless interface be either turned "on" in the network manager applet (e.g. Mint 15), or excluded from Network Manager control (e.g. Kubuntu 14.04). In the second case add the following to /etc/NetworkManager/NetworkManager.conf:
[keyfile]
unmanaged-devices=mac:mac_address_of_wireless_interface
Also check /etc/init.d/hostapd for whether the variable DAEMON_CONF is set; if not, set it to the path of the config file:
DAEMON_CONF=/etc/hostapd/hostapd.conf
If nothing else works, you can try to find the cause by running hostapd directly instead of in service mode, with extra logging options enabled. For example:
hostapd -d /etc/hostapd/hostapd.conf