commit 0121ead71d532ad27980027f0bda30ea591d8e27
Author: bwasserm <bwasserm@andrew.cmu.edu>
Date:   Fri Oct 7 22:26:37 2011 -0400

    Added the ROS stack with the libscout, motors, and sonar packages to the reposititory, and associated files. There are probably still things missing. Libscout won't run properly, but it and motors will compile. Sonar is probably broken, but nothing depends on it yet, so this shouldn't be an issue.

diff --git a/scout/libscout/src/constants.h b/scout/libscout/src/constants.h
index 4b0d4a4..8e89ee0 100644
--- a/scout/libscout/src/constants.h
+++ b/scout/libscout/src/constants.h
@@ -45,17 +45,16 @@
 /* Libscout Defines */
 
 /* Init defines */
-const int LIB_ALL = ~0;
-const int LIB_MOTORS = 0x1;
-const int LIB_SONAR = 0x2;
-const int LIB_CLIFFSENSORS = 0x4;
-const int LIB_HEADLIGHTS = 0x8;
+#define LIB_ALL -1
+#define LIB_MOTORS 0x1
+#define LIB_SONAR 0x2
+#define LIB_CLIFFSENSORS 0x4
+#define LIB_HEADLIGHTS 0x8
 
 /* Status defines */
-const int LIB_ERROR = ~0;
-const int LIB_OK = 0;
+//TODO MAKE ENUMS
+#define LIB_ERROR -1
+#define LIB_OK 0
 
-/* Global objects */
-ros::NodeHandle libscout_node;
 
 #endif
diff --git a/scout/libscout/src/libmotors.cpp b/scout/libscout/src/libmotors.cpp
index fd28190..ccf728a 100644
--- a/scout/libscout/src/libmotors.cpp
+++ b/scout/libscout/src/libmotors.cpp
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2007 Colony Project
+ * Copyright (c) 2011 Colony Project
  * 
  * Permission is hereby granted, free of charge, to any person
  * obtaining a copy of this software and associated documentation
@@ -39,33 +39,85 @@
 
 #include "libmotors.h"
 
+/* ROS node created in libscout.cpp */
+extern ros::NodeHandle libscout_node;
+
+/** ROS publisher and client declaration */
+ros::Publisher set_motors_publisher;
+ros::ServiceClient query_motors_client;
+
+/*!
+ * \brief Initialize the motors module of libscout.
+ *
+ * Initialize the libscout node as a publisher of set_motors and a client of
+ * query_motors.
+ */
 void libmotors_init(){
   set_motors_publisher = libscout_node.advertise<motors::set_motors>("libmotors", 10);
   query_motors_client = libscout_node.serviceClient<motors::query_motors>("libmotors");
 }
 
-int motors_set(int speed, int which){
+/*!
+ * \brief Set motor speeds
+ * Sets the speeds of the motors as a percentage of top speed. Can selectively
+ * select which motors to set, and which to remain at previous speed.
+ * \param speed The speed the motors should be set to
+ * \param which A bitmask of which motors should be set
+ * \return Function status
+ */
+int motors_set(int speed, int which, char units=MOTOR_PERCENT){
   /** \todo Set fields of the message based on params */
   
   if(!ros::ok()){
     return LIB_ERROR;
   }
-  
+ 
+  /* Set the speed for each motor according to the which bitmask*/
   motors::set_motors msg;
-  msg.fl_speed = speed;
-  msg.fr_speed = speed;
-  msg.bl_speed = speed;
-  msg.br_speed = speed;
+  if(which & MOTOR_FL_REV){
+    msg.fl_speed = -1 * speed;
+  }
+  if(which & MOTOR_FR_REV){
+    msg.fr_speed = -1 * speed;
+  }
+  if(which & MOTOR_BL_REV){
+    msg.bl_speed = -1 * speed;
+  }
+  if(which & MOTOR_BR_REV){
+    msg.br_speed = -1 * speed;
+  }if(which & MOTOR_FL){
+    msg.fl_speed = speed;
+  }
+  if(which & MOTOR_FR){
+    msg.fr_speed = speed;
+  }
+  if(which & MOTOR_BL){
+    msg.bl_speed = speed;
+  }
+  if(which & MOTOR_BR){
+    msg.br_speed = speed;
+  }
+
+  /* Specify which units the speeds are given in */
+  msg.units = units;
 
-//  set_motors_publisher.publish(msg);
+  /* Publishes message to set_motors topic */
+  set_motors_publisher.publish(msg);
   ros::spinOnce();
 
   return LIB_OK;
 }
 
-int motors_speed_set(int speed, int which){
-  return LIB_OK;
-}
+/*!
+ * \brief Query the current speeds of the motors
+ *
+ * Sends a request to the query_motors service which will reply with the
+ *  current speed of each motor.
+ *
+ * \param which A bitmask that will specify which motor speed should be
+ *  returned
+ * \return The speed of the selected motor, or LIB_ERR if no motor selected
+ */
 
 int motors_query(int which){
   motors::query_motors srv;
@@ -79,10 +131,9 @@ int motors_query(int which){
         return srv.response.bl_speed;
       case MOTOR_BR:
         return srv.response.br_speed;
-      default:{
+      default:
         ROS_WARN("Bad WHICH in motors_query.");
         return LIB_ERROR;
-      }
     }
   }else{
     ROS_ERROR("Failed to call service query_motors");
diff --git a/scout/libscout/src/libmotors.h b/scout/libscout/src/libmotors.h
index a74a7f3..d1b98e9 100644
--- a/scout/libscout/src/libmotors.h
+++ b/scout/libscout/src/libmotors.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2007 Colony Project
+ * Copyright (c) 2011 Colony Project
  * 
  * Permission is hereby granted, free of charge, to any person
  * obtaining a copy of this software and associated documentation
@@ -42,23 +42,36 @@
 #include "motors/query_motors.h"
 #include "motors/set_motors.h"
 
-
 #ifndef _LIBMOTORS_H_
 #define _LIBMOTORS_H_
 
 /* Defines */
-const int MOTOR_FL = 0x8;
-const int MOTOR_FR = 0x4;
-const int MOTOR_BL = 0x2;
-const int MOTOR_BR = 0x1;
-
-
-ros::Publisher set_motors_publisher;
-ros::ServiceClient query_motors_client;
+#define MOTOR_ALL 0xF
+#define MOTOR_ALL_REV 0xF0
+#define MOTOR_NONE 0x0
+#define MOTOR_FL 0x8
+#define MOTOR_FR 0x4
+#define MOTOR_BL 0x2
+#define MOTOR_BR 0x1
+#define MOTOR_FL_REV 0x80
+#define MOTOR_FR_REV 0x40
+#define MOTOR_BL_REV 0x20
+#define MOTOR_BR_REV 0x10
+#define MOTOR_FRONT MOTOR_FL | MOTOR_FR
+#define MOTOR_BACK MOTOR_BR | MOTOR_BR
+#define MOTOR_LEFT MOTOR_FL | MOTOR_BL
+#define MOTOR_RIGHT MOTOR_FR | MOTOR_BR
+#define MOTOR_LEFT_REV MOTOR_FL_REV | MOTOR_BL_REV
+#define MOTOR_RIGHT_REV MOTOR_FR_REV | MOTOR_BR_REV
+#define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
+#define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
+#define MOTOR_PERCENT 'p'
+#define MOTOR_MMS 'm'
+#define MOTOR_CMS 'c'
 
 void libmotors_init();
 int motors_set(int speed, int which);
-int motors_speed_set(int speed, int which);
 int motors_query(int which);
 
 #endif
+
diff --git a/scout/libscout/src/libscout.cpp b/scout/libscout/src/libscout.cpp
index 5126e37..d4f30ea 100644
--- a/scout/libscout/src/libscout.cpp
+++ b/scout/libscout/src/libscout.cpp
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2007 Colony Project
+ * Copyright (c) 2011 Colony Project
  * 
  * Permission is hereby granted, free of charge, to any person
  * obtaining a copy of this software and associated documentation
@@ -39,6 +39,9 @@
 
 #include "libscout.h"
 
+/* Global objects */
+ros::NodeHandle libscout_node;
+
 /** \todo Decide how to call user behaviors
   * I'm thinking that there be a function call in main that calls the "main"
   fn in the user behavior. However, their "main" function will not be called 
@@ -52,22 +55,39 @@
   user behavior, or any other package that works below the surface.
 **/
 
-
+/*!
+ * \brief LibScout. Primary library for interfacing behaviors with scout hw.
+ * 
+ * This is the main function for libscout. It is run when the node starts and
+ * starts the library. It calls init() which initializes the clients and
+ * publishers/subscribers for the other parts of the library.
+ *
+ * \param argc The number of command line arguments (should be 1)
+ * \param argv The array of command line arguments
+ **/
 int main(int argc, char **argv){
   ros::init(argc, argv, "libscout");
 
   init(LIB_ALL);
   /** \todo remove this test code **/
-//  motors_set(100, 0);
-//  ROS_INFO("%d", motors_query(0));
+//  motors_set(100, MOTOR_ALL);
+  ROS_INFO("%d", motors_query(MOTOR_FL));
 
   return 0;
 }
 
+/*!
+ * \brief Initializes modules in the libscout.
+ * 
+ * Calls init functions for each module in libscout.
+ * \param modules A bitmask of the modules that will be initialized.
+ **/
 int init(int modules){
+  /** \todo Copy this if for each module that gets added to the library */
   if(modules & LIB_MOTORS){
     libmotors_init();
   }
   /** \todo Add other lib inits **/
   return 0;
 }
+
diff --git a/scout/libscout/src/libscout.h b/scout/libscout/src/libscout.h
index e525dc7..7abe856 100644
--- a/scout/libscout/src/libscout.h
+++ b/scout/libscout/src/libscout.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2007 Colony Project
+ * Copyright (c) 2011 Colony Project
  * 
  * Permission is hereby granted, free of charge, to any person
  * obtaining a copy of this software and associated documentation
@@ -44,9 +44,9 @@
 #include "constants.h"
 #include "libmotors.h"
 
-
 /* Libscout functions */
 int main(int argc, char **argv);
 int init(int modules);
 
 #endif
+
diff --git a/scout/messages/CMakeLists.txt b/scout/messages/CMakeLists.txt
new file mode 100644
index 0000000..853165b
--- /dev/null
+++ b/scout/messages/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
diff --git a/scout/messages/Makefile b/scout/messages/Makefile
new file mode 100644
index 0000000..b75b928
--- /dev/null
+++ b/scout/messages/Makefile
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/scout/messages/mainpage.dox b/scout/messages/mainpage.dox
new file mode 100644
index 0000000..9309244
--- /dev/null
+++ b/scout/messages/mainpage.dox
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b messages is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
diff --git a/scout/messages/manifest.xml b/scout/messages/manifest.xml
new file mode 100644
index 0000000..16ea9a5
--- /dev/null
+++ b/scout/messages/manifest.xml
@@ -0,0 +1,14 @@
+<package>
+  <description brief="messages">
+
+     messages
+
+  </description>
+  <author>Ben</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/messages</url>
+
+</package>
+
+
diff --git a/scout/messages/srv/query_sonar.srv b/scout/messages/srv/query_sonar.srv
new file mode 100644
index 0000000..90f3dc5
--- /dev/null
+++ b/scout/messages/srv/query_sonar.srv
@@ -0,0 +1,6 @@
+int8 direction
+---
+#front sonar
+int16 distance0
+#back sonar
+int16 distance1
diff --git a/scout/motors/msg/set_motors.msg b/scout/motors/msg/set_motors.msg
index 490f41c..9a2ca5c 100644
--- a/scout/motors/msg/set_motors.msg
+++ b/scout/motors/msg/set_motors.msg
@@ -3,3 +3,4 @@ int8 fl_speed
 int8 fr_speed
 int8 bl_speed
 int8 br_speed
+int8 units
diff --git a/scout/motors/src/motors.cpp b/scout/motors/src/motors.cpp
index 6fb76e1..a5d570a 100644
--- a/scout/motors/src/motors.cpp
+++ b/scout/motors/src/motors.cpp
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2007 Colony Project
+ * Copyright (c) 2011 Colony Project
  * 
  * Permission is hereby granted, free of charge, to any person
  * obtaining a copy of this software and associated documentation
@@ -36,15 +36,22 @@
 
 #include "ros/ros.h"
 #include "motors.h"
+//#include "libscout/src/constants.h"
 #include <cstdlib>
 
 /**
  * @defgroup motors Motors
  * @brief Functions for using the motors
  *
- * @{
  **/
 
+/* Motor state variables */
+/** \todo Fix types: static */
+int motor_fl_speed; /**< The current speed of the front left motor. */
+int motor_fr_speed; /**< The current speed of the front right motor. */
+int motor_bl_speed; /**< The current speed of the back left motor. */
+int motor_br_speed; /**< The current speed of the back right motor. */
+
 /*!
  * \brief Motors driver. This is a ROS node that controls motor speeds.
  *
@@ -108,11 +115,7 @@ bool motors_query(motors::query_motors::Request &req,
   res.bl_speed = motor_bl_speed;
   res.br_speed = motor_br_speed;
 
-  ROS_INFO("Motor speeds queried");
+  ROS_DEBUG("Motor speeds queried");
   return true;
 }
 
-
-/**
- * }
- **/
diff --git a/scout/motors/src/motors.h b/scout/motors/src/motors.h
index b45f520..dbaaa02 100644
--- a/scout/motors/src/motors.h
+++ b/scout/motors/src/motors.h
@@ -43,11 +43,6 @@
 #include "motors/query_motors.h"
 #include "motors/set_motors.h"
 
-/* Motor state variables */
-int motor_fl_speed; /**< The current speed of the front left motor. */
-int motor_fr_speed; /**< The current speed of the front right motor. */
-int motor_bl_speed; /**< The current speed of the back left motor. */
-int motor_br_speed; /**< The current speed of the back right motor. */
 
 /** @brief Initialize the motors module and driver **/
 int main(int argc, char **argv);
diff --git a/scout/sonar/CMakeLists.txt b/scout/sonar/CMakeLists.txt
new file mode 100644
index 0000000..853165b
--- /dev/null
+++ b/scout/sonar/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
diff --git a/scout/sonar/Makefile b/scout/sonar/Makefile
new file mode 100644
index 0000000..b75b928
--- /dev/null
+++ b/scout/sonar/Makefile
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/scout/sonar/mainpage.dox b/scout/sonar/mainpage.dox
new file mode 100644
index 0000000..d20bbce
--- /dev/null
+++ b/scout/sonar/mainpage.dox
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b sonar is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
diff --git a/scout/sonar/manifest.xml b/scout/sonar/manifest.xml
new file mode 100644
index 0000000..6d38aae
--- /dev/null
+++ b/scout/sonar/manifest.xml
@@ -0,0 +1,16 @@
+<package>
+  <description brief="sonar">
+
+     sonar
+
+  </description>
+  <author>Ben</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/sonar</url>
+  <depend package="roscpp"/>
+  <depend package="std_msgs"/>
+
+</package>
+
+
diff --git a/scout/sonar/src/sonar.cpp b/scout/sonar/src/sonar.cpp
new file mode 100644
index 0000000..3bc3064
--- /dev/null
+++ b/scout/sonar/src/sonar.cpp
@@ -0,0 +1,175 @@
+/**
+ * Copyright (c) 2007 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ **/
+
+
+/**
+ * @file sonar.cpp
+ * @brief Sonar
+ *
+ * Implementation of functions for sonar use.
+ *
+ * @author Colony Project, CMU Robotics Club
+ * @author Benjamin Wasserman
+ **/
+
+#include "scout/sonar.h"
+#include "ros/ros.h"
+#include <cstdlib>
+
+/**
+ * @defgroup sonar Sonar
+ * @brief Functions for using the sonar
+ *
+ * @{
+ **/
+
+int8_t sonar_stepper_pos; //This should only contain values in [0,47]
+/**
+ * Sonar driver/service. This is a ROS node that communicates directly with the sonar
+ hardware. It serves the query_sonar service, and takes a direction, and responds with a distance. It is registered by sonar_driver_init.
+ *
+ **/
+bool sonar_get_distance(sonar::query_sonar::Request   &req,
+                  sonar::query_sonar::Response  &res){
+  /* Set the stepper position (position from 0-47 for direction)*/
+  //stepper_set(req.direction)
+  /* Delay if necessary for stepper to get in position */
+  /* Get value from sonar */
+  //res.distance0 = sonar_sensor(0);
+  //res.distance1 = sonar_sensor(1);
+  ROS_INFO("request: sonar direction #%d", req.direction);
+  ROS_INFO("response: sonar distances %d %d", res.distance0, res.distance1);
+  return true;
+}
+
+/**
+ * Initialize the sonar driver. This must be called before any sonar
+ * requests are made. It will then run in the background until the sonar node is
+ * shut down.
+ *
+ * @see sonar_get_distance
+ **/
+void main(int argc, char **argv){
+  /* Initialize in ROS the sonar driver node */
+  ros::init(argc, argv, "sonar_driver");
+
+  /* Advertise that this serves the query_sonar service */
+  ros::NodeHandle n;
+  ros:ServiceServer service = n.advertiseService("query_sonar", sonar_get_distance);
+  
+  /* Initialize hardware for sonar */
+  //Hardware init functions here
+    
+  ROS_INFO("Ready to read sonar.");
+  ros::spin();
+
+  return 0;
+}
+
+/**
+ * Initialize the sonar system. This must be called before sonar_read.
+ * 
+ * @see sonar_read
+ * @see sonar_read_raw
+ **/
+void sonar_init(void){
+  sonar_stepper_pos = 0;
+}
+
+/**
+ * Read raw sonar data from the sonar module via the query_sonar service. 
+ *
+ * @param direction The direction to set the stepper motor to. (0-47)
+ * @param sensor The sonar sensor that should have its value returned. (0-1)
+ * @return distance The distance in the specified direction. Or -1 if an
+ * illegal parameter or error.
+ **/
+int sonar_read_raw(int direction, int sensor){
+  /* Check for illegal arguments */
+  if(direction < 0 || direction > 47 || sensor < 0 || sensor > 1){
+    ROS_WARNING("Illegal argument");
+    return -1;
+  }
+  /* Register client to query_sonar service. */
+  ros::NodeHandle n;
+  ros::ServiceClient client = n.serviceClient<messages::query_sonar>("query_sonar");
+  /* Create and populate query */
+  messages::query_sonar srv;
+  srv.request.direction = direction;
+  /* Send query */
+  if(client.call(srv)){
+    /* Return value of correct sensor */
+    if(sensor){
+      ROS_INFO("Sonar 1 at %d reads %d", direction, srv.response.distance1);
+      return srv.response.distance1;
+    }else{
+      ROS_INFO("Sonar 0 at %d reads %d", direction, srv.response.distance0);
+      return srv.response.distance0;
+    }
+  }else{
+    ROS_ERROR("Failed to call service query_sonar");
+    return -1;
+  }
+}
+
+/**
+ * Read sonar values in human readable fashion. Wrapper for
+ * sonar_read_raw.
+ *
+ * @param direction Direction to get sonar value at. Will be rounded to
+ *  closest possible position. In degrees. Can be negative or > 360.
+ * @return distance Distance that sonar reads in the given direction.
+ * @see sonar_read_raw
+ **/
+int sonar_read(int direction){
+  int dir1; //The position of the second sonar head;
+  int distance;
+  /* Get angle in 0<=dir<359 */
+  direction %= 360;
+  /* Get stepper positions */
+  direction /= 48;
+  /* Move stepper to closest position to get reading */
+  if(direction == sonar_stepper_pos){
+    /* If direction is the same */
+    distance = sonar_read_raw(direction, 0);
+  }else if(direction == sonar_stepper_pos + 24 || direction + 24 == sonar_stepper_pos){
+    /* If direction is opposite last direction */
+    distance = sonar_read_raw(sonar_stepper_pos, 1);
+  }else{
+    /*TODO: Replace this next line with code that will move the stepper the
+    ** minimum necessary distance to get one of the heads in the desired
+    ** position. Update sonar_stepper_pos to this new position, and call
+    ** sonar_read_raw with the stepper position (not necessarily the
+    ** direction of the reading) and which sonar sensor should be
+    ** returned (0 or 1). 
+    */
+    distance = sonar_read_raw(direction,0);
+  }
+  /*TODO: Make a function to convert raw sonar distances to cm for
+  ** usability. Replace next line with line that calls function.
+  */
+  //return convert_to_cm(distance);
+  return distance;
+}
diff --git a/scout/sonar/src/sonar.h b/scout/sonar/src/sonar.h
new file mode 100644
index 0000000..5e7a11e
--- /dev/null
+++ b/scout/sonar/src/sonar.h
@@ -0,0 +1,56 @@
+/**
+ * Copyright (c) 2007 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ **/
+
+
+/**
+ * @file sonar.h
+ * @brief Contains sonar declarations and functions
+ * 
+ * Contains functions and definitions for the use of
+ * sonar
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
+/* Author: Ben Wasserman
+*/
+
+#ifndef _SONAR_H_
+#define _SONAR_H_
+
+/** @brief Initialize the sonar in the library module **/
+void sonar_init(void);
+/** @brief Initialize the sonar module and drivers **/
+void sonar_module_init(void);
+/** @brief Read raw sonar data from the sonar module. Takes stepper
+** position and sensor to read **/
+int sonar_read_raw(int direction, int sensor);
+/** @brief Wrapper for reading sonar data (angle in degrees and distance in
+** cm) **/
+int sonar_read(int direction);
+/** @brief Subscribe function to sonar topic **/
+void sonar_subscribe(int *function());
+
+#endif
diff --git a/scout/sonar/srv/query_sonar.srv b/scout/sonar/srv/query_sonar.srv
new file mode 100644
index 0000000..90f3dc5
--- /dev/null
+++ b/scout/sonar/srv/query_sonar.srv
@@ -0,0 +1,6 @@
+int8 direction
+---
+#front sonar
+int16 distance0
+#back sonar
+int16 distance1
