Commit 74f61a9f authored by Timothy Stack's avatar Timothy Stack

More robot stuff, start in on path planning and cleanup rmcd a bit.

	* robots/emc/emcd.c: Update for obstacle-related changes in mtp.

	* robots/mtp/mtp.x, robots/mtp/mtp.h, robots/mtp/mtp.c,
	robots/mtp/mtp_send.c: Add messages for requesting/reporting robot
	sensor contacts, plus some cosmetic changes to the obstacle_config
	structure.

	* robots/primotion/garcia-pilot.cc,
	robots/primotion/pilotClient.hh, robots/primotion/pilotClient.cc:
	Add support for sending "contact" reports, which contain points
	where the sensors have detected an object.

	* robots/rmcd/GNUmakefile.in, robots/rmcd/obstacles.h,
	robots/rmcd/obstacles.c, robots/rmcd/pilotConnection.h,
	robots/rmcd/pilotConnection.c, robots/rmcd/rclip.h,
	robots/rmcd/rclip.c, robots/rmcd/rmcd.c: Cleanup and add in some
	obstacle avoidance.
parent d3910751
......@@ -573,10 +573,10 @@ void parse_config_file(char *config_file) {
}
else {
oc[oc_size].id = id;
oc[oc_size].x1 = x1;
oc[oc_size].y1 = y1;
oc[oc_size].x2 = x2;
oc[oc_size].y2 = y2;
oc[oc_size].xmin = x1;
oc[oc_size].ymin = y1;
oc[oc_size].xmax = x2;
oc[oc_size].ymax = y2;
oc_size += 1;
}
}
......@@ -726,7 +726,8 @@ void ev_callback(event_handle_t handle,
struct mtp_packet mp;
event_notification_get_arguments(handle, notification, args, sizeof(args));
/* XXX copy the current X, Y, and orientation! */
if (event_arg_get(args, "X", &value) > 0) {
if (sscanf(value, "%f", &x) != 1) {
error("X argument in event is not a float: %s\n", value);
......@@ -1299,7 +1300,7 @@ int vmc_callback(elvin_io_handler_t handler,
struct mtp_update_position *up_copy, *up_in;
up_in = &mp->data.mtp_payload_u.update_position;
if ((up->status == MTP_POSITION_STATUS_ERROR) &&
if (up && (up->status == MTP_POSITION_STATUS_ERROR) &&
(up_in->status != MTP_POSITION_STATUS_ERROR)) {
struct mtp_packet mp_reply;
......
......@@ -314,6 +314,9 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->vers = MTP_VERSION;
while (tag != MA_TAG_DONE) {
struct contact_point *cp;
int lpc;
assert(tag >= MA_TAG_DONE);
assert(tag < MA_TAG_MAX);
......@@ -399,6 +402,10 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.wiggle_status.robot_id =
va_arg(args, int);
break;
case MTP_REQUEST_REPORT:
mp->data.mtp_payload_u.request_report.robot_id =
va_arg(args, int);
break;
default:
assert(0);
break;
......@@ -543,6 +550,17 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.wiggle_request.wiggle_type =
va_arg(args, mtp_wiggle_t);
break;
case MA_ContactPointCount:
mp->data.mtp_payload_u.contact_report.count = va_arg(args, int);
break;
case MA_ContactPoints:
cp = va_arg(args, struct contact_point *);
for (lpc = 0;
lpc < mp->data.mtp_payload_u.contact_report.count;
lpc++) {
mp->data.mtp_payload_u.contact_report.points[lpc] = cp[lpc];
}
break;
}
tag = va_arg(args, mtp_tag_t);
......@@ -581,6 +599,7 @@ float mtp_theta(float theta)
void mtp_print_packet(FILE *file, struct mtp_packet *mp)
{
struct mtp_garcia_telemetry *mgt;
struct mtp_contact_report *mcr;
int lpc;
assert(mp != NULL);
......@@ -680,10 +699,10 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
" obstacle[%d]:\t%d %f/%f x %f/%f\n",
lpc,
mcr->obstacles.obstacles_val[lpc].id,
mcr->obstacles.obstacles_val[lpc].x1,
mcr->obstacles.obstacles_val[lpc].y1,
mcr->obstacles.obstacles_val[lpc].x2,
mcr->obstacles.obstacles_val[lpc].y2);
mcr->obstacles.obstacles_val[lpc].xmin,
mcr->obstacles.obstacles_val[lpc].ymin,
mcr->obstacles.obstacles_val[lpc].xmax,
mcr->obstacles.obstacles_val[lpc].ymax);
}
break;
......@@ -717,13 +736,15 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
" y:\t\t%f\n"
" theta:\t%f\n"
" status:\t%d\n"
" timestamp:\t%f\n",
" timestamp:\t%f\n"
" command_id:\t%d\n",
mp->data.mtp_payload_u.update_position.robot_id,
mp->data.mtp_payload_u.update_position.position.x,
mp->data.mtp_payload_u.update_position.position.y,
mp->data.mtp_payload_u.update_position.position.theta,
mp->data.mtp_payload_u.update_position.status,
mp->data.mtp_payload_u.update_position.position.timestamp);
mp->data.mtp_payload_u.update_position.position.timestamp,
mp->data.mtp_payload_u.update_position.command_id);
break;
case MTP_UPDATE_ID:
......@@ -824,54 +845,76 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
break;
}
break;
case MTP_WIGGLE_REQUEST:
fprintf(file,
" opcode:\twiggle-request\n"
" id:\t%d\n",
mp->data.mtp_payload_u.wiggle_request.robot_id
);
switch (mp->data.mtp_payload_u.wiggle_request.wiggle_type) {
case MTP_WIGGLE_START:
fprintf(file,
" wiggle_type:\tstart\n"
);
break;
case MTP_WIGGLE_180_R:
fprintf(file,
" wiggle_type:\t180deg right\n"
);
break;
case MTP_WIGGLE_180_R_L:
fprintf(file,
" wiggle_type:\t180deg right, 180deg left\n"
);
break;
case MTP_WIGGLE_360_R:
fprintf(file,
" wiggle_type:\t360deg right\n"
);
break;
case MTP_WIGGLE_360_R_L:
fprintf(file,
" wiggle_type:\t360deg right, 360deg left\n"
);
break;
default:
assert(0);
break;
}
break;
fprintf(file,
" opcode:\twiggle-request\n"
" id:\t%d\n",
mp->data.mtp_payload_u.wiggle_request.robot_id
);
switch (mp->data.mtp_payload_u.wiggle_request.wiggle_type) {
case MTP_WIGGLE_START:
fprintf(file,
" wiggle_type:\tstart\n"
);
break;
case MTP_WIGGLE_180_R:
fprintf(file,
" wiggle_type:\t180deg right\n"
);
break;
case MTP_WIGGLE_180_R_L:
fprintf(file,
" wiggle_type:\t180deg right, 180deg left\n"
);
break;
case MTP_WIGGLE_360_R:
fprintf(file,
" wiggle_type:\t360deg right\n"
);
break;
case MTP_WIGGLE_360_R_L:
fprintf(file,
" wiggle_type:\t360deg right, 360deg left\n"
);
break;
default:
assert(0);
break;
}
break;
case MTP_WIGGLE_STATUS:
fprintf(file,
" opcode:\twiggle-status\n"
" id:\t%d\n"
" status:\t%d\n",
mp->data.mtp_payload_u.wiggle_status.robot_id,
mp->data.mtp_payload_u.wiggle_status.status
);
break;
fprintf(file,
" opcode:\twiggle-status\n"
" id:\t%d\n"
" status:\t%d\n",
mp->data.mtp_payload_u.wiggle_status.robot_id,
mp->data.mtp_payload_u.wiggle_status.status
);
break;
case MTP_REQUEST_REPORT:
fprintf(file,
" opcode:\trequest-report\n"
" id:\t%d\n",
mp->data.mtp_payload_u.request_report.robot_id);
break;
case MTP_CONTACT_REPORT:
mcr = &mp->data.mtp_payload_u.contact_report;
fprintf(file,
" opcode:\tcontact-report\n"
" count:\t%d\n",
mcr->count);
for (lpc = 0; lpc < mcr->count; lpc++) {
fprintf(file,
" contact[%d]:\t%f %f\n",
lpc,
mcr->points[lpc].x,
mcr->points[lpc].y);
}
break;
default:
printf("%p %d\n", &mp->data.opcode, mp->data.opcode);
fprintf(stderr, "error: unknown opcode %d\n", mp->data.opcode);
......
......@@ -146,7 +146,9 @@ typedef enum {
MA_RequestID, /*< (int) */
MA_CommandID, /*< (int) */
MA_GarciaTelemetry, /*< (mtp_garcia_telemetry *) */
MA_WiggleType, /*< (mtp_wiggle_t) */
MA_WiggleType, /*< (mtp_wiggle_t) */
MA_ContactPointCount, /*< (int) */
MA_ContactPoints, /*< (contact_point *) */
MA_TAG_MAX
} mtp_tag_t;
......
......@@ -114,6 +114,10 @@ enum mtp_opcode_t {
* the result of a wiggle.
*/
MTP_WIGGLE_STATUS = 71,
MTP_REQUEST_REPORT = 80,
MTP_CONTACT_REPORT = 81,
MTP_OPCODE_MAX
......@@ -145,7 +149,7 @@ enum mtp_status_t {
MTP_POSITION_STATUS_MOVING = 2,
MTP_POSITION_STATUS_ERROR = 3,
MTP_POSITION_STATUS_COMPLETE = 4,
MTP_POSITION_STATUS_OBSTRUCTED = 5,
MTP_POSITION_STATUS_CONTACT = 5,
MTP_POSITION_STATUS_ABORTED = 6,
MTP_POSITION_STATUS_CYCLE_COMPLETE = 32
};
......@@ -172,12 +176,12 @@ struct camera_config {
struct obstacle_config {
int id;
float x1;
float y1;
float z1;
float x2;
float y2;
float z2;
float xmin;
float ymin;
float zmin;
float xmax;
float ymax;
float zmax;
};
struct global_bound {
......@@ -295,6 +299,16 @@ struct mtp_wiggle_status {
mtp_status_t status;
};
struct contact_point {
float x;
float y;
};
struct mtp_contact_report {
int count;
contact_point points[8];
};
union mtp_telemetry switch (mtp_robot_type_t type) {
case MTP_ROBOT_GARCIA: mtp_garcia_telemetry garcia;
};
......@@ -315,6 +329,8 @@ union mtp_payload switch (mtp_opcode_t opcode) {
case MTP_TELEMETRY: mtp_telemetry telemetry;
case MTP_WIGGLE_REQUEST: mtp_wiggle_request wiggle_request;
case MTP_WIGGLE_STATUS: mtp_wiggle_status wiggle_status;
case MTP_REQUEST_REPORT: mtp_request_position request_report;
case MTP_CONTACT_REPORT: mtp_contact_report contact_report;
};
/**
......
......@@ -380,9 +380,11 @@ static int interpret_options(int *argcp, char ***argvp)
else if (strcasecmp(argv[0], "command-stop") == 0)
opcode = MTP_COMMAND_STOP;
else if (strcasecmp(argv[0], "wiggle-request") == 0)
opcode = MTP_WIGGLE_REQUEST;
opcode = MTP_WIGGLE_REQUEST;
else if (strcasecmp(argv[0], "wiggle-status") == 0)
opcode = MTP_WIGGLE_STATUS;
opcode = MTP_WIGGLE_STATUS;
else if (strcasecmp(argv[0], "request-report") == 0)
opcode = MTP_REQUEST_REPORT;
else {
fprintf(stderr, "error: unknown command: %s\n", argv[0]);
usage();
......@@ -550,37 +552,45 @@ static int interpret_options(int *argcp, char ***argvp)
MA_RobotID, args.id,
MA_TAG_DONE);
break;
case MTP_WIGGLE_REQUEST:
if (args.wiggle_type == -1)
if (args.wiggle_type == -1)
required_option("W");
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_WiggleType, args.wiggle_type,
MA_TAG_DONE
);
break;
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_WiggleType, args.wiggle_type,
MA_TAG_DONE
);
break;
case MTP_WIGGLE_STATUS:
if (args.status == -1)
required_option("s");
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_Status, args.status,
MA_TAG_DONE
);
break;
if (args.status == -1)
required_option("s");
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_Status, args.status,
MA_TAG_DONE
);
break;
case MTP_REQUEST_REPORT:
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_TAG_DONE);
break;
default:
fprintf(stderr,
"internal error: command %s not supported at the moment...\n",
......
......@@ -301,7 +301,7 @@ int main(int argc, char *argv[])
perror("accept");
}
else {
pilotClient *pc = new pilotClient(cfd, wm);
pilotClient *pc = new pilotClient(cfd, wm, db);
if (debug) {
fprintf(stderr,
......
......@@ -12,6 +12,8 @@
#include "config.h"
#include <math.h>
#include "pilotClient.hh"
/**
......@@ -59,6 +61,15 @@ void pilotMoveCallback::call(int status)
case aGARCIA_ERRFLAG_WONTEXECUTE:
ms = MTP_POSITION_STATUS_ABORTED;
break;
case aGARCIA_ERRFLAG_STALL:
case aGARCIA_ERRFLAG_FRONTR_LEFT:
case aGARCIA_ERRFLAG_FRONTR_RIGHT:
case aGARCIA_ERRFLAG_REARR_LEFT:
case aGARCIA_ERRFLAG_REARR_RIGHT:
case aGARCIA_ERRFLAG_SIDER_LEFT:
case aGARCIA_ERRFLAG_SIDER_RIGHT:
ms = MTP_POSITION_STATUS_CONTACT;
break;
default:
fprintf(stderr, "error: couldn't complete move %d\n", status);
ms = MTP_POSITION_STATUS_ERROR;
......@@ -81,8 +92,8 @@ void pilotMoveCallback::call(int status)
pilotClient *pilotClient::pc_rmc_client = NULL;
pilotClient::pilotClient(int fd, wheelManager &wm)
: pc_wheel_manager(wm)
pilotClient::pilotClient(int fd, wheelManager &wm, dashboard &db)
: pc_wheel_manager(wm), pc_dashboard(db)
{
assert(fd > 0);
......@@ -237,6 +248,85 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
}
break;
case MTP_REQUEST_REPORT:
{
struct mtp_garcia_telemetry *mgt;
struct contact_point points[8];
struct mtp_packet cmp;
int count = 0;
mgt = this->pc_dashboard.getTelemetry();
if ((mgt->front_ranger_left != 0.0) &&
(mgt->front_ranger_right != 0.0)) {
float min_range;
if (mgt->front_ranger_left < mgt->front_ranger_right)
min_range = mgt->front_ranger_left;
else
min_range = mgt->front_ranger_right;
points[count].x = cos(0.0) * min_range;
points[count].y = sin(0.0) * min_range;
count += 1;
}
else if (mgt->front_ranger_left != 0.0) {
points[count].x = cos(0.40) * mgt->front_ranger_left;
points[count].y = sin(0.40) * mgt->front_ranger_left;
count += 1;
}
else if (mgt->front_ranger_right != 0.0) {
points[count].x = cos(-0.40) * mgt->front_ranger_right;
points[count].y = sin(-0.40) * mgt->front_ranger_right;
count += 1;
}
if (mgt->side_ranger_left != 0.0) {
points[count].x = cos(M_PI_2) * mgt->side_ranger_left;
points[count].y = sin(M_PI_2) * mgt->side_ranger_left;
count += 1;
}
if (mgt->side_ranger_right != 0.0) {
points[count].x = cos(-M_PI_2) * mgt->side_ranger_right;
points[count].y = sin(-M_PI_2) * mgt->side_ranger_right;
count += 1;
}
if ((mgt->rear_ranger_left != 0.0) &&
(mgt->rear_ranger_right != 0.0)) {
float min_range;
if (mgt->rear_ranger_left < mgt->rear_ranger_right)
min_range = mgt->rear_ranger_left;
else
min_range = mgt->rear_ranger_right;
points[count].x = cos(M_PI) * min_range;
points[count].y = sin(M_PI) * min_range;
count += 1;
}
else if (mgt->rear_ranger_left != 0.0) {
points[count].x = cos(M_PI - 0.40) * mgt->rear_ranger_left;
points[count].y = sin(M_PI - 0.40) * mgt->rear_ranger_left;
count += 1;
}
else if (mgt->rear_ranger_right != 0.0) {
points[count].x = cos(-M_PI + 0.40) * mgt->rear_ranger_right;
points[count].y = sin(-M_PI + 0.40) * mgt->rear_ranger_right;
count += 1;
}
mtp_init_packet(&cmp,
MA_Opcode, MTP_CONTACT_REPORT,
MA_Role, MTP_ROLE_ROBOT,
MA_ContactPointCount, count,
MA_ContactPoints, points,
MA_TAG_DONE);
mtp_send_packet(this->getHandle(), &cmp);
retval = true;
}
break;
default:
fprintf(stderr, "warning: unhandled opcode - %d\n", mp->data.opcode);
break;
......
......@@ -21,6 +21,7 @@
#include "mtp.h"
#include "dashboard.hh"
#include "wheelManager.hh"
/**
......@@ -55,7 +56,7 @@ public:
* @param wm The reference to the wheelManager that should be used to drive
* the robot.
*/
pilotClient(int fd, wheelManager &wm);
pilotClient(int fd, wheelManager &wm, dashboard &db);
/**
* Destruct a pilotClient object. This method will close the client
......@@ -127,7 +128,8 @@ private:
* The wheelManager the RMC client will use to drive the robot.
*/
wheelManager &pc_wheel_manager;
dashboard &pc_dashboard;
};
#endif
......@@ -29,8 +29,10 @@ LIBS += -lmtp -ltb -lm
test_rmcd.sh: rmcd
rmcd: rmcd.o ../mtp/libmtp.a
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ rmcd.o $(LIBS)
OBJS = rclip.o obstacles.o pilotConnection.o rmcd.o
rmcd: $(OBJS) ../mtp/libmtp.a
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(OBJS) $(LIBS)
install: all
-mkdir -p $(INSTALL_DIR)/opsdir/sbin
......
#include "config.h"
#include <stdio.h>
#include <assert.h>
#include "rclip.h"
#include "obstacles.h"
struct obstacle_config *ob_find_obstacle(struct mtp_config_rmc *mcr,
rc_line_t line)
{
struct obstacle_config *retval = NULL;
int lpc;
assert(mcr != NULL);
for (lpc = 0; lpc < mcr->obstacles.obstacles_len; lpc++) {
struct rc_line line_cp = *line;
struct obstacle_config *oc;
oc = &mcr->obstacles.obstacles_val[lpc];
printf(" %f %f -> %f %f -- %f %f %f %f\n",
line->x0, line->y0,
line->x1, line->y1,
oc->xmin, oc->ymin,
oc->xmax, oc->ymax);
if (rc_clip_line(&line_cp, oc)) {
retval = oc;
break;
}
}
return retval;
}
#ifndef _obstacles_h
#define _obstacles_h
#include "mtp.h"
#include "rclip.h"
struct obstacle_config *ob_find_obstacle(struct mtp_config_rmc *mcr,
rc_line_t line);
#endif
This diff is collapsed.
#ifndef _pilot_connection_h
#define _pilot_connection_h
#include <sys/types.h>
#include <sys/time.h>
#include <unistd.h>
#include "mtp.h"
typedef enum {
PS_ARRIVED,
PS_PENDING_POSITION,
PS_REFINING_POSITION,
PS_REFINING_ORIENTATION,
PS_START_WIGGLING,
PS_WIGGLING,
PS_MAX
} pilot_state_t;
enum {
PCB_CONNECTING,
PCB_CONNECTED,
PCB_VISION_POSITION,
PCB_WAYPOINT,
PCB_CONTACT,
};
enum {
PCF_CONNECTING = (1L << PCB_CONNECTING),