khepera.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *
 *
 *  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
 *
 */

/* Copyright (C) 2004
 *   Toby Collett, University of Auckland Robotics Group
 */


/** @ingroup drivers */
/** @{ */
/** @defgroup driver_khepera khepera
 * @brief K-Team Khepera mobile robot

The khepera driver is used to interface to the K-Team khepera robot. 

This driver is experimental and should be treated with caution. At
this point it supports the @ref interface_position2d and 
@ref interface_ir interfaces.

TODO: 
 - Add support for position control (currently only velocity control)
 - Add proper calibration for IR sensors

@par Compile-time dependencies

- none

@par Provides

- @ref interface_position2d
- @ref interface_ir

@par Requires

- none

@par Supported configuration requests

- The @ref interface_position2d interface supports:
  - PLAYER_POSITION_GET_GEOM_REQ
  - PLAYER_POSITION_MOTOR_POWER_REQ
  - PLAYER_POSITION_VELOCITY_MODE_REQ
  - PLAYER_POSITION_RESET_ODOM_REQ
  - PLAYER_POSITION_SET_ODOM_REQ
- The @ref interface_ir interface supports:
  - PLAYER_IR_POSE_REQ

@par Configuration file options

- port (string)
  - Default: "/dev/ttyUSB0"
  - Serial port used to communicate with the robot.
- scale_factor (float)
  - Default: 10
  - As the khepera is so small the actual geometry doesnt make much sense
    with many of the existing defaults so the geometries can all be scaled
    by this factor.
- encoder_res (float)
  - Default: 1.0/12.0
  - The wheel encoder resolution.
- position_pose (float tuple)
  - Default: [0 0 0]
  - The pose of the robot in player coordinates (mm, mm, deg).
- position_size (float tuple)
  - Default: [57 57]
  - The size of the robot approximated to a rectangle (mm, mm).
- ir_pose_count (integer)
  - Default: 8
  - The number of ir poses.
- ir_poses (float tuple)
  - Default: [10 24 90 19 17 45 25 6 0 25 -6 0 19 -17 -45 10 -24 -90 -24 -10 180 -24 10 180]
  - Poses of the IRs (mm mm deg for each one)

@par Example 

@verbatim
driver
(
  name "khepera"
  provides ["position2d:0" "ir:0"]
)
@endverbatim

@author Toby Collett
*/
/** @} */

#include <fcntl.h>
#include <signal.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>
#include <stdlib.h>  /* for abs() */
#include <netinet/in.h>
#include <ctype.h>

#include <assert.h>

#include <khepera.h>

#include <libplayercore/playercore.h>

// we need to debug different things at different times
//#define DEBUG_POS
//#define DEBUG_SERIAL
#define DEBUG_CONFIG

// useful macros
#define DEG2RAD(x) (((double)(x))*0.01745329251994)
#define RAD2DEG(x) (((double)(x))*57.29577951308232)

//#define DEG2RAD_FIX(x) ((x) * 17453)
//#define RAD2DEG_FIX(x) ((x) * 57295780)
#define DEG2RAD_FIX(x) ((x) * 174)
#define RAD2DEG_FIX(x) ((x) * 572958)


/* initialize the driver.
 *
 * returns: pointer to new REBIR object
 */
Driver*
Khepera_Init(ConfigFile *cf, int section)
{
  return (Driver *) new Khepera( cf, section);
}

/* register the Khepera IR driver in the drivertable
 *
 * returns: 
 */
void
Khepera_Register(DriverTable *table) 
{
  table->AddDriver("khepera", Khepera_Init);
}

int Khepera::ProcessMessage(MessageQueue * resp_queue, player_msghdr * hdr, void * data)
{
	assert(hdr);
	assert(data);

	if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_POSE, ir_addr))
	{
		Publish(ir_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geometry->ir, sizeof(geometry->ir));
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, position_addr))
	{
		player_position2d_cmd_vel_t * poscmd = reinterpret_cast<player_position2d_cmd_vel_t *> (data);

		// need to calculate the left and right velocities
		int transvel = static_cast<int> (static_cast<int> (poscmd->vel.px) * geometry->encoder_res);
		int rotvel = static_cast<int> (static_cast<int> (poscmd->vel.pa) * geometry->encoder_res * geometry->position.size.sw* geometry->scale);
		int leftvel = transvel - rotvel;
		int rightvel = transvel + rotvel;

		// now we set the speed
		if (this->motors_enabled) 
			SetSpeed(leftvel,rightvel);
		else 
			SetSpeed(0,0);
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, position_addr))
	{
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geometry->position, sizeof(geometry->position));
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, position_addr))
	{
		this->motors_enabled = ((player_position2d_power_config_t *)data)->state;
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,hdr->subtype);
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, position_addr))
	{
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,hdr->subtype);
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM, position_addr))
	{
		ResetOdometry();
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,hdr->subtype);
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, position_addr))
	{
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,hdr->subtype);
		return 0;
	}

	return -1;
}

Khepera::Khepera(ConfigFile *cf, int section) : Driver(cf, section)
{
  // zero ids, so that we'll know later which interfaces were requested
  memset(&position_addr, 0, sizeof(this->position_addr));
  memset(&ir_addr, 0, sizeof(ir_addr));

  this->position_subscriptions = this->ir_subscriptions = 0;

  // Do we create a robot position interface?
  if(cf->ReadDeviceAddr(&(this->position_addr), section, "provides", 
                      PLAYER_POSITION2D_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->position_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }

  // Do we create an ir interface?
  if(cf->ReadDeviceAddr(&(this->ir_addr), section, "provides", 
                      PLAYER_IR_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->ir_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }

  // Read config file options
  geometry = new player_khepera_geom_t;
  geometry->PortName= NULL;
  geometry->scale = 0;

  //set up the poll parameters... used for the comms
  // over the serial port to the Kam
  //write_pfd.events = POLLOUT;
  //read_pfd.events = POLLIN;

  // now we have to look up our parameters.  this should be given as an argument
  if (geometry->PortName == NULL)
    geometry->PortName = strdup(cf->ReadString(section, "port", KHEPERA_DEFAULT_SERIAL_PORT));
  if (geometry->scale == 0)
    geometry->scale = cf->ReadFloat(section, "scale_factor", KHEPERA_DEFAULT_SCALE);

  // set sub type here
//  geometry->position.subtype = PLAYER_POSITION_GET_GEOM_REQ;

  geometry->encoder_res = cf->ReadFloat(section,"encoder_res", KHEPERA_DEFAULT_ENCODER_RES);

  // Load position config
  geometry->position.pose.px = cf->ReadTupleFloat(section,"position_pose",0,0) * geometry->scale;
  geometry->position.pose.py = cf->ReadTupleFloat(section,"position_pose",1,0) * geometry->scale;
  geometry->position.pose.pa = cf->ReadTupleFloat(section,"position_pose",2,0) * geometry->scale;

  // load dimension of the base
  geometry->position.size.sw = cf->ReadTupleFloat(section,"position_size",0,57) * geometry->scale;
  geometry->position.size.sl = cf->ReadTupleFloat(section,"position_size",1,57) * geometry->scale;

  // load ir geometry config
  geometry->ir.poses_count = (cf->ReadInt(section,"ir_pose_count", 8));
  if (geometry->ir.poses_count == 8 && cf->ReadTupleFloat(section,"ir_poses",0,-1) == -1)
  {
    // load the default ir geometry
    geometry->ir.poses[0].px = 10/1000*geometry->scale;
    geometry->ir.poses[0].py = 24/1000*geometry->scale;
    geometry->ir.poses[0].pa = DTOR(90);

    geometry->ir.poses[1].px = 19/1000*geometry->scale;
    geometry->ir.poses[1].py = 17/1000*geometry->scale;
    geometry->ir.poses[1].pa = DTOR(45);

    geometry->ir.poses[2].px = 25/1000*geometry->scale;
    geometry->ir.poses[2].py = 6/1000*geometry->scale;
    geometry->ir.poses[2].pa = DTOR(0);

    geometry->ir.poses[3].px = 25/1000*geometry->scale;
    geometry->ir.poses[3].py = -6/1000*geometry->scale;
    geometry->ir.poses[3].pa = DTOR(0);

    geometry->ir.poses[4].px = 19/1000*geometry->scale;
    geometry->ir.poses[4].py = -17/1000*geometry->scale;
    geometry->ir.poses[4].pa = DTOR(-45);

    geometry->ir.poses[5].px = 10/1000*geometry->scale;
    geometry->ir.poses[5].py = -24/1000*geometry->scale;
    geometry->ir.poses[5].pa = DTOR(-90);

    geometry->ir.poses[6].px = -24/1000*geometry->scale;
    geometry->ir.poses[6].py = -10/1000*geometry->scale;
    geometry->ir.poses[6].pa = DTOR(180);

    geometry->ir.poses[7].px = -24/1000*geometry->scale;
    geometry->ir.poses[7].py = 10/1000*geometry->scale;
    geometry->ir.poses[7].pa = DTOR(180);
  }
  else
  {
    // laod geom from config file
    for (unsigned int i = 0; i < geometry->ir.poses_count; ++i)
    {
      geometry->ir.poses[i].px = cf->ReadTupleFloat(section,"ir_poses",3*i,0)*geometry->scale;
      geometry->ir.poses[i].py = cf->ReadTupleFloat(section,"ir_poses",3*i+1,0)*geometry->scale;
      geometry->ir.poses[i].pa = cf->ReadTupleFloat(section,"ir_poses",3*i+2,0);
    }				
  }
  // laod ir calibration from config file
  geometry->ir_calib_a = new double[geometry->ir.poses_count];
  geometry->ir_calib_b = new double[geometry->ir.poses_count];
  for (unsigned int i = 0; i < geometry->ir.poses_count; ++i)
  {
    geometry->ir_calib_a[i] = cf->ReadTupleFloat(section,"ir_calib_a", i, KHEPERA_DEFAULT_IR_CALIB_A);
    geometry->ir_calib_b[i] = cf->ReadTupleFloat(section,"ir_calib_b", i, KHEPERA_DEFAULT_IR_CALIB_B);
  }
  geometry->ir.poses_count = geometry->ir.poses_count;

  // zero position counters
  last_lpos = 0;
  last_rpos = 0;
  last_x_f=0;
  last_y_f=0;
  last_theta = 0.0;
  
  
}

int 
Khepera::Subscribe(player_devaddr_t addr)
{
  int setupResult;

  // do the subscription
  if((setupResult = Driver::Subscribe(addr)) == 0)
  {
    // also increment the appropriate subscription counter
    switch(addr.interf)
    {
      case PLAYER_POSITION2D_CODE:
        this->position_subscriptions++;
        break;
      case PLAYER_IR_CODE:
        this->ir_subscriptions++;
        break;
    }
  }

  return(setupResult);
}

int 
Khepera::Unsubscribe(player_devaddr_t addr)
{
  int shutdownResult;

  // do the unsubscription
  if((shutdownResult = Driver::Unsubscribe(addr)) == 0)
  {
    // also decrement the appropriate subscription counter
    switch(addr.interf)
    {
      case PLAYER_POSITION2D_CODE:
        assert(--this->position_subscriptions >= 0);
        break;
      case PLAYER_IR_CODE:
        assert(--this->ir_subscriptions >= 0);
        break;
    }
  }

  return(shutdownResult);
}

/* called the first time a client connects
 *