Commit 7e2040d0 authored by Timothy Stack's avatar Timothy Stack

If a robot destination intersects multiple known obstacles, merge them

into a single larger one.
parent 0ff9ec8a
#include "config.h"
#include <float.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)
#define min(x, y) ((x) < (y)) ? (x) : (y)
#define max(x, y) ((x) > (y)) ? (x) : (y)
int ob_find_obstacle(struct obstacle_config *oc_out,
struct mtp_config_rmc *mcr,
rc_line_t line)
{
struct obstacle_config *retval = NULL;
int lpc;
int lpc, retval = 0;
assert(oc_out != NULL);
assert(mcr != NULL);
assert(line != NULL);
oc_out->xmin = oc_out->ymin = FLT_MAX;
oc_out->xmax = oc_out->ymax = 0;
for (lpc = 0; lpc < mcr->obstacles.obstacles_len; lpc++) {
struct rc_line line_cp = *line;
struct obstacle_config *oc;
......@@ -26,8 +34,12 @@ struct obstacle_config *ob_find_obstacle(struct mtp_config_rmc *mcr,
oc->xmin, oc->ymin,
oc->xmax, oc->ymax);
if (rc_clip_line(&line_cp, oc)) {
retval = oc;
break;
oc_out->xmin = min(oc_out->xmin, oc->xmin);
oc_out->ymin = min(oc_out->ymin, oc->ymin);
oc_out->xmax = max(oc_out->xmax, oc->xmax);
oc_out->ymax = max(oc_out->ymax, oc->ymax);
retval = 1;
}
}
......
......@@ -5,7 +5,8 @@
#include "mtp.h"
#include "rclip.h"
struct obstacle_config *ob_find_obstacle(struct mtp_config_rmc *mcr,
rc_line_t line);
int ob_find_obstacle(struct obstacle_config *oc,
struct mtp_config_rmc *mcr,
rc_line_t line);
#endif
......@@ -240,7 +240,6 @@ struct pilot_connection *pc_find_pilot(int robot_id)
void pc_plot_waypoint(struct pilot_connection *pc)
{
struct obstacle_config *oc;
struct rc_line rl;
assert(pc != NULL);
......@@ -250,10 +249,11 @@ void pc_plot_waypoint(struct pilot_connection *pc)
rl.x1 = pc->pc_goal_pos.x;
rl.y1 = pc->pc_goal_pos.y;
if ((pc->pc_obstacle_count == 0) &&
(oc = ob_find_obstacle(pc_data.pcd_config, &rl)) != NULL) {
ob_find_obstacle(&pc->pc_obstacles[pc->pc_obstacle_count],
pc_data.pcd_config,
&rl)) {
info("debug: preloaded obstacle\n");
pc->pc_obstacles[pc->pc_obstacle_count] = *oc;
pc->pc_obstacle_count += 1;
}
......@@ -745,8 +745,6 @@ void pc_change_state(struct pilot_connection *pc, pilot_state_t ps)
switch (ps) {
case PS_REFINING_POSITION:
pc->pc_flags &= ~PCF_IN_PROGRESS;
if (pc->pc_state != ps) {
pc->pc_tries_remaining = MAX_REFINE_RETRIES;
}
......@@ -851,10 +849,6 @@ void pc_change_state(struct pilot_connection *pc, pilot_state_t ps)
case PS_PENDING_POSITION:
case PS_START_WIGGLING:
if ((ps == PS_PENDING_POSITION) &&
(pc->pc_state != PS_ARRIVED) && (pc->pc_state != PS_WIGGLING)) {
pc->pc_flags |= PCF_IN_PROGRESS;
}
mtp_init_packet(&pmp,
MA_Opcode, MTP_COMMAND_STOP,
MA_Role, MTP_ROLE_RMC,
......@@ -983,9 +977,7 @@ static void pc_handle_update(struct pilot_connection *pc,
case MTP_POSITION_STATUS_IDLE:
/* Response to a COMMAND_STOP. */
pc->pc_flags &= ~PCF_VISION_POSITION;
if (mup->command_id == 1 &&
!(pc->pc_flags & PCF_IN_PROGRESS) &&
pc->pc_state == PS_PENDING_POSITION) {
if (pc->pc_state == PS_PENDING_POSITION) {
pc_change_state(pc, PS_REFINING_POSITION);
}
break;
......@@ -1119,7 +1111,8 @@ static void pc_handle_report(struct pilot_connection *pc,
}
if (pc->pc_obstacle_count == 0 &&
(oc = ob_find_obstacle(pc_data.pcd_config, &rl)) != NULL) {
ob_find_obstacle(&oc_fake, pc_data.pcd_config, &rl)) {
oc = &oc_fake;
if (debug) {
info("debug: found obstacle %d\n", oc->id);
}
......
......@@ -27,7 +27,6 @@ enum {
PCB_WAYPOINT,
PCB_CONTACT,
PCB_WIGGLE_REVERSE,
PCB_IN_PROGRESS,
};
enum {
......@@ -37,7 +36,6 @@ enum {
PCF_WAYPOINT = (1L << PCB_WAYPOINT),
PCF_CONTACT = (1L << PCB_CONTACT),
PCF_WIGGLE_REVERSE = (1L << PCB_WIGGLE_REVERSE),
PCF_IN_PROGRESS = (1L << PCB_IN_PROGRESS),
};
struct pilot_connection {
......
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