Colony » Colony Scout » Colony Scout OS

/**

 * 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 sonar.cpp

 * @brief Sonar

 *

 * Implementation of functions for sonar use.

 *

 * @author Colony Project, CMU Robotics Club

 * @author Alex Zirbel

 **/

#include <ros/ros.h>

#include <cstdlib>

#include "sonar.h"

/**

 * @defgroup sonar Sonar

 * @brief Functions for using the sonar

 *

 * @{

 **/

/** Current position of the sonar. Should only contain values in [0,23]. */

int8_t sonar_stepper_pos;

bool is_on;

/**

 * Initialize the sonar system. This must be called before sonar_read.

 * 

 * @see sonar_read

 * @see sonar_read_raw

 **/

void sonar_init(void)

{

    ROS_INFO("Sonar_init called [unimplemented].");

    sonar_stepper_pos = 0;

}

/**

 * Turns the sonar on and off.

 */

bool handle_sonar_toggle(sonar::sonar_toggle::Request  &req,

                         sonar::sonar_toggle::Response &res)

{

    if (req.set_on && !is_on)

    {

        ROS_INFO("Turning on the sonar [unimplemented]");

    }

    else if (req.set_off && is_on)

    {

        ROS_INFO("Turning off the sonar [unimplemented]");

    }

    res.ack = true;

    return true;

}

/**

 * @brief Sets the sonar scan range. Values from 0 to 23 are valid.

 * 

 * Handles setting the sonar scan range. The scan range is specified by

 * the front sonar, so a scan range of 0 to 23 is a full scan.

 * After the scan range is set, the robot will continue reading values

 * only from that range until the range is reset. Old values can be identified

 * by the header on the distance reading message.

 */

bool handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,

                           sonar::sonar_set_scan::Response &res)

{

    // Code to set the sonar to scan from

    // req.stop_l to req.stop_r

    ROS_INFO("Setting sonar scan range to [%s, %s] [unimplemented]",

             req.stop_l,

             req.stop_r);

    res.ack = true;

    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.

 **/

void main(int argc, char **argv)

{

    is_on = true;

    /* Initialize hardware for sonar */

    // Hardware init functions here

    sonar_init();

    /* Initialize the sonar driver node in ROS */

    ros::init(argc, argv, "sonar_driver");

    /* Advertise messages and services */

    ros::NodeHandle n;

    ros:ServiceServer service0 = n.advertiseService("sonar_toggle",

                                                    handle_sonar_toggle);

    ros:ServiceServer service1 = n.advertiseService("sonar_set_scan",

                                                    handle_sonar_set_scan);

    ROS_INFO("Sonar: Ready");

    // Sonar reading and publishing here

    ros::spin();

    return 0;

}

/** @} **/