/ - Diff - Colony Scout OS - Robotics Club

Revision 85538662

ID	855386623f71e8f31cd5440dc59858a73954bf2a

Added by Alex Zirbel over 12 years ago

Updated the sonar module prototype.

The biggest change to the proposed node is a restructuring of the sonar code style. Two services (sonar_toggle and sonar_set_scan) allow another node to turn the sonar on and off. The sonar constantly scans, keeps track of its position, and publishes all its readings in the topic sonar_direction.msg.

Added the appropriate message and service files, and the code skeleton.

The most obvious missing part of the code is the part which controls the movement of the sonar sensor, which should constantly scan between two ranges without user intervention.

scout/Makefile
1		include $(shell rospack find mk)/cmake_stack.mk
	1	include $(shell rospack find mk)/cmake_stack.mk

     set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
     #uncomment if you have defined messages
     #rosbuild_genmsg()
     rosbuild_genmsg()
     #uncomment if you have defined services
     rosbuild_gensrv()

     \mainpage
     \htmlinclude manifest.html
     \b sonar is ...
     <!--
     Provide an overview of your package.
     -->
     \b The sonar node manages the readings and motion of the sonar sensor. The
     sonar works by constantly scanning, and publishing the distance readings
     into a ros topic viewable by other nodes.
     \section codeapi Code API

     <package>
       <description brief="sonar">
          sonar
          Operates the sonar sensor and provides a way to access distance readings.
       </description>
       <author>Ben</author>
       <author>Alex</author>
       <license>BSD</license>
       <review status="unreviewed" notes=""/>
       <url>http://ros.org/wiki/sonar</url>

scout/sonar/msg/sonar_direction.msg
	1	# Gives a timestamp so nodes make sure they use current readings
	2	Header header
	3
	4	# Distance reading from the front sonar
	5	int8 distance0
	6	# Distance reading from the back sonar
	7	int8 distnace1

      * Implementation of functions for sonar use.
+     *
      * @author Colony Project, CMU Robotics Club
      * @author Benjamin Wasserman
      * @author Alex Zirbel
      **/
     #include "scout/sonar.h"
-...
      * @{
      **/
     int8_t sonar_stepper_pos; //This should only contain values in [0,47]
     /** Current position of the sonar. Should only contain values in [0,23]. */
     int8_t sonar_stepper_pos;
     bool is_on;
     /**
      * 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.
+     *
      * Initialize the sonar system. This must be called before sonar_read.
+     *
      * @see sonar_read
      * @see sonar_read_raw
      **/
     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;
     void sonar_init(void)
+    {
         sonar_stepper_pos = 0;
+    }
     /**
      * 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();
      * Turns the sonar on and off.
      */
     bool handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
                              sonar::sonar_toggle::Response &res)
+    {
         if (req.set_on)
             ROS_INFO("Turning on the sonar [unimplemented]");
         else
             ROS_INFO("Turning off the sonar [unimplemented]");
         res.ack = true;
       return 0;
         return true;
+    }
     /**
      * Initialize the sonar system. This must be called before sonar_read.
      * @brief Sets the sonar scan range. Values from 0 to 23 are valid.
+     *
      * @see sonar_read
      * @see sonar_read_raw
      **/
     void sonar_init(void){
       sonar_stepper_pos = 0;
+    }
      * 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
     /**
      * 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;
+      }
         ROS_INFO("Setting sonar scan range to [%s, %s] [unimplemented]",
                  req.stop_l,
                  req.stop_r);
         res.ack = true;
         return true;
+    }
     /**
      * 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
      * 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.
      **/
     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;
     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");
         ros::spin();
         return 0;
+    }
     /** @} **/

      * @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());

scout/sonar/srv/query_sonar.srv
1		int8 direction
2		---
3		#front sonar
4		int16 distance0
5		#back sonar
6		int16 distance1

scout/sonar/srv/sonar_set_scan.srv
	1	# Specify the left and right position stops for the scan.
	2	# Valid position stops are 0 to 23.
	3	int8 stop_l
	4	int8 stop_r
	5	---
	6	# Sonar responds with an acknowledgement; true means no errors
	7	bool ack

scout/sonar/srv/sonar_toggle.srv
	1	# Set to true to turn the sonar on; false to turn off
	2	bool set_on
	3	---
	4	# Sonar responds with an acknowledgement; true means no errors
	5	bool ack

Also available in: Unified diff

Project

General

Profile

Colony » Colony Scout » Colony Scout OS

Revision 85538662