Commit 12ea0901 authored by Timothy Stack's avatar Timothy Stack

Merge changes from the branch

parent 7d7111fe
......@@ -18,7 +18,7 @@ PROGS = garcia-pilot
endif
all client: $(PROGS)
install:
install control-install:
client-install: client
$(INSTALL) -m 755 -d $(DESTDIR)$(CLIENT_BINDIR)
$(INSTALL) -m 755 -d $(DESTDIR)$(CLIENT_ETCDIR)
......
......@@ -68,7 +68,7 @@ dashboard::~dashboard()
{
}
void dashboard::startMove(acpGarcia &garcia)
void dashboard::startMove()
{
assert(this->db_move_start_time.tv_sec == 0);
assert(this->db_move_start_time.tv_usec == 0);
......@@ -78,7 +78,7 @@ void dashboard::startMove(acpGarcia &garcia)
gettimeofday(&this->db_move_start_time, NULL);
}
void dashboard::endMove(acpGarcia &garcia)
void dashboard::endMove(float &left_odometer, float &right_odometer)
{
struct timeval tv, diff, total, grande_total;
acpValue zvalue(0.0f);
......@@ -96,10 +96,12 @@ void dashboard::endMove(acpGarcia &garcia)
this->db_telemetry.move_time_usec = grande_total.tv_usec;
/* ... update our global odometers, and */
this->db_telemetry.left_odometer +=
fabs(this->db_garcia.getNamedValue("distance-left")->getFloatVal());
this->db_telemetry.right_odometer +=
fabs(this->db_garcia.getNamedValue("distance-right")->getFloatVal());
left_odometer = this->db_garcia.getNamedValue("distance-left")->
getFloatVal();
right_odometer = this->db_garcia.getNamedValue("distance-right")->
getFloatVal();
this->db_telemetry.left_odometer += fabs(left_odometer);
this->db_telemetry.right_odometer += fabs(right_odometer);
/* ... clear the garcia's per-move odometers. */
this->db_garcia.setNamedValue("distance-left", &zvalue);
......
......@@ -137,12 +137,12 @@ public:
/**
* Signal the start of a movement.
*/
void startMove(acpGarcia &garcia);
void startMove();
/**
* Signal the end of a movement and update the odometers accordingly.
*/
void endMove(acpGarcia &garcia);
void endMove(float &left_odometer, float &right_odometer);
/**
* @return True if the battery level is below the threshold.
......
......@@ -56,9 +56,6 @@ static const char *BATTERY_LOG_PATH = "/var/log/battery.log";
*/
static volatile int looping = 1;
extern char **environ;
static char **reexec_argv;
/**
* Signal handler for SIGINT, SIGTERM, and SIGQUIT signals. Sets looping to
* zero and aborts() if it is called more than three times in case the program
......@@ -137,21 +134,15 @@ static void usage(void)
int main(int argc, char *argv[])
{
int lpc, c, port = PILOT_PORT, serv_sock, on_off = 1;
const char *logfile = DEFAULT_LOG_PATH, *pidfile = NULL;
int c, port = PILOT_PORT, serv_sock, on_off = 1;
const char *logfile = NULL, *pidfile = NULL;
const char *batteryfile = BATTERY_LOG_PATH;
int retval = EXIT_SUCCESS;
std::list<int> fd_list;
unsigned long now;
FILE *batterylog;
aIOLib ioRef;
sigset_t ss;
aErr err;
reexec_argv = argv;
sigfillset(&ss);
sigprocmask(SIG_UNBLOCK, &ss, NULL);
while ((c = getopt(argc, argv, "hdp:l:i:b:")) != -1) {
switch (c) {
case 'h':
......@@ -217,14 +208,7 @@ int main(int argc, char *argv[])
fprintf(stderr,
"error: cannot open battery log file - %s\n",
batteryfile);
}
for (lpc = 3; lpc < 32; lpc++) {
struct stat st;
if (fstat(lpc, &st) == 0) {
fd_list.push_back(lpc);
}
exit(1);
}
acpGarcia garcia;
......@@ -290,6 +274,7 @@ int main(int argc, char *argv[])
perror("listen");
}
else {
int rmc_telemetry_timeout = 60;
pilotClient::list clients;
fd_set readfds, writefds;
......@@ -301,49 +286,6 @@ int main(int argc, char *argv[])
FD_ZERO(&writefds);
FD_SET(serv_sock, &readfds);
for (lpc = 3; lpc < 32; lpc++) {
struct stat st;
if ((fstat(lpc, &st) == 0) &&
(std::find(fd_list.begin(),
fd_list.end(),
lpc) == fd_list.end())) {
fcntl(lpc, F_SETFD, 1);
}
}
for (lpc = 3; lpc < 128; lpc++) {
struct sockaddr_in sin;
socklen_t sin_len;
sin_len = sizeof(sin);
if ((lpc != serv_sock) &&
(getpeername(lpc,
(struct sockaddr *)&sin,
&sin_len) == 0)) {
struct mtp_packet ump;
if (debug) {
printf("recovered client: %s:%d\n",
inet_ntoa(sin.sin_addr),
ntohs(sin.sin_port));
}
pilotClient *pc = new pilotClient(lpc, wm, db);
pc->setFD(&readfds);
pc->setFD(&writefds);
clients.push_back(pc);
mtp_init_packet(&ump,
MA_Opcode, MTP_UPDATE_POSITION,
MA_Role, MTP_ROLE_ROBOT,
MA_Status, MTP_POSITION_STATUS_IDLE,
MA_CommandID, 0,
MA_TAG_DONE);
mtp_send_packet(pc->getHandle(), &ump);
}
}
wm.setDashboard(&db);
do {
fd_set rreadyfds = readfds, wreadyfds = writefds;
......@@ -394,6 +336,21 @@ int main(int argc, char *argv[])
MA_Role, MTP_ROLE_RMC,
MA_GarciaTelemetry, db.getTelemetry(),
MA_TAG_DONE);
if (pilotClient::pc_rmc_client != NULL) {
rmc_telemetry_timeout -= 1;
if (rmc_telemetry_timeout <= 0) {
pilotClient *pc = pilotClient::pc_rmc_client;
rmc_telemetry_timeout = 60;
if (mtp_send_packet(pc->getHandle(), &tmp) !=
MTP_PP_SUCCESS) {
fprintf(stderr,
"error: cannot send telemetry to "
"rmcd\n");
}
}
}
}
for (i = clients.begin(); i != clients.end(); ) {
......@@ -416,7 +373,8 @@ int main(int argc, char *argv[])
} while (pc && pc->getHandle()->mh_remaining);
}
if ((pc->getRole() == MTP_ROLE_EMULAB) &&
if (!bad_client &&
(pc->getRole() == MTP_ROLE_EMULAB) &&
pc->isFDSet(&wreadyfds)) {
if (do_telem &&
mtp_send_packet(pc->getHandle(), &tmp) !=
......
......@@ -14,10 +14,9 @@
#include <math.h>
#include "garciaUtil.hh"
#include "pilotClient.hh"
extern int debug;
/**
* Callback passed to the wheelManager when performing movements.
*/
......@@ -26,7 +25,7 @@ class pilotMoveCallback : public wmCallback
public:
pilotMoveCallback(pilotClient::list &pcl, int command_id);
pilotMoveCallback(pilotClient::list &pcl, int command_id, float theta);
/**
* Callback that will send an MTP_UPDATE_POSITION packet back to the client
......@@ -34,28 +33,35 @@ public:
*
* @param status The completion status of the move.
*/
virtual void call(int status);
virtual void call(int status, float odometer);
private:
pilotClient::list &notify_list;
pilotClient::list &pmc_notify_list;
int command_id;
int pmc_command_id;
float pmc_theta;
};
pilotMoveCallback::pilotMoveCallback(pilotClient::list &pcl, int command_id)
: notify_list(pcl), command_id(command_id)
pilotMoveCallback::pilotMoveCallback(pilotClient::list &pcl,
int command_id,
float theta)
: pmc_notify_list(pcl),
pmc_command_id(command_id),
pmc_theta(theta)
{
}
void pilotMoveCallback::call(int status)
void pilotMoveCallback::call(int status, float odometer)
{
pilotClient::iterator i;
mtp_packet_t mp;
mtp_status_t ms;
fprintf(stderr, "callback: %d\n", status);
if (debug) {
fprintf(stderr, "debug: callback status -- %d\n", status);
}
switch (status) {
case aGARCIA_ERRFLAG_NORMAL:
......@@ -84,7 +90,10 @@ void pilotMoveCallback::call(int status)
MA_Opcode, MTP_UPDATE_POSITION,
MA_Role, MTP_ROLE_ROBOT,
MA_Status, ms,
MA_CommandID, this->command_id,
MA_X, cosf(this->pmc_theta) * odometer,
MA_Y, sinf(this->pmc_theta) * odometer,
MA_Theta, this->pmc_theta,
MA_CommandID, this->pmc_command_id,
MA_TAG_DONE);
if (debug > 1) {
......@@ -92,7 +101,9 @@ void pilotMoveCallback::call(int status)
mtp_print_packet(stderr, &mp);
}
for (i = this->notify_list.begin(); i != this->notify_list.end(); i++) {
for (i = this->pmc_notify_list.begin();
i != this->pmc_notify_list.end();
i++) {
pilotClient *pc = *i;
mtp_send_packet(pc->getHandle(), &mp);
......@@ -193,6 +204,7 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
}
if ((mcg->position.x != 0.0) || (mcg->position.y != 0.0)) {
float theta;
if (debug) {
fprintf(stderr,
......@@ -201,11 +213,14 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
mcg->position.y);
}
/* XXX Need to figure out the real theta and send that back. */
theta = atan2f(mcg->position.y, mcg->position.x);
this->pc_wheel_manager.
setDestination(mcg->position.x,
mcg->position.y,
new pilotMoveCallback(notify_list,
mcg->command_id));
mcg->command_id,
theta));
}
else {
if (debug) {
......@@ -217,7 +232,8 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
this->pc_wheel_manager.setOrientation(
mcg->position.theta,
new pilotMoveCallback(notify_list,
mcg->command_id));
mcg->command_id,
mcg->position.theta));
}
retval = true;
......@@ -228,6 +244,7 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
if (this->pc_role == MTP_ROLE_RMC) {
struct mtp_packet cmp, rmp;
pilotClient::iterator i;
bool send_idle = false;
mcs = &mp->data.mtp_payload_u.command_stop;
......@@ -235,13 +252,16 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
fprintf(stderr, "debug: full stop\n");
}
this->pc_wheel_manager.stop();
mtp_init_packet(&rmp,
MA_Opcode, MTP_UPDATE_POSITION,
MA_Role, MTP_ROLE_ROBOT,
MA_Status, MTP_POSITION_STATUS_IDLE,
MA_CommandID, mcs->command_id,
MA_TAG_DONE);
if (!this->pc_wheel_manager.stop()) {
mtp_init_packet(&rmp,
MA_Opcode, MTP_UPDATE_POSITION,
MA_Role, MTP_ROLE_ROBOT,
MA_Status, MTP_POSITION_STATUS_IDLE,
MA_CommandID, mcs->command_id,
MA_TAG_DONE);
send_idle = true;
}
cmp = *mp;
cmp.role = MTP_ROLE_ROBOT;
......@@ -250,7 +270,8 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
if (pc->getRole() == MTP_ROLE_EMULAB)
mtp_send_packet(pc->getHandle(), &cmp);
mtp_send_packet(pc->getHandle(), &rmp);
if (send_idle)
mtp_send_packet(pc->getHandle(), &rmp);
}
retval = true;
......@@ -261,6 +282,7 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
{
struct mtp_garcia_telemetry *mgt;
struct contact_point points[8];
pilotClient::iterator i;
struct mtp_packet cmp;
int count = 0;
......@@ -335,8 +357,13 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
MA_ContactPointCount, count,
MA_ContactPoints, points,
MA_TAG_DONE);
mtp_send_packet(this->getHandle(), &cmp);
for (i = notify_list.begin(); i != notify_list.end(); i++) {
pilotClient *pc = *i;
mtp_send_packet(pc->getHandle(), &cmp);
}
retval = true;
}
break;
......
......@@ -250,7 +250,7 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback)
* Check if we can make the move by backing up instead of turning all the
* way around and moving forward.
*/
if ((distance <= 0.25) && (fabsf(angle) > M_PI_2)) {
if ((distance <= 0.25f) && fabsf(angle) > M_PI_2) {
if (angle >= 0.0)
angle -= M_PI;
else
......@@ -279,7 +279,7 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback)
if ((move = this->createMove(distance, callback)) == NULL) {
/* Skipping everything. */
if (callback != NULL) {
callback->call(aGARCIA_ERRFLAG_WONTEXECUTE);
callback->call(aGARCIA_ERRFLAG_WONTEXECUTE, 0);
delete callback;
callback = NULL;
......@@ -305,7 +305,7 @@ void wheelManager::setOrientation(float orientation, wmCallback *callback)
if ((pivot = this->createPivot(orientation, callback)) == NULL) {
if (callback != NULL) {
callback->call(aGARCIA_ERRFLAG_WONTEXECUTE);
callback->call(aGARCIA_ERRFLAG_WONTEXECUTE, 0);
delete callback;
callback = NULL;
......@@ -317,9 +317,11 @@ void wheelManager::setOrientation(float orientation, wmCallback *callback)
}
}
void wheelManager::stop(void)
bool wheelManager::stop(void)
{
this->wm_garcia.flushQueuedBehaviors();
return this->wm_moving;
}
void wheelManager::motionStarted(void)
......@@ -327,7 +329,9 @@ void wheelManager::motionStarted(void)
if (debug) {
fprintf(stderr, "debug: motion started\n");
}
this->wm_moving = true;
if ((this->wm_last_status != aGARCIA_ERRFLAG_NORMAL) &&
(this->wm_last_status != aGARCIA_ERRFLAG_ABORT)) {
if (debug) {
......@@ -339,13 +343,15 @@ void wheelManager::motionStarted(void)
}
this->wm_dashboard->addUserLEDClient(&this->wm_moving_notice);
this->wm_dashboard->startMove(this->wm_garcia);
this->wm_dashboard->startMove();
}
void wheelManager::motionFinished(acpObject *behavior,
int status,
wmCallback *callback)
{
float odometer = 0.0f;
if (debug) {
fprintf(stderr, "debug: motion finished -- %d\n", status);
}
......@@ -361,6 +367,8 @@ void wheelManager::motionFinished(acpObject *behavior,
}
if (status != aGARCIA_ERRFLAG_WONTEXECUTE) {
float left_odometer, right_odometer;
if ((status != aGARCIA_ERRFLAG_NORMAL) &&
(status != aGARCIA_ERRFLAG_ABORT)) {
if (debug) {
......@@ -370,14 +378,25 @@ void wheelManager::motionFinished(acpObject *behavior,
this->wm_dashboard->addUserLEDClient(&this->wm_error_notice);
}
this->wm_dashboard->endMove(this->wm_garcia);
this->wm_dashboard->endMove(left_odometer, right_odometer);
if ((left_odometer / fabsf(left_odometer)) ==
(right_odometer / fabsf(right_odometer))) {
printf(" %f %f -- %f %f\n",
left_odometer, right_odometer,
left_odometer / left_odometer,
right_odometer / right_odometer);
odometer = left_odometer;
}
this->wm_dashboard->remUserLEDClient(&this->wm_moving_notice);
this->wm_last_status = status;
}
if (callback != NULL) {
callback->call(this->wm_last_status);
this->wm_moving = false; // XXX This assumes callbacks on the last move
callback->call(this->wm_last_status, odometer);
delete callback;
callback = NULL;
......
......@@ -42,7 +42,7 @@ public:
* @param status The completion status of the move. (One of the
* aGARCIA_ERRFLAG values.)
*/
virtual void call(int status) = 0;
virtual void call(int status, float odometer) = 0;
};
......@@ -145,7 +145,7 @@ public:
/**
* Stop the robot in its tracks and flush any queued behaviors.
*/
virtual void stop(void);
virtual bool stop(void);
/**
* Internal callback method used to update the dashboard when a pivot/move
......@@ -187,6 +187,8 @@ private:
*/
int wm_last_status;
bool wm_moving;
/**
* Pointer to the dashboard that should be updated when doing pivots/moves.
*/
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment