Colony » Colony Scout

/**

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

}