Colony » Colony Scout

#define LIB_ALL -1

#define LIB_MOTORS 0x1

#define LIB_SONAR 0x2

#define LIB_CLIFFSENSORS 0x4

#define LIB_HEADLIGHTS 0x8

//TODO MAKE ENUMS

#define LIB_ERROR -1

#define LIB_OK 0

 * Copyright (c) 2011 Colony Project

/* 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.

 */

/*!

 * \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){

  /* Set the speed for each motor according to the which bitmask*/

  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;

  /* Publishes message to set_motors topic */

  set_motors_publisher.publish(msg);

/*!

 * \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

 */

      default:

 * Copyright (c) 2011 Colony Project

#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'

 * Copyright (c) 2011 Colony Project

/* Global objects */

ros::NodeHandle libscout_node;

/*!

 * \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

 **/

//  motors_set(100, MOTOR_ALL);

  ROS_INFO("%d", motors_query(MOTOR_FL));

/*!

 * \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.

 **/

  /** \todo Copy this if for each module that gets added to the library */

 * Copyright (c) 2011 Colony Project

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})

include $(shell rospack find mk)/cmake.mk

/**

\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.

-->

*/

<package>

  <description brief="messages">

     messages

  </description>

  <author>Ben</author>

  <license>BSD</license>

  <review status="unreviewed" notes=""/>

  <url>http://ros.org/wiki/messages</url>

</package>

int8 direction

---

#front sonar

int16 distance0

#back sonar

int16 distance1

int8 units

 * Copyright (c) 2011 Colony Project

//#include "libscout/src/constants.h"

/* 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. */

  ROS_DEBUG("Motor speeds queried");

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})

include $(shell rospack find mk)/cmake.mk

/**

\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.

-->

*/

<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>

/**

 * 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;

}

/**

 * 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

int8 direction

---

#front sonar

int16 distance0

#back sonar

int16 distance1