/ - Diff - Colony Scout OS - Robotics Club

scout/libscout/CMakeLists.txt
29	29	#rosbuild_add_executable(example examples/example.cpp)
30	30	#target_link_libraries(example ${PROJECT_NAME})
31	31
32		rosbuild_add_executable(libscout_node src/behavior.cpp src/libscout.cpp src/libmotors.cpp src/libheadlights.cpp src/libbuttons.cpp)
	32	#rosbuild_add_executable(libscout_node src/behavior.cpp src/libscout.cpp src/libmotors.cpp src/libheadlights.cpp src/libbuttons.cpp)
	33	rosbuild_add_executable(alex_behavior src/alex_behavior.cpp src/libscout.h src/MotorControl.cpp src/HeadlightControl.cpp)

     /**
      * Copyright (c) 2011 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 constants.h
      * @brief Contains top level scout library constants
+     *
      * Contains constants for use in libscout
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      **/
     #ifndef _CONSTANTS_H_
     #define _CONSTANTS_H_
     #include "ros/ros.h"
     /* ROS defines */
     #define QUEUE_SIZE 10
     /* Libscout Defines */
     /* Init defines */
     #define LIB_ALL -1
     #define LIB_MOTORS 0x1
     #define LIB_SONAR 0x2
     #define LIB_CLIFFSENSORS 0x4
     #define LIB_HEADLIGHTS 0x8
     #define LIB_BUTTONS 0x10
     /* Status defines */
     //TODO MAKE ENUMS
     #define LIB_ERROR -1
     #define LIB_OK 0
     #endif

     /**
      * Copyright (c) 2011 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 HeadlightControl.cpp
      * @brief Contains headlights declarations and functions
+     *
      * Contains functions and definitions for the use of
      * headlights
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      * @author Alex Zirbel
      */
     #include "HeadlightControl.h"
     /** ROS publisher and client declaration */
     /**
      * @brief Initialize the headlights module of libscout.
+     *
      * Initialize the libscout node as a publisher of set_headlights.
      */
     HeadlightControl::HeadlightControl(const ros::NodeHandle& libscout_node)
         : node(libscout_node)
+    {
         set_headlights_pub = node.advertise<headlights::set_headlights>
             ("set_headlights", QUEUE_SIZE);
+    }
     /**
      * @brief Set headlight colors as a single RGB value
+     *
      * Sets the colors of the headlights as a web color value. Can selectively
      * select which motors to set, and which to remain at previous color.
      * @param color The color the headlights should be set to expressed as 0xRRGGBB
      * @param which A bitmask of which headlights should be set
      */
     void HeadlightControl::set(int color, int which)
+    {
         int red, green, blue;
         red = (color & RED) >> REDSHIFT;
         green = (color & GREEN) >> GREENSHIFT;
         blue = (color & BLUE) >> BLUESHIFT;
         set_rgb(red, green, blue, which);
+    }
     /**
      * @brief Set headlight colors as separate RGB values
+     *
      * Sets the colors of the headlights as a web color value. Can selectively
      * select which motors to set, and which to remain at previous color.
      * @param red The red value the headlights should be set to
      * @param green The green value the headlights should be set to
      * @param blue The blue value the headlights should be set to
      * @param which A bitmask of which headlights should be set
      */
     void HeadlightControl::set_rgb(int red, int green, int blue, int which)
+    {
         headlights::set_headlights msg;
         if(which & HL_LEFT)
+        {
             msg.left_red = red;
             msg.left_green = green;
             msg.left_blue = blue;
+        }
         else
+        {
             msg.left_red = NO_SET;
             msg.left_green = NO_SET;
             msg.left_blue = NO_SET;
+        }
         if(which & HL_RIGHT)
+        {
             msg.right_red = red;
             msg.right_green = green;
             msg.right_blue = blue;
+        }
         else
+        {
             msg.right_red = NO_SET;
             msg.right_green = NO_SET;
             msg.right_blue = NO_SET;
+        }
         /* Publishes message to set_motors topic */
         set_headlights_pub.publish(msg);
         ros::spinOnce();
+    }

     /**
      * Copyright (c) 2011 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 HeadlightControl.h
      * @brief Contains headlights declarations and functions
+     *
      * Contains functions and definitions for the use of
      * headlights
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      * @author Alex Zirbel
      */
     #ifndef _HEADLIGHT_CONTROL_H_
     #define _HEADLIGHT_CONTROL_H_
     #include <ros/ros.h>
     #include <libscout/constants.h>
     #include <headlights/set_headlights.h>
     /* Defines */
     #define WHITE		0xFFFFFF
     #define OFF			0x000000
     #define RED 		0xFF0000
     #define GREEN 	    0x00FF00
     #define BLUE		0x0000FF
     #define YELLOW	    0xFFFF00
     #define CYAN		0x00FFFF
     #define PINK		0xFF00FF
     #define HL_RIGHT	0x1
     #define HL_LEFT		0x2
     #define HL_BOTH		0x3
     #define NO_SET	-1
     #define REDSHIFT 		16
     #define GREENSHIFT 	    8
     #define BLUESHIFT		0
     class HeadlightControl
+    {
         public:
             HeadlightControl(const ros::NodeHandle& libscout_node);
             void set(int color, int which);
             void set_rgb(int red, int green, int blue, int which);
         private:
             ros::NodeHandle node;
             ros::Publisher set_headlights_pub;
     };
     #endif

     /**
      * Copyright (c) 2011 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 MotorControl.cpp
      * @brief Contains motor declarations and functions
+     *
      * Contains functions and definitions for the use of
      * motors
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      * @author Alex Zirbel
      **/
     #include "MotorControl.h"
     /**
      * @brief Initialize the motors module of libscout.
+     *
      * Initialize the libscout node as a publisher of set_motors and a client of
      * query_motors.
      */
     MotorControl::MotorControl(const ros::NodeHandle& libscout_node)
         : node(libscout_node)
+    {
         set_motors_pub =
             node.advertise<motors::set_motors>("set_motors", QUEUE_SIZE);
         query_motors_client =
             node.serviceClient<motors::query_motors>("query_motors");
+    }
     /**
      * @brief Sets all the speeds according to the specificiation of which motors
      *        to set.
+     *
      * @param which A bitmask of which motors need to be set.
      * @param speed The value to set the motors to.
      * @param units The units the speed is expressed in.
      * @return Function status
      */
     void MotorControl::set(int which, float speed, char units)
+    {
         float speed_fl, speed_fr, speed_bl, speed_br;
         speed_fl = speed_fr = speed_bl = speed_br = speed;
         if(which & MOTOR_FL_REV)
+        {
             speed_fl = -speed;
+        }
         if(which & MOTOR_FR_REV)
+        {
             speed_fr = -speed;
+        }
         if(which & MOTOR_BL_REV)
+        {
             speed_bl = -speed;
+        }
         if(which & MOTOR_BR_REV)
+        {
             speed_br = -speed;
+        }
         set_each(which, speed_fl, speed_fr, speed_bl, speed_br, units);
+    }
     /**
      * @brief Sets the left and right sides of scout to different speeds.
+     *
      * @param speed_l The speed of both left motors.
      * @param speed_r The speed of both right motors.
      * @param units The units to set to.
      * @return Function status
      */
     void MotorControl::set_sides(float speed_l, float speed_r, char units)
+    {
         set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r, units);
+    }
     /**
      * @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 keep at previous speed.
+     *
      * @param which A bitmask of which motors should be set.
      * @param speed The speed the motors should be set to.
      * @param units Optional units for the speeds.
      * @return Function status
      */
     void MotorControl::set_each(int which,
                                 float speed_fl, float speed_fr,
                                 float speed_bl, float speed_br,
                                 char units)
+    {
         check_which_ok(which);
         motors::set_motors msg;
         /* Tell the motors node which motors need to be updated */
         msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
         if(which & (MOTOR_FL | MOTOR_FL_REV))
+        {
             msg.fl_set = true;
             motor_fl_speed = rel_to_abs(speed_fl, units);
+        }
         if(which & (MOTOR_FR | MOTOR_FR_REV))
+        {
             msg.fr_set = true;
             motor_fr_speed = rel_to_abs(speed_fr, units);
+        }
         if(which & (MOTOR_BL | MOTOR_BL_REV))
+        {
             msg.bl_set = true;
             motor_bl_speed = rel_to_abs(speed_bl, units);
+        }
         if(which & (MOTOR_BR | MOTOR_BR_REV))
+        {
             msg.br_set = true;
             motor_br_speed = rel_to_abs(speed_br, units);
+        }
         /* Set all the speeds (the booleans in the set_motors message determine
          * which ones will be used) */
         msg.fl_speed = (int) motor_fl_speed;
         msg.fr_speed = (int) motor_fr_speed;
         msg.bl_speed = (int) motor_bl_speed;
         msg.br_speed = (int) motor_br_speed;
         /* Publishes message to set_motors topic */
         set_motors_pub.publish(msg);
         ros::spinOnce();
+    }
     /**
      * Double-checks the which variable to make sure the front and back
      * bits are not both set for a single motor.
+     *
      * @param which The which motor specfication to check.
      */
     void MotorControl::check_which_ok(int which)
+    {
         ROS_ERROR_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
             "FL Motor set in both directions!");
         ROS_ERROR_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
             "FR Motor set in both directions!");
         ROS_ERROR_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
             "BL Motor set in both directions!");
         ROS_ERROR_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
             "BR Motor set in both directions!");
+    }
     /**
      * @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.
+     *
      * @TODO Change so we can get multiple motor speeds with one call
+     *
      * @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
      */
     float MotorControl::query(int which)
+    {
         motors::query_motors srv;
         if(query_motors_client.call(srv))
+        {
             switch(which)
+            {
                 case MOTOR_FL:
                     return srv.response.fl_speed;
                 case MOTOR_FR:
                     return srv.response.fr_speed;
                 case MOTOR_BL:
                     return srv.response.bl_speed;
                 case MOTOR_BR:
                     return srv.response.br_speed;
                 default:
                     ROS_WARN("Bad WHICH in motors_query.");
                     return LIB_ERROR;
+            }
+        }
         else
+        {
             ROS_ERROR("Failed to call service query_motors");
             return LIB_ERROR;
+        }
         return 0;
+    }
     /**
      * @brief Converts set speeds (of various units) to absolute speeds.
+     *
      * @param speed The speed expressed in the desired units
      * @param units The units the desired speed is measured in
      * @return The absolute speed of the motor
      **/
     float MotorControl::rel_to_abs(float rel_speed, int units)
+    {
         switch(units)
+        {
           case MOTOR_ABSOLUTE:/* Speed given as absolute */
             return rel_speed;
           case MOTOR_PERCENT:/* Convert from percentage */
             return rel_speed * MAXSPEED / 100;
           case MOTOR_MMS:/* Convert from mm/s */
             /** @todo Make math to do this conversion **/
             return rel_speed;
           case MOTOR_CMS:/* Convert from cm/s */
             /** @todo Make math to do this conversion **/
             return rel_speed;
           default: /* The units aren't recognized */
             /** @todo Decide on default case. Either percent or absolute. **/
             return rel_speed;
+        }
+    }
     /**
      * @brief Convert absolute speeds to speeds of various units.
+     *
      * @param speed The speed expressed in absolute units.
      * @param units The units the desired speed is measured in.
      * @return The relative speed of the motor in desired units.
      **/
     float MotorControl::abs_to_rel(float abs_speed, int units)
+    {
         switch(units)
+        {
           case MOTOR_ABSOLUTE:/* Speed given as absolute */
             return abs_speed;
           case MOTOR_PERCENT:/* Convert from percentage */
             return abs_speed * 100 / MAXSPEED;
           case MOTOR_MMS:/* Convert from mm/s */
             /** @todo Make math to do this conversion **/
             return abs_speed;
           case MOTOR_CMS:/* Convert from cm/s */
             /** @todo Make math to do this conversion **/
             return abs_speed;
           default: /* The units aren't recognized */
             /** @todo Decide on default case. Either percent or absolute. **/
             return abs_speed;
+        }
+    }

     /**
      * Copyright (c) 2011 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 MotorControl.h
      * @brief Contains motor declarations and functions
+     *
      * Contains functions and definitions for the use of
      * motors
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      * @author Alex Zirbel
      **/
     #ifndef _MOTOR_CONTROL_H_
     #define _MOTOR_CONTROL_H_
     #include <ros/ros.h>
     /// @TODO maybe not so good! Then packages have to depend on libscout.
     #include <libscout/constants.h>
     #include <motors/query_motors.h>
     #include <motors/set_motors.h>
     /* Motor-specific defines */
     #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 MAXSPEED 255
     #define MOTOR_PERCENT 'p'
     #define MOTOR_MMS 'm'
     #define MOTOR_CMS 'c'
     #define MOTOR_ABSOLUTE 'a'
     #define MOTOR_DEFAULT_UNIT MOTOR_PERCENT
     class MotorControl
+    {
         public:
             /** Set up the motor node and prepare to communicate over ROS */
             MotorControl(const ros::NodeHandle& libscout_node);
             /** Uses which to specify what to change, and sets all to same speed */
             void set(int which, float speed, char units=MOTOR_DEFAULT_UNIT);
             /** Sets each side to a different speed */
             void set_sides(float speed_l, float speed_r,
                          char units=MOTOR_DEFAULT_UNIT);
             /** Sets each motor speed individually */
             void set_each(int which,
                         float speed_fl, float speed_fr,
                         float speed_bl, float speed_br,
                         char units=MOTOR_DEFAULT_UNIT);
             /** Requests the current motor speeds */
             float query(int which);
         private:
             /** Error if which sets a motor to both forward and backward */
             void check_which_ok(int which);
             float rel_to_abs(float rel_speed, int units);
             float abs_to_rel(float abs_speed, int units);
             /* Absolute speeds, but allowing fractions. Useful for PID, etc. */
             float motor_fl_speed;
             float motor_fr_speed;
             float motor_bl_speed;
             float motor_br_speed;
             /** ROS publisher and client declaration */
             ros::Publisher set_motors_pub;
             ros::ServiceClient query_motors_client;
             ros::NodeHandle node;
     };
     #endif

     /**
      * Copyright (c) 2011 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 alex_behavior.cpp
      * @brief A simple robot behavior, smart-runaround style.
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Alex Zirbel
      */
     #include "libscout.h"
     /**
      * @brief Main. The main function for the behavior.
+     *
      * This is the main function for libscout. It calls init() which initializes
      * the clients and publishers/subscribers for the other parts of the library.
      */
     int main(int argc, char **argv)
+    {
         ros::init(argc, argv, "libscout");
         ros::NodeHandle node;
         MotorControl motors(node);
         ros::Rate loop_rate(10);
         while(ros::ok())
+        {
             motors.set_sides(20, 80);
             ros::spinOnce();
             loop_rate.sleep();
+        }
         return 0;
+    }

scout/libscout/src/behavior.cpp
71	71	msg.bl_speed = 125;
72	72	msg.fr_speed = 125;
73	73	msg.br_speed = 125;
74		msg.which = 0;
75	74	msg.units = MOTOR_ABSOLUTE;
76	75
77	76	/* publish */

     /**
      * Copyright (c) 2011 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 constants.h
      * @brief Contains top level scout library constants
+     *
      * Contains constants for use in libscout
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      **/
     #ifndef _CONSTANTS_H_
     #define _CONSTANTS_H_
     #include "ros/ros.h"
     /* ROS defines */
     #define QUEUE_SIZE 10
     /* Libscout Defines */
     /* Init defines */
     #define LIB_ALL -1
     #define LIB_MOTORS 0x1
     #define LIB_SONAR 0x2
     #define LIB_CLIFFSENSORS 0x4
     #define LIB_HEADLIGHTS 0x8
     #define LIB_BUTTONS 0x10
     /* Status defines */
     //TODO MAKE ENUMS
     #define LIB_ERROR -1
     #define LIB_OK 0
     #endif

     /**
      * Copyright (c) 2011 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 libheadlights.cpp
      * @brief Contains headlights declarations and functions
+     *
      * Contains functions and definitions for the use of
      * headlights
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      **/
     #include "libheadlights.h"
     /* ROS node created in libscout.cpp */
     extern ros::NodeHandle node;
     /** ROS publisher and client declaration */
     ros::Publisher set_headlights_publisher;
     /*!
      * \brief Initialize the headlights module of libscout.
+     *
      * Initialize the libscout node as a publisher of set_headlights.
      */
     void libheadlights_init()
+    {
       set_headlights_publisher =
     		node.advertise<headlights::set_headlights>("libheadlights", 10);
+    }
     /*!
      * \brief Set headlight colors as a single RGB value
      * Sets the colors of the headlights as a web color value. Can selectively
      * select which motors to set, and which to remain at previous color.
      * \param color The color the headlights should be set to expressed as 0xRRGGBB
      * \param which A bitmask of which headlights should be set
      * \return Function status
      */
     int headlights_set(int color, int which)
+    {
       int red, green, blue;
     	red = (color & RED) >> REDSHIFT;
     	green = (color & GREEN) >> GREENSHIFT;
     	blue = (color & BLUE) >> BLUESHIFT;
     	return headlights_set_rgb(red, green, blue, which);
+    }
     /*!
      * \brief Set headlight colors as separate RGB values
      * Sets the colors of the headlights as a web color value. Can selectively
      * select which motors to set, and which to remain at previous color.
      * \param red The red value the headlights should be set to
      * \param green The green value the headlights should be set to
      * \param blue The blue value the headlights should be set to
      * \param which A bitmask of which headlights should be set
      * \return Function status
      */
     int headlights_set_rgb(int red, int green, int blue, int which)
+    {
       /** \todo Set fields of the message based on params */
     	headlights::set_headlights msg;
       if(!ros::ok())
+    	{
         return LIB_ERROR;
+      }
     	if(which & HL_LEFT)
+    	{
     		msg.left_red = red;
     		msg.left_green = green;
     		msg.left_blue = blue;
+    	}
     	else
+    	{
     		msg.left_red = NO_SET;
     		msg.left_green = NO_SET;
     		msg.left_blue = NO_SET;
+    	}
     	if(which & HL_RIGHT)
+    	{
     		msg.right_red = red;
     		msg.right_green = green;
     		msg.right_blue = blue;
+    	}
     	else
+    	{
     		msg.right_red = NO_SET;
     		msg.right_green = NO_SET;
     		msg.right_blue = NO_SET;
+    	}
       /* Publishes message to set_motors topic */
       set_headlights_publisher.publish(msg);
       ros::spinOnce();
       return LIB_OK;
+    }

     /**
      * Copyright (c) 2011 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 libheadlights.h
      * @brief Contains headlights declarations and functions
+     *
      * Contains functions and definitions for the use of
      * headlights
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      **/
     #include "ros/ros.h"
     #include "constants.h"
     #include "headlights/set_headlights.h"
     #ifndef _LIBheadlights_H_
     #define _LIBheadlights_H_
     /* Defines */
     #define WHITE		0xFFFFFF
     #define OFF			0x000000
     #define RED 		0xFF0000
     #define GREEN 	0x00FF00
     #define BLUE		0x0000FF
     #define YELLOW	0xFFFF00
     #define CYAN		0x00FFFF
     #define PINK		0xFF00FF
     #define HL_RIGHT	0x1
     #define HL_LEFT		0x2
     #define HL_BOTH		0x3
     #define NO_SET	-1
     #define REDSHIFT 		16
     #define GREENSHIFT 	 8
     #define BLUESHIFT		 0
     void libheadlights_init();
     int headlights_set(int color, int which);
     int headlights_set_rgb(int red, int green, int blue, int which);
     #endif

     /**
      * Copyright (c) 2011 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 libmotors.cpp
      * @brief Contains motor declarations and functions
+     *
      * Contains functions and definitions for the use of
      * motors
+     *
      * @author Colony Project, CMU Robotics Club
      **/
     #include "libmotors.h"
     /* ROS node created in libscout.cpp */
     extern ros::NodeHandle 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 =
             node.advertise<motors::set_motors>("set_motors", QUEUE_SIZE);
         query_motors_client =
             node.serviceClient<motors::query_motors>("query_motors");
+    }
     /**
      * @brief Sets all the speeds according to the specificiation of which motors
      *        to set.
+     *
      * @param which A bitmask of which motors need to be set.
      * @param speed The value to set the motors to.
      * @param units The units the speed is expressed in.
      * @return Function status
      */
     int motors_set(int which, float speed, char units)
+    {
         float speed_fl, speed_fr, speed_bl, speed_br;
         speed_fl = speed_fr = speed_bl = speed_br = speed;
         if(which & MOTOR_FL_REV)
+        {
             speed_fl = -speed;
+        }
         if(which & MOTOR_FR_REV)
+        {
             speed_fr = -speed;
+        }
         if(which & MOTOR_BL_REV)
+        {
             speed_bl = -speed;
+        }
         if(which & MOTOR_BR_REV)
+        {
             speed_br = -speed;
+        }
         return motors_set_each(which, speed_fl, speed_fr, speed_bl, speed_br,
                                units);
+    }
     /**
      * @brief Sets the left and right sides of scout to different speeds.
+     *
      * @param speed_l The speed of both left motors.
      * @param speed_r The speed of both right motors.
      * @param units The units to set to.
      * @return Function status
      */
     int motors_set_sides(float speed_l, float speed_r, char units)
+    {
         return motors_set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r,
                                    units);
+    }
     /**
      * @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 which A bitmask of which motors should be set.
      * @param speed The speed the motors should be set to.
      * @param units Optional units for the speeds.
      * @return Function status
      */
     int motors_set_each(int which,
                         float speed_fl, float speed_fr,
                         float speed_bl, float speed_br,
                         char units)
+    {
         check_which_ok(which);
         motors::set_motors msg;
         if(!ros::ok())
+        {
             return LIB_ERROR;
+        }
         /* Set all the speeds (the booleans in the set_motors message determine
          * which ones will be used) */
         msg.fl_speed = speed_fl;
         msg.fr_speed = speed_fr;
         msg.bl_speed = speed_bl;
         msg.br_speed = speed_br;
         /* Tell the motors node which motors need to be updated */
         msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
         if(which & (MOTOR_FL | MOTOR_FL_REV))
+        {
             msg.fl_set = true;
+        }
         if(which & (MOTOR_FR | MOTOR_FR_REV))
+        {
             msg.fr_set = true;
+        }
         if(which & (MOTOR_BL | MOTOR_BL_REV))
+        {
             msg.bl_set = true;
+        }
         if(which & (MOTOR_BR | MOTOR_BR_REV))
+        {
             msg.br_set = true;
+        }
         /* Specify which units the speeds are given in */
         msg.units = units;
         /* Publishes message to set_motors topic */
         set_motors_publisher.publish(msg);
         ros::spinOnce();
         return LIB_OK;
+    }
     /**
      * Double-checks the which variable to make sure the front and back
      * bits are not both set for a single motor.
+     *
      * @param which The which motor specfication to check.
      */
     void check_which_ok(int which)
+    {
         ROS_WARN_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
             "FL Motor set in both directions!");
         ROS_WARN_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
             "FR Motor set in both directions!");
         ROS_WARN_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
             "BL Motor set in both directions!");
         ROS_WARN_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
             "BR Motor set in both directions!");
+    }
     /**
      * @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.
+     *
      * @todo Change so we can get multiple motor speeds with one call
+     *
      * @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
      */
     float motors_query(int which)
+    {
         motors::query_motors srv;
         if(query_motors_client.call(srv))
+        {
             switch(which)
+            {
                 case MOTOR_FL:
                     return srv.response.fl_speed;
                 case MOTOR_FR:
                     return srv.response.fr_speed;
                 case MOTOR_BL:
                     return srv.response.bl_speed;
                 case MOTOR_BR:
                     return srv.response.br_speed;
                 default:
                     ROS_WARN("Bad WHICH in motors_query.");
                     return LIB_ERROR;
+            }
+        }
         else
+        {
             ROS_ERROR("Failed to call service query_motors");
             return LIB_ERROR;
+        }
         return 0;
+    }

     /**
      * Copyright (c) 2011 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 libmotors.h
      * @brief Contains motor declarations and functions
+     *
      * Contains functions and definitions for the use of
      * motors
+     *
      * @author Colony Project, CMU Robotics Club
      **/
     #include "ros/ros.h"
     #include "constants.h"
     #include "motors/query_motors.h"
     #include "motors/set_motors.h"
     #ifndef _LIBMOTORS_H_
     #define _LIBMOTORS_H_
     /* Defines */
     #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'
     #define MOTOR_ABSOLUTE 'a'
     #define MOTOR_DEFAULT_UNIT MOTOR_PERCENT
     void libmotors_init();
     int motors_set(int which, float speed, char units=MOTOR_DEFAULT_UNIT);
     int motors_set_sides(float speed_l, float speed_r,
                          char units=MOTOR_DEFAULT_UNIT);
     int motors_set_each(int which,
                         float speed_fl, float speed_fr,
                         float speed_bl, float speed_br,
                         char units=MOTOR_DEFAULT_UNIT);
     void check_which_ok(int which);
     float motors_query(int which);
     #endif

      * 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 libscout.cpp
-...
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Ben Wasserman
      **/
      */
     #include "libscout.h"
     /* Global objects */
     ros::NodeHandle 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
       main. However, this has to be able to point to different functions by
       changing as little code in this file as possible. Also, the user behaviors
       should live in a different directory (possibly a different package).
       However, this other package should not be executed as a separate node,
       because that is the job of the libscout node/package to handle all the ROS
       stuff, and keep it under the lid. The user only has to call normal c
       functions. I also want to reduce the need to recompile this package when the
       user behavior, or any other package that works below the surface.
     **/
     /** @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
      main. However, this has to be able to point to different functions by
      changing as little code in this file as possible. Also, the user behaviors
      should live in a different directory (possibly a different package).
      However, this other package should not be executed as a separate node,
      because that is the job of the libscout node/package to handle all the ROS
      stuff, and keep it under the lid. The user only has to call normal c
      functions. I also want to reduce the need to recompile this package when the
      user behavior, or any other package that works below the surface.
      */
     /*!
      * \brief Initializes modules in the libscout.
     /**
      * @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, int argc, char **argv){
     	ros::init(argc, argv, "libscout");
      * @param modules A bitmask of the modules that will be initialized.
      */
     int init(int modules, int argc, char **argv)
+    {
         ros::init(argc, argv, "libscout");
         ros::NodeHandle node;
         MotorControl motors = new MotorControl(node);
         HeadLightControl headlights = new HeadlightControl(node);
       /** \todo Copy this if for each module that gets added to the library */
       if(modules & LIB_MOTORS){
         libmotors_init();
+      }
     	if(modules & LIB_HEADLIGHTS){
         libheadlights_init();
+      }
       if(modules & LIB_BUTTONS){
         libbuttons_init();
+      }
       /** \todo Add other lib inits **/
       return 0;
         /** @todo Copy this if for each module that gets added to the library */
         /*if(modules & LIB_MOTORS)
+        {
             libmotors_init();
+        }
         if(modules & LIB_HEADLIGHTS)
+        {
             libheadlights_init();
+        }
         if(modules & LIB_BUTTONS)
+        {
             libbuttons_init();
         }*/
         /** @todo Add other lib inits **/
         return 0;
+    }

      * OTHER DEALINGS IN THE SOFTWARE.
      **/
     /**
      * @file libscout.h
      * @brief Contains top level scout library functions
-...
     #ifndef _LIBSCOUT_H_
     #define _LIBSCOUT_H_
     #include "ros/ros.h"
     #include "constants.h"

Project

General

Profile

Colony » Colony Scout » Colony Scout OS

Revision a8480867