utils.c

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/***************************************************************************
 * Desc: Some utilities (in here for speed)
 * Author: Andrew H
 * Date: 8 Oct 2004
 * CVS: $Id: utils.c,v 1.3 2004/12/06 06:36:43 inspectorg Exp $
 **************************************************************************/

#include "Python.h"


// Convert laser ranges to a set of points
static PyObject *ranges_to_points(PyObject *self, PyObject *args)
{
  int i, len;
  double min, res, scale;
  double x, y, r, b;
  PyObject *pysrc, *pydst, *pyt;
    
  if (!PyArg_ParseTuple(args, "dddiO", &min, &res, &scale, &len, &pysrc))
    return NULL;
  if (!PySequence_Check(pysrc))
    return NULL;
  if (len > PySequence_Length(pysrc))
    return PyErr_Format(PyExc_IndexError, "range data has invalid length");
  
  pydst = PyList_New(len);
  
  for (i = 0; i < len; i++)
  {
    pyt = PySequence_GetItem(pysrc, i);
    r = PyFloat_AsDouble(pyt);
    Py_DECREF(pyt);
    
    b = min + res * i;    
    x = r * cos(b) / scale;
    y = r * sin(b) / scale;

    pyt = PyTuple_New(2);
    PyTuple_SetItem(pyt, 0, PyFloat_FromDouble(x));
    PyTuple_SetItem(pyt, 1, PyFloat_FromDouble(y));
    PyList_SetItem(pydst, i, pyt);
  }
  
  return pydst;
}



// Convert disparity images to RGB images.
// Both input and output images are represented by strings.
static PyObject *disparity_to_rgb(PyObject *self, PyObject *args)
{
  PyObject *pysrc, *pydst;
  float *src;
  float max_disp;
  unsigned char *dst, *d;  
  int src_len, dst_len;
  int i, c;

  if (!PyArg_ParseTuple(args, "O!", &PyString_Type, &pysrc))
    return NULL;
  
  PyString_AsStringAndSize(pysrc, (char**) &src, &src_len);
  src_len /= sizeof(float);
  dst_len = src_len * 3;
  dst = malloc(dst_len);

  // Auto-gain control
  max_disp = 0.0;
  for (i = 0; i < src_len; i++)
  {
    if (src[i] > max_disp)
      max_disp = src[i];
  }
  
  d = dst;
  for (i = 0; i < src_len; i++)
  {
    c = (int) (255 * src[i] / max_disp);
    d[0] = c;
    d[1] = c;    
    d[2] = c;
    d += 3;
  }

  pydst = PyString_FromStringAndSize(dst, dst_len);
  free(dst);
  
  return pydst;
}


// Module methods
static PyMethodDef module_methods[] =
{
  {"ranges_to_points", ranges_to_points, METH_VARARGS},
  {"disparity_to_rgb", disparity_to_rgb, METH_VARARGS},
  {NULL, NULL}
};


// Initialize the module
void initutils(void)
{
  PyObject *moduleob;        
  moduleob = Py_InitModule("utils", module_methods);

  return;
}