erratic.cc

机器人仿真软件

// -*- mode:C++; tab-width:2; c-basic-offset:2; indent-tabs-mode:1; -*-

/**
  *  Copyright (C) 2006
  *     Videre Design
  *  Copyright (C) 2000  
  *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
  *
	*  Videre Erratic robot driver for Player
	*
	*  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
**/

/**
  *  This driver is adapted from the p2os driver of player 1.6.
**/

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_erratic erratic
 * @brief Erratic

This driver talks to the embedded computer in the Erratic robot, which
mediates communication to the devices of the robot.

@par Compile-time dependencies

- none

@par Provides

The erratic driver provides the following device interfaces, some of
them named:

- "odometry" @ref interface_position2d
  - This interface returns odometry data, and accepts velocity commands.

- @ref interface_power
  - Returns the current battery voltage (12 V when fully charged).

- @ref interface_aio
  - Returns data from analog input pins

- @ref interface_ir
  - Returns ranges from IR sensors, assuming they're connected to the analog input pins

@par Supported configuration requests

- "odometry" @ref interface_position2d :
  - PLAYER_POSITION_SET_ODOM_REQ
  - PLAYER_POSITION_MOTOR_POWER_REQ
  - PLAYER_POSITION_RESET_ODOM_REQ
  - PLAYER_POSITION_GET_GEOM_REQ
  - PLAYER_POSITION_VELOCITY_MODE_REQ
- @ref interface_ir :
  - PLAYER_SONAR_GET_GEOM_REQ

@par Configuration file options

- port (string)
  - Default: "/dev/ttyS0"
- direct_wheel_vel_control (integer)
  - Default: 0
  - Send direct wheel velocity commands to Erratic (as opposed to sending
    translational and rotational velocities and letting Erratic smoothly
    achieve them).
- max_trans_vel (length)
  - Default: 0.5 m/s
  - Maximum translational velocity
- max_rot_vel (angle)
  - Default: 100 deg/s
  - Maximum rotational velocity
- trans_acc (length)
  - Default: 0
  - Maximum translational acceleration, in length/sec/sec; nonnegative.
    Zero means use the robot's default value.
- trans_decel (length)
  - Default: trans_acc
  - Maximum translational deceleration, in length/sec/sec; nonpositive.
    Zero means use the robot's default value.
- rot_acc (angle)
  - Default: 0
  - Maximum rotational acceleration, in angle/sec/sec; nonnegative.
    Zero means use the robot's default value.
- rot_decel (angle)
  - Default: rot_acc
  - Maximum rotational deceleration, in angle/sec/sec; nonpositive.
    Zero means use the robot's default value.
- pid_trans_p (integer)
  - Default: -1
  - Translational PID setting; proportional gain.
    Negative means use the robot's default value.
- pid_trans_d (integer)
  - Default: -1
  - Translational PID setting; derivative gain.
    Negative means use the robot's default value.
- pid_rot_p (integer)
  - Default: -1
  - Rotational PID setting; proportional gain.
    Negative means use the robot's default value.
- pid_rot_d (integer)
  - Default: -1
  - Rotational PID setting; derivative gain.
    Negative means use the robot's default value.
- motor_pwm_frequency (integer)
  - Default: -1
  - Frequency of motor PWM.
    Bounds determined by robot.
    Negative means use the robot's default value.
- motor_pwm_max_on (float)
  - Default: 1
  - Maximum motor duty cycle.
- save_settings_in_robot (integer)
  - Default: 0
  - A value of 1 installs current settings as default values in the robot.


@par Example

@verbatim
driver
(
	name "erratic"
	plugin "erratic"

	provides [ "position2d:0"
	           "power:0"
	           "aio:0"
	           "ir:0" ]

	port "/dev/ttyUSB0"

	max_trans_vel 3
	max_rot_vel 720

	trans_acc 1
	rot_acc 200

	direct_wheel_vel_control 0
)
@endverbatim

@author Joakim Arfvidsson, Brian Gerkey, Kasper Stoy, James McKenna
*/
/** @} */



// This must be first per pthread reference
#include <pthread.h>

#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <termios.h>

#include "erratic.h"

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

bool debug_mode = FALSE;



/** Setting up, tearing down **/

// These get the driver in where it belongs, loading
Driver* Erratic_Init(ConfigFile* cf, int section) {
  return (Driver*)(new Erratic(cf,section));
}
void Erratic_Register(DriverTable* table) {
	table->AddDriver("erratic", Erratic_Init);
}
#if 0
extern "C" {
	int player_driver_init(DriverTable* table)
	{
		Erratic_Register(table);
		#define QUOTEME(x) #x
		//#define DATE "##VIDERE_DATE##\"
		printf("  Erratic robot driver %s, build date %s\n", ERRATIC_VERSION, ERRATIC_DATE);
		return 0;
	}
}
#endif

// Constructor of the driver from configuration entry
Erratic::Erratic(ConfigFile* cf, int section) : Driver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN) {
	// zero ids, so that we'll know later which interfaces were requested
	memset(&this->position_id, 0, sizeof(player_devaddr_t));
	memset(&this->power_id, 0, sizeof(player_devaddr_t));
	memset(&this->aio_id, 0, sizeof(player_devaddr_t));

	memset(&this->last_position_cmd, 0, sizeof(player_position2d_cmd_vel_t));
	memset(&this->last_car_cmd, 0, sizeof(player_position2d_cmd_car_t));

	this->position_subscriptions = 0;
	this->aio_ir_subscriptions = 0;

	// intialise members
	motor_packet = NULL;

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

	// Do we create a power interface?
	if(cf->ReadDeviceAddr(&(this->power_id), section, "provides", PLAYER_POWER_CODE, -1, NULL) == 0) {
		if(this->AddInterface(this->power_id) != 0) {
			this->SetError(-1);
			return;
		}
	}
	
	// Do we create a aio interface?
	if(cf->ReadDeviceAddr(&(this->aio_id), section, "provides", PLAYER_AIO_CODE, -1, NULL) == 0) {
		if(this->AddInterface(this->aio_id) != 0) {
			this->SetError(-1);
			return;
		}
	}

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

	// build the table of robot parameters.
	initialize_robot_params();

	// Read config file options
	this->psos_serial_port = cf->ReadString(section,"port",DEFAULT_VIDERE_PORT);
	this->direct_wheel_vel_control = cf->ReadInt(section, "direct_wheel_vel_control", 0);
	this->motor_max_speed = (int)rint(1e3 * cf->ReadLength(section, "max_trans_vel", MOTOR_DEF_MAX_SPEED));
	this->motor_max_turnspeed = (int)rint(RTOD(cf->ReadAngle(section, "max_rot_vel", MOTOR_DEF_MAX_TURNSPEED)));
	this->motor_max_trans_accel = (short)rint(1.0e3 * cf->ReadLength(section, "trans_acc", 0));
	this->motor_max_trans_decel = (short)rint(1.0e3 * cf->ReadLength(section, "trans_decel", 0));
	this->motor_max_rot_accel = (short)rint(RTOD(cf->ReadAngle(section, "rot_acc", 0)));
	this->motor_max_rot_decel = (short)rint(RTOD(cf->ReadAngle(section, "rot_decel", 0)));

	this->pid_trans_p = cf->ReadInt(section, "pid_trans_p", -1);
	this->pid_trans_v = cf->ReadInt(section, "pid_trans_v", -1);
	this->pid_trans_i = cf->ReadInt(section, "pid_trans_i", -1);
	this->pid_rot_p = cf->ReadInt(section, "pid_rot_p", -1);
	this->pid_rot_v = cf->ReadInt(section, "pid_rot_v", -1);
	this->pid_rot_i = cf->ReadInt(section, "pid_rot_i", -1);

	this->motor_pwm_frequency = cf->ReadInt(section, "motor_pwm_frequency", -1);
	this->motor_pwm_max_on = (uint16_t)(cf->ReadFloat(section, "motor_pwm_max_on", -1) * 1000.0);

	this->use_vel_band = 0;//cf->ReadInt(section, "use_vel_band", 0);

	this->print_all_packets = 0;
	this->print_status_summary = 1;

	debug_mode = cf->ReadInt(section, "debug", 0);
	save_settings_in_robot = cf->ReadInt(section, "save_settings_in_robot", 0);

	this->read_fd = -1;
	this->write_fd = -1;
	
	// Data mutexes and semaphores
	pthread_mutex_init(&send_queue_mutex, 0);
	pthread_cond_init(&send_queue_cond, 0);
	pthread_mutex_init(&motor_packet_mutex, 0);
	
	if (Connect()) {
		printf("Error connecting to Erratic robot\n");
		exit(1);
	}
}

// Called by player when the driver is asked to connect
int Erratic::Setup() {
	// We don't care, we connect at startup anyway
	return 0;
}

// Establishes connection and starts threads
int Erratic::Connect() {
	printf("  Erratic connection initializing (%s)...",this->psos_serial_port); fflush(stdout);

	//Try opening both our file descriptors
	if ((this->read_fd = open(this->psos_serial_port, O_RDONLY, S_IRUSR | S_IWUSR)) < 0) {
		perror("Erratic::Setup():open():");
		return(1);
	};
	if ((this->write_fd = open(this->psos_serial_port, O_WRONLY, S_IRUSR | S_IWUSR)) < 0) {
		perror("Erratic::Setup():open():");
		close(this->read_fd); this->read_fd = -1;
		return(1);
	}

	// Populate term and set initial baud rate
	int bauds[] = {B38400, B115200};
	int numbauds = sizeof(bauds), currbaud = 0;
	struct termios read_term, write_term;
	{
		if(tcgetattr( this->read_fd, &read_term ) < 0 ) {
			perror("Erratic::Setup():tcgetattr():");
			close(this->read_fd); close(this->write_fd);
			this->read_fd = -1; this->write_fd = -1;
			return(1);
		} 
		
		cfmakeraw(&read_term);
		//cfsetspeed(&read_term, bauds[currbaud]);
		cfsetispeed(&read_term, bauds[currbaud]);
		cfsetospeed(&read_term, bauds[currbaud]);
			
		if(tcsetattr(this->read_fd, TCSAFLUSH, &read_term ) < 0) {
			perror("Erratic::Setup():tcsetattr(read channel):");
			close(this->read_fd); close(this->write_fd);
			this->read_fd = -1; this->write_fd = -1;
			return(1);
		}
	}
	
	
	// Empty buffers 
	if (tcflush(this->read_fd, TCIOFLUSH ) < 0) {
		perror("Erratic::Setup():tcflush():");
		close(this->read_fd); close(this->write_fd);
		this->read_fd = -1; this->write_fd = -1;
		return(1);
	}
	if (tcflush(this->write_fd, TCIOFLUSH ) < 0) {
		perror("Erratic::Setup():tcflush():");
		close(this->read_fd); close(this->write_fd);
		this->read_fd = -1; this->write_fd = -1;
		return(1);
	}
	
	// will send config packets until a response is gotten
	int num_sync_attempts = 10;
	int num_patience = 200;
	int failed = 0;
	enum { NO_SYNC, AFTER_FIRST_SYNC, AFTER_SECOND_SYNC, READY } communication_state = NO_SYNC;
	ErraticPacket received_packet;
	while(communication_state != READY && num_patience-- > 0) {
		// Send a configuration request
		if (num_patience % 5 == 0) {
			ErraticPacket packet;
			unsigned char command = (command_e)configuration;
			packet.Build(&command, 1);
			packet.Send(this->write_fd);
		}

		// Receive a reply to see if we got it yet
		uint8_t receive_error;
		if((receive_error = received_packet.Receive(this->read_fd))) {
			if (receive_error == (receive_result_e)failure)
				printf("Error receiving\n");
			// If we still have retries, just get another packet
			if((communication_state == NO_SYNC) && (num_sync_attempts >= 0)) {
				num_sync_attempts--;
				usleep(ROBOT_CYCLETIME);
				continue;
			}
			// Otherwise, try next speed or give up completely
			else {
			// Couldn't connect; try different speed.
				if(++currbaud < numbauds) {
				// Set speeds for the descriptors
					cfsetispeed(&read_term, bauds[currbaud]);
					cfsetospeed(&write_term, bauds[currbaud]);
					if( tcsetattr(this->read_fd, TCSAFLUSH, &read_term ) < 0 ) {
						perror("Erratic::Setup():tcsetattr():");
						close(this->read_fd); close(this->write_fd);
						this->read_fd = -1; this->write_fd = -1;
						return(1);
					}
					if( tcsetattr(this->write_fd, TCSAFLUSH, &write_term ) < 0 ) {
						perror("Erratic::Setup():tcsetattr():");
						close(this->read_fd); close(this->write_fd);
						this->read_fd = -1; this->write_fd = -1;
						return(1);
					}

					// Flush the descriptors
					if(tcflush(this->read_fd, TCIOFLUSH ) < 0 ) {
						perror("Erratic::Setup():tcflush():");
						close(this->read_fd); close(this->write_fd);
						this->read_fd = -1; this->write_fd = -1;
						return(1);
					}
					if(tcflush(this->write_fd, TCIOFLUSH ) < 0 ) {
						perror("Erratic::Setup():tcflush():");
						close(this->read_fd); close(this->write_fd);
						this->read_fd = -1; this->write_fd = -1;
						return(1);
					}

					// Give same slack to new speed
					num_sync_attempts = 10;
					continue;
				}
				// Tried all speeds, abort
				else failed = 1;
			}
		}

		// If we gave up
		if (failed) break;

		// If we successfully got a packet, check if it's the one we're waiting for
		if (received_packet.packet[3] == 0x20) communication_state = READY;

		usleep(ROBOT_CYCLETIME);
	}

	if(failed) {
		printf("  Couldn't synchronize with Erratic robot.\n");
		printf("  Most likely because the robot is not connected to %s\n", this->psos_serial_port);
		close(this->read_fd); close(this->write_fd);
		this->read_fd = -1; this->write_fd = -1;
		return(1);
	}
	else if (communication_state != READY) {
		printf("  Couldn't synchronize with Erratic robot.\n");
		printf("  We heard something though. Is the sending part of the cable dead?\n");
		close(this->read_fd); close(this->write_fd);
		this->read_fd = -1; this->write_fd = -1;
		return(1);
	}

	char name[20], type[20], subtype[20];
	sprintf(name, "%s", &received_packet.packet[5]);
	sprintf(type, "%s", &received_packet.packet[25]);
	sprintf(subtype, "%s", &received_packet.packet[45]);

	// Open the driver, and tickle it a bit
	{
		ErraticPacket packet;

		unsigned char command = (command_e)open_controller;
		packet.Build(&command, 1);
		packet.Send(this->write_fd);
		usleep(ROBOT_CYCLETIME);

		command = (command_e)pulse;
		packet.Build(&command, 1);
		packet.Send(this->write_fd);
		usleep(ROBOT_CYCLETIME);
	}

	printf(" done.\n  Connected to \"%s\", a %s %s\n", name, type, subtype);

	// Set the robot type statically for now (there is only one!)
	param_idx = 0;

	// Create a packet and set initial odometry position (the ErraticMotorPacket is persistent)