If a robot destination intersects multiple known obstacles, merge them

into a single larger one.

robots/rmcd/obstacles.c

 
 #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;
 	}
     }
 

robots/rmcd/obstacles.h

@@ -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

robots/rmcd/pilotConnection.c

@@ -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);
 	    }

robots/rmcd/pilotConnection.h

@@ -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 {