[Playerstage-commit] code/player/utils/playerv dev_ranger.c, NONE, 1.1 Makefile.am, 1.42, 1.43 dev_laser.c, 1.26, 1.27 playerv.c, 1.47, 1.48 playerv.h, 1.48, 1.49 registry.c, 1.27, 1.28

[Toby C. <th...@us...>]

Update of /cvsroot/playerstage/code/player/utils/playerv
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22655/utils/playerv

Modified Files:
	Makefile.am dev_laser.c playerv.c playerv.h registry.c 
Added Files:
	dev_ranger.c 
Log Message:
added geoffs ranger interface


Index: registry.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/registry.c,v
retrieving revision 1.27
retrieving revision 1.28
diff -C2 -d -r1.27 -r1.28
*** registry.c	3 May 2006 12:29:12 -0000	1.27
--- registry.c	20 May 2007 00:30:15 -0000	1.28
***************
*** 101,104 ****
--- 101,113 ----
        break;
  
+     case PLAYER_RANGER_CODE:
+       device->proxy = ranger_create(mainwnd, opt, client,
+                                     device->addr.index,
+                                     device->drivername,
+                                     device->subscribe);
+       device->fndestroy = (fndestroy_t) ranger_destroy;
+       device->fnupdate = (fnupdate_t) ranger_update;
+       break;
+ 
      case PLAYER_SONAR_CODE:
        device->proxy = sonar_create(mainwnd, opt, client,

Index: playerv.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/playerv.h,v
retrieving revision 1.48
retrieving revision 1.49
diff -C2 -d -r1.48 -r1.49
*** playerv.h	31 Jan 2007 22:09:57 -0000	1.48
--- playerv.h	20 May 2007 00:30:15 -0000	1.49
***************
*** 230,233 ****
--- 230,234 ----
    rtk_menuitem_t *res025_item, *res050_item, *res100_item;
    rtk_menuitem_t *range_mm_item, *range_cm_item, *range_dm_item;
+   rtk_menuitem_t *style_item;
    // Figure for drawing the scan
    rtk_fig_t *scan_fig;
***************
*** 534,537 ****
--- 535,583 ----
  
  /***************************************************************************
+  * Ranger
+  ***************************************************************************/
+ 
+ // Ranger device info
+ typedef struct
+ {
+   // Driver name
+   char *drivername;
+ 
+   // Ranger device proxy
+   playerc_ranger_t *proxy;
+ 
+   // Timestamp on most recent data
+   double datatime;
+ 
+   // Menu stuff
+   rtk_menu_t *menu;
+   rtk_menuitem_t *subscribe_item;
+   rtk_menuitem_t *style_item;
+   rtk_menuitem_t *intns_item;
+   rtk_menuitem_t *device_item;
+   // Figure for drawing the scan
+   rtk_fig_t **scan_fig;
+   // Need to track this for creating figures
+   mainwnd_t *mainwnd;
+ 
+   // Properties from the device that may be necessary
+   double start_angle;
+   double resolution;
+ 
+ } ranger_t;
+ 
+ 
+ // Create a ranger device
+ ranger_t *ranger_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
+                         int index,  const char *drivername, int subscribe);
+ 
+ // Destroy a ranger device
+ void ranger_destroy(ranger_t *ranger);
+ 
+ // Update a ranger device
+ void ranger_update(ranger_t *ranger);
+ 
+ 
+ /***************************************************************************
   * Blobfinder device
   ***************************************************************************/

Index: dev_laser.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/dev_laser.c,v
retrieving revision 1.26
retrieving revision 1.27
diff -C2 -d -r1.26 -r1.27
*** dev_laser.c	8 Oct 2005 02:38:34 -0000	1.26
--- dev_laser.c	20 May 2007 00:30:15 -0000	1.27
***************
*** 59,62 ****
--- 59,63 ----
    laser->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
    laser->subscribe_item = rtk_menuitem_create(laser->menu, "Subscribe", 1);
+   laser->style_item = rtk_menuitem_create(laser->menu, "Filled", 1);
  #if 0
    laser->res025_item = rtk_menuitem_create(laser->menu, "0.25 deg resolution", 1);
***************
*** 71,74 ****
--- 72,76 ----
    // Set the initial menu state
    rtk_menuitem_check(laser->subscribe_item, subscribe);
+   rtk_menuitem_check(laser->style_item, 1);
  
    // Construct figures
***************
*** 97,100 ****
--- 99,103 ----
  #endif
    rtk_menuitem_destroy(laser->subscribe_item);
+   rtk_menuitem_destroy(laser->style_item);
    rtk_menu_destroy(laser->menu);
  
***************
*** 226,236 ****
    rtk_fig_clear(laser->scan_fig);
  
!   // TESTING (should use menu option)
!   if (laser->proxy->info.addr.index == 0)
!     style = 1;
!   else
!     style = 0;
! 
!   if (style == 0)
    {
      rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC);
--- 229,233 ----
    rtk_fig_clear(laser->scan_fig);
  
!   if (!rtk_menuitem_ischecked(laser->style_item))
    {
      rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC);
***************
*** 251,255 ****
      }
    }
!   else if (style == 1)
    {
      res = laser->proxy->scan_res / 2;
--- 248,252 ----
      }
    }
!   else
    {
      res = laser->proxy->scan_res / 2;

Index: playerv.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/playerv.c,v
retrieving revision 1.47
retrieving revision 1.48
diff -C2 -d -r1.47 -r1.48
*** playerv.c	19 Aug 2006 01:17:26 -0000	1.47
--- playerv.c	20 May 2007 00:30:15 -0000	1.48
***************
*** 88,91 ****
--- 88,92 ----
  - @ref interface_power
  - @ref interface_ptz
+ - @ref interface_ranger
  - @ref interface_sonar
  - @ref interface_wifi

Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/Makefile.am,v
retrieving revision 1.42
retrieving revision 1.43
diff -C2 -d -r1.42 -r1.43
*** Makefile.am	3 May 2006 12:29:12 -0000	1.42
--- Makefile.am	20 May 2007 00:30:15 -0000	1.43
***************
*** 29,33 ****
                    dev_sonar.c \
                    dev_wifi.c \
!                   dev_gripper.c
  #dev_localize.c
  
--- 29,34 ----
                    dev_sonar.c \
                    dev_wifi.c \
!                   dev_gripper.c \
!                   dev_ranger.c
  #dev_localize.c
  

--- NEW FILE: dev_ranger.c ---
/*
 *  PlayerViewer
 *  Copyright (C) Andrew Howard 2002
 *
 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */

#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "playerv.h"

// Draw the ranger scan
void ranger_draw(ranger_t *ranger);

// Create a ranger device
ranger_t* ranger_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
                        int index, const char *drivername, int subscribe)
{
  char label[64];
  char section[64];
  ranger_t *ranger;

  ranger = malloc(sizeof(ranger_t));

  ranger->proxy = playerc_ranger_create(client, index);
  ranger->drivername = strdup(drivername);
  ranger->datatime = 0;

  snprintf(section, sizeof(section), "ranger:%d", index);

  // Construct the menu
  snprintf(label, sizeof(label), "ranger:%d (%s)", index, ranger->drivername);
  ranger->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
  ranger->subscribe_item = rtk_menuitem_create(ranger->menu, "Subscribe", 1);
  ranger->style_item = rtk_menuitem_create(ranger->menu, "Filled", 1);
  ranger->intns_item = rtk_menuitem_create(ranger->menu, "Draw intensity data", 1);
  ranger->device_item = rtk_menuitem_create(ranger->menu, "Singular", 1);

  // Set the initial menu state
  rtk_menuitem_check(ranger->subscribe_item, subscribe);
  rtk_menuitem_check(ranger->style_item, 1);
  rtk_menuitem_check(ranger->intns_item, 1);

  ranger->mainwnd = mainwnd;
  ranger->scan_fig = NULL;

  return ranger;
}


void ranger_delete_figures(ranger_t *ranger)
{
  int ii;

  if (ranger->scan_fig != NULL)
  {
    for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
      rtk_fig_destroy(ranger->scan_fig[ii]);
    free(ranger->scan_fig);
    ranger->scan_fig = NULL;
  }
}


// Destroy a ranger device
void ranger_destroy(ranger_t *ranger)
{
  ranger_delete_figures(ranger);

  if (ranger->proxy->info.subscribed)
    playerc_ranger_unsubscribe(ranger->proxy);
  playerc_ranger_destroy(ranger->proxy);

  rtk_menuitem_destroy(ranger->subscribe_item);
  rtk_menuitem_destroy(ranger->style_item);
  rtk_menuitem_destroy(ranger->intns_item);
  rtk_menuitem_destroy(ranger->device_item);
  rtk_menu_destroy(ranger->menu);

  free(ranger->drivername);

  free(ranger);
}


// Update a ranger device
void ranger_update(ranger_t *ranger)
{
  int ii;

  // Update the device subscription
  if (rtk_menuitem_ischecked(ranger->subscribe_item))
  {
    if (!ranger->proxy->info.subscribed)
    {
      if (playerc_ranger_subscribe(ranger->proxy, PLAYER_OPEN_MODE) != 0)
        PRINT_ERR1("libplayerc error: %s", playerc_error_str());

      // Get the ranger geometry
      if (playerc_ranger_get_geom(ranger->proxy) != 0)
        PRINT_ERR1("libplayerc error: %s", playerc_error_str());

      // Try to get the min_angle and resolution properties (don't care if can't get them)
      if (playerc_device_get_dblprop(&ranger->proxy->info, "min_angle", &ranger->start_angle) != 0)
        ranger->start_angle = 0.0f;
      if (playerc_device_get_dblprop(&ranger->proxy->info, "resolution", &ranger->resolution) != 0)
        ranger->resolution = 0.0f;

      // Delete any current figures
      ranger_delete_figures(ranger);
      // Create the figures
      if ((ranger->scan_fig = malloc(ranger->proxy->sensor_count * sizeof(rtk_fig_t*))) == NULL )
      {
        PRINT_ERR1("Failed to allocate memory for %d figures to display ranger", ranger->proxy->sensor_count);
        return;
      }
      for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
      {
        ranger->scan_fig[ii] = rtk_fig_create(ranger->mainwnd->canvas, ranger->mainwnd->robot_fig, 1);
        rtk_fig_origin(ranger->scan_fig[ii],
                       ranger->proxy->sensor_poses[ii].px,
                       ranger->proxy->sensor_poses[ii].py,
                       ranger->proxy->sensor_poses[ii].pyaw);
      }
    }
  }
  else
  {
    if (ranger->proxy->info.subscribed)
      if (playerc_ranger_unsubscribe(ranger->proxy) != 0)
        PRINT_ERR1("libplayerc error: %s", playerc_error_str());
    ranger_delete_figures(ranger);
  }
  rtk_menuitem_check(ranger->subscribe_item, ranger->proxy->info.subscribed);

  if (ranger->proxy->info.subscribed)
  {
    // Draw in the ranger scan if it has been changed
    if (ranger->proxy->info.datatime != ranger->datatime)
    {
      ranger_draw(ranger);
      ranger->datatime = ranger->proxy->info.datatime;
    }
  }
  else
  {
    // Dont draw the scan
    for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
      rtk_fig_show(ranger->scan_fig[ii], 0);
  }
}

// Converts a range reading into a point in the CS of the ranger device
// based on the pose of the sensor the reading is likely to belong to
void range_to_point(ranger_t *ranger, int index, int sensor_num, double angle, double *point)
{
  // Point position = point from range -> rotate by range angle// -> rotate by sensor yaw -> translate by sensor position
  point[0] = ranger->proxy->ranges[index] * cos(angle);// + ranger->proxy->sensor_poses[sensor_num].pyaw) + ranger->proxy->sensor_poses[sensor_num].px;
  point[1] = ranger->proxy->ranges[index] * sin(angle);// + ranger->proxy->sensor_poses[sensor_num].pyaw) + ranger->proxy->sensor_poses[sensor_num].py;
}

// Draw the ranger scan
void ranger_draw(ranger_t *ranger)
{
  int ii = 0, jj = 0;
  int point_count;
  double point1[2], point2[2], points[PLAYER_LASER_MAX_SAMPLES + 1][2];
  double current_angle = 0.0f, temp = 0.0f;
  unsigned int ranges_per_sensor = 0;

  // Drawing type depends on the selected sensor type
  // Singular sensors (e.g. sonar sensors):
  //   Draw a cone for the first range scan of each sensor
  // Non-singular sensors (e.g. laser scanner):
  //   Draw the edge of the scan and empty space

  // Calculate the number of ranges per sensor
  if (ranger->proxy->sensor_count == 0)
    ranges_per_sensor = ranger->proxy->ranges_count;
  else
    ranges_per_sensor = ranger->proxy->ranges_count / ranger->proxy->sensor_count;

  if (rtk_menuitem_ischecked(ranger->device_item))
  {
    // Draw sonar-like
    temp = 20.0f * M_PI / 180.0f / 2.0f;
    for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
    {
      rtk_fig_show(ranger->scan_fig[ii], 1);
      rtk_fig_clear(ranger->scan_fig[ii]);
      rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_SONAR_SCAN);

      // Draw a cone for the first range for each sensor
      // Assume the range is straight ahead (ignore min_angle and resolution properties)
      points[0][0] = 0.0f;
      points[0][1] = 0.0f;
      points[1][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(-temp);
      points[1][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(-temp);
      points[2][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(temp);
      points[2][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(temp);
      rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, 3, points, 1);

      // Draw the sensor itself
      rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER);
      rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0);
    }
  }
  else
  {
    // Draw laser-like
    if (rtk_menuitem_ischecked(ranger->style_item))
    {
      // Draw each sensor in turn
      for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
      {
        rtk_fig_show(ranger->scan_fig[ii], 1);
        rtk_fig_clear(ranger->scan_fig[ii]);

        // Draw empty space
        points[0][0] = ranger->proxy->sensor_poses[ii].px;
        points[0][1] = ranger->proxy->sensor_poses[ii].py;
        point_count = 1;
        current_angle = ranger->start_angle;
        // Loop over the ranges
        for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
        {
          range_to_point(ranger, jj, ii, current_angle, points[point_count]);
          // Move round to the angle of the next range
          current_angle += ranger->resolution;
          point_count++;
        }
        rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_EMP);
        rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, point_count, points, 1);

        // Draw occupied space
        rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
        current_angle = ranger->start_angle;
        for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
        {
          range_to_point(ranger, jj, ii, current_angle - ranger->resolution, point1);
          range_to_point(ranger, jj, ii, current_angle + ranger->resolution, point2);
          rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0], point2[1]);
          current_angle += ranger->resolution;
        }
      }
    }
    else
    {
      // Draw a range scan for each individual sensor in the device
      for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
      {
        rtk_fig_show(ranger->scan_fig[ii], 1);
        rtk_fig_clear(ranger->scan_fig[ii]);

        rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
        current_angle = ranger->start_angle;
        // Get the first point
        range_to_point(ranger, ii * ranges_per_sensor, ii, ranger->start_angle, point1);
        // Loop over the rest of the ranges
        for (jj = ii * ranges_per_sensor + 1; jj < (ii + 1) * ranges_per_sensor; jj++)
        {
          range_to_point(ranger, jj, ii, current_angle, point2);
          // Draw a line from point 1 (previous point) to point 2 (current point)
          rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0], point2[1]);
          point1[0] = point2[0];
          point1[1] = point2[1];
          // Move round to the angle of the next range
          current_angle += ranger->resolution;
        }
      }
    }

    // For each sensor...
    for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
    {
      if (rtk_menuitem_ischecked(ranger->intns_item))
      {
        // Draw an intensity scan
        if (ranger->proxy->intensities_count > 0)
        {
          current_angle = ranger->start_angle;
          for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
          {
            if (ranger->proxy->intensities[0] != 0)
            {
              range_to_point(ranger, jj, ii, current_angle, point1);
              rtk_fig_rectangle(ranger->scan_fig[ii], point1[0], point1[1], 0, 0.05, 0.05, 1);
            }
            current_angle += ranger->resolution;
          }
        }
      }

      // Draw the sensor itself
      rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER);
      rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0);
    }
  }
}