Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
emulab
emulab-devel
Commits
7e2040d0
Commit
7e2040d0
authored
Mar 07, 2005
by
Timothy Stack
Browse files
If a robot destination intersects multiple known obstacles, merge them
into a single larger one.
parent
0ff9ec8a
Changes
4
Hide whitespace changes
Inline
Side-by-side
robots/rmcd/obstacles.c
View file @
7e2040d0
#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
View file @
7e2040d0
...
...
@@ -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
View file @
7e2040d0
...
...
@@ -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
View file @
7e2040d0
...
...
@@ -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
{
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment