/scout/libscout/src/SonarControl.cpp - Diff - Colony Scout OS - Robotics Club

Revision c492be62

ID	c492be629f06e3cf71fdf832ce2cd5ac1ec779f8

Added by Alex Zirbel over 12 years ago

Updated the licensing information in many files.

This is a broken commit because I decided to do this at a bad time. Sorry! The build will be working after next commit, I promise (and I won't push till then). This commit reflects the licensing in the files after I used my auto-add/remove script.

      * 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 SonarControl.cpp
      * @brief Contains sonar function imeplementations
      * @brief Contains sonar function implementation
+     *
      * Contains functions for the use of sonar control.
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Priyanka Deo
      * @author Alex Zirbel
      **/
     #include "SonarControl.h"
     using namespace std;
     /**
      * @brief Initialize the sonar module of libscout.
+     *
      * Initialize the libscout node as a publisher of sonar_set_scan and
      * sonar_toggle and a client of query_sonar.
      */
     SonarControl::SonarControl(const ros::NodeHandle& libscout_node)
     SonarControl::SonarControl(const ros::NodeHandle& libscout_node,
                                string scoutname)
         : node(libscout_node)
+    {
         sonar_set_scan_pub =
             node.advertise<sonar::sonar_set_scan>("set_sonar_scan", QUEUE_SIZE);
             node.advertise<sonar::sonar_set_scan>(scoutname + "/set_sonar_scan",
                                                   QUEUE_SIZE);
         sonar_toggle_pub =
     	node.advertise<sonar::sonar_toggle>("toggle_sonar", QUEUE_SIZE);
         query_sonar =
             node.serviceClient<sonar::sonar_direction>("query_sonar");
             node.advertise<sonar::sonar_toggle>(scoutname + "/toggle_sonar",
                                                 QUEUE_SIZE);
         sonar_distance_sub = node.subscribe(scoutname + "sonar_distance",
                                             QUEUE_SIZE, distance_callback);
         // Initialize all the sonar readings to 0 and timestamps to undefined
         for (int i = 0; i < 48; i++)
+        {
             readings[i] = 0;
             timestamps[i] = ros::Time::now(); // A common shortcut for no reading
+        }
+    }
     /**
      * Update the array of sonar values, and the last read timestamps,
      * to reflect the new reading received.
      */
     void SonarControl::distance_callback(const sonar::sonar_distance::ConstPtr& msg)
+    {
         // Error checking so that the array doesn't cause a segfault
         if (msg->pos < 0 || msg-> pos > 23)
+        {
             ROS_ERROR("SonarControl received an invalid sonar position.");
             return;
+        }
         readings[msg->pos] = msg->distance0;
         readings[msg->pos + 24] = msg->distance1;
         timestamps[msg->pos] = msg->header.stamp;
         timestamps[msg->pos + 24] = msg->header.stamp;
+    }
     /**
-...
+     *
      * @param position Value between 0-180 of degree position to set sonar
      * @return NONE
      * @todo should return bool. See below.
      */
     void SonarControl::set(int position)
     void SonarControl::set_single(int position)
+    {
         set_range(position, position);
+    }
-...
      * @param start_pos The leftmost (smallest) value that the sonar can take
      * @param end_pos The rightmost (largest) value that the sonar can take
      * @return NONE
      * @todo Also misimplemented, should return bool. Look at srv def in sonar/
      */
     void SonarControl::set_range(int start_pos, int end_pos)
+    {
         /** Turn sonar on */
         // Check that the range is valid
         if (start_pos < 0 || end_pos < 0 || start_pos > 23 || end_pos > 23)
+        {
             ROS_ERROR("Commanded SonarControl::set_range to a bad value.");
             return;
+        }
         // Sort the start and end positions into increasing order
         if (start_pos > end_pos)
+        {
             int temp = start_pos;
             start_pos = end_pos;
             end_pos = temp;
+        }
         // Turn sonar on
         sonar::sonar_toggle on_msg;
         on_msg.set_on = true;
         sonar_toggle_pub.publish(on_msg);
         /** Set scan range */
         // Set scan range
         sonar::sonar_set_scan msg;
         //TODO Enable some checks of start and end position
         msg.stop_l = start_pos;
         msg.stop_r = end_pos;
         sonar_set_scan_pub.publish(msg);
         ros::spinOnce();
         ros::spinOnce();  ///<! Sure? Does this belong here?
+    }
     /**
      * @brief Turn off sonar readings
+     *
      * Stops sonar from panning and taking readings.
      * @todo Misimplemented. Needs to use a service call and return the ack as bool
+     *
      * @return NONE
      */
     float SonarControl::set_off()
+    {
         /** Turn sonar off */
         sonar::sonar_toggle on_msg;
         on_msg.set_on = false;
         sonar_toggle_pub.publish(on_msg);
+    }
     /**
      * @brief Query the current front sonar readings
+     *
      * Sends a request to the query_sonar service which will reply with the
      *  current readings of the sonar.
+     *
      * @TODO Change so we can get multiple sonar readings with one call
+     *
      * @return The reading of the front sonar
      */
     float SonarControl::query_front()
+    {
         sonar::sonar_direction srv;
         if(query_sonar.call(srv))
+        {
             srv.response.distance0;
+        }
         else
+        {
             ROS_FATAL("Failed to call service query_motors");
+        }
         return 0;
+    }
     /**
      * @brief Query the current back sonar readings
+     *
      * Sends a request to the query_sonar service which will reply with the
      *  current readings of the sonar.
+     *
      * @TODO Change so we can get multiple sonar readings with one call
+     *
      * @return The reading of the back sonar
      */
     float SonarControl::query_back()
+    {
         sonar::sonar_direction srv;
         if(query_sonar.call(srv))
+        {
             srv.response.distance1;
+        }
         else
+        {
             ROS_FATAL("Failed to call service query_motors");
+        }
         return 0;
+    }
     /**
      * @brief Converts value returne by sonar to physical distances.
-...
      * @param sonar_value The returned value of the sonar
      * @return The physical distance measured by the sonar.
      **/
     float sonar_to_dist(float sonar_value)
+    {
     //float sonar_to_dist(float sonar_value)
     //{
         //TODO impelement later based on sonar readings
         return sonar_value;
+    }
     //    return sonar_value;
     //}
     /**
      * @brief Converts values from physical distances to values read by sonar
-...
      * @param distance The physical distance as measured.
      * @return The value read by the sonar that corresponds to the given distance
      **/
     float dist_to_sonar(float distance)
+    {
     //float dist_to_sonar(float distance)
     //{
         //TODO implement later based on sonar readings
         return distance;
+    }
     //    return distance;
     //}

Also available in: Unified diff

Project

General

Profile

Colony » Colony Scout » Colony Scout OS

Revision c492be62