def state_cb(self, msg):
if self.target in msg.name:
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
enu_pose = navigator_tools.pose_to_numpy(msg.pose[target_index])
self.last_ecef = self.enu_to_ecef(enu_pose[0])
self.last_enu = enu_pose
self.last_odom = Odometry(
header=navigator_tools.make_header(frame='/enu'),
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(
*self.last_enu)),
twist=TwistWithCovariance(twist=twist))
self.last_absodom = Odometry(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(
self.last_ecef, self.last_enu[1])),
twist=TwistWithCovariance(twist=twist))
def state_cb(self, msg):
if rospy.Time.now() - self.last_state_sub_time < self.state_sub_max_prd:
return
self.last_state_sub_time = rospy.Time.now()
if self.target in msg.name:
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
enu_pose = navigator_tools.pose_to_numpy(msg.pose[target_index])
self.last_ecef = self.enu_to_ecef(enu_pose[0])
self.last_enu = enu_pose
self.last_odom = Odometry(
header=navigator_tools.make_header(frame='/enu'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(*self.last_enu)
),
twist=TwistWithCovariance(
twist=twist
)
)
self.last_absodom = Odometry(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(self.last_ecef, self.last_enu[1])
),
twist=TwistWithCovariance(
twist=twist
)
)
def state_cb(self, msg):
if self.target in msg.name:
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
enu_pose = navigator_tools.pose_to_numpy(msg.pose[target_index])
self.last_ecef = self.enu_to_ecef(enu_pose[0])
self.last_enu = enu_pose
self.last_odom = Odometry(
header=navigator_tools.make_header(frame='/enu'),
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(*self.last_enu)
),
twist=TwistWithCovariance(
twist=twist
)
)
self.last_absodom = Odometry(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(self.last_ecef, self.last_enu[1])
),
twist=TwistWithCovariance(
twist=twist
)
)
def state_cb(self, msg):
if rospy.Time.now(
) - self.last_state_sub_time < self.state_sub_max_prd:
return
self.last_state_sub_time = rospy.Time.now()
if self.target in msg.name:
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
enu_pose = navigator_tools.pose_to_numpy(msg.pose[target_index])
self.last_ecef = self.enu_to_ecef(enu_pose[0])
self.last_enu = enu_pose
self.last_odom = Odometry(
header=navigator_tools.make_header(frame='/enu'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(
*self.last_enu)),
twist=TwistWithCovariance(twist=twist))
self.last_absodom = Odometry(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/measurement',
pose=PoseWithCovariance(
pose=navigator_tools.numpy_quat_pair_to_pose(
self.last_ecef, self.last_enu[1])),
twist=TwistWithCovariance(twist=twist))
def move_cb(self, msg):
fprint("Move request received!", msg_color="blue")
if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
fprint("Move type '{}' not found".format(msg.move_type), msg_color='red')
self.move_server.set_aborted(MoveResult('move_type'))
return
self.blind = msg.blind
p = PoseStamped()
p.header = navigator_tools.make_header(frame="enu")
p.pose = msg.goal
self.goal_pose_pub.publish(p)
# Sleep before you continue
rospy.sleep(1)
yaw = trns.euler_from_quaternion(navigator_tools.rosmsg_to_numpy(msg.goal.orientation))[2]
if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)):
fprint("Not feasible", msg_color='red')
self.move_server.set_aborted(MoveResult('occupied'))
return
fprint("Finished move!", newline=2)
self.move_server.set_succeeded(MoveResult(''))
def __init__(self):
'''Alarm Handler
Listen for alarms, call scenarios
TODO:
- Add alarm queue
- Handle alarms in sequence (Don't get stuck in the callback)
- bag (if set) EVERY single alarm received
'''
# Queue size is large because you bet your ass we are addressing every alarm
self.alarm_sub = rospy.Subscriber('/alarm_raise', Alarm, self.alarm_callback, queue_size=100)
self.alarm_pub = rospy.Publisher('/alarm', Alarm, queue_size=100)
self.alarms = {}
self.scenarios = {}
for alarm in meta_alarms_inv:
self.alarms[alarm] = Alarm(alarm_name=alarm, clear=True, header=navigator_tools.make_header())
self.alarm_republish_timer = rospy.Timer(rospy.Duration(0.05), self.republish_alarms)
# Go through everything in the navigator_alarm.alarm_handlers package
for candidate_alarm_name in dir(alarm_handlers):
# Discard __* nonsense
if not candidate_alarm_name.startswith('_'):
# Verify that it is actually an alarm handler by checking if the class inherits from `HandlerBase`
handlers = inspect.getmembers(getattr(alarm_handlers, candidate_alarm_name), inspect.isclass)
handlers = [handler for handler in handlers if handler[0] != "HandlerBase" and \
issubclass(handler[1], HandlerBase)]
for handler_name, handler_class in handlers:
rospy.loginfo("Registered scenario with name '{}'".format(handler_class.alarm_name))
self.scenarios[handler_class.alarm_name] = handler_class()
def get_message(self):
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten(), -100, 100).astype(np.int8)
return ogrid
def get_message(self):
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def pub_ogrid(self, *args):
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame="enu")
ogrid.info = self.map_metadata
# Over saturate and clip to get range of [-1, 1]
self.grid = np.clip(self.grid * 1000, -1, 1)
ogrid.data = self.grid.flatten().astype(np.int8).tolist()
self.ogrid_pub.publish(ogrid)
def position_to_object(self, position, color, id, name="totem"):
obj = PerceptionObject()
obj.id = int(id)
obj.header = navigator_tools.make_header(frame="enu")
obj.name = name
obj.position = navigator_tools.numpy_to_point(position)
obj.color.r = color[0]
obj.color.g = color[1]
obj.color.b = color[2]
obj.color.a = 1
return obj
def position_to_object(self, position, color, id, name="totem"):
obj = PerceptionObject()
obj.id = int(id)
obj.header = navigator_tools.make_header(frame="enu")
obj.name = name
obj.position = navigator_tools.numpy_to_point(position)
obj.color.r = color[0]
obj.color.g = color[1]
obj.color.b = color[2]
obj.color.a = 1
return obj
def publish_odom(self, *args):
if self.last_odom is None:
return
msg = self.last_odom
if self.target in msg.name:
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.state_pub.publish(
header=navigator_tools.make_header(frame='/enu'),
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
self.absstate_pub.publish(
header=navigator_tools.make_header(frame='/ecef'),
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
def get_message(self):
if self.grid is None:
fprint("Ogrid was requested but no ogrid was found. Using blank.", msg_color='yellow')
self.grid = np.zeros((self.height / self.resolution, self.width / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def raise_alarm(self,
problem_description=None,
parameters=None,
severity=None):
'''
Arguments are override parameters
'''
got_problem_description = (problem_description != "") or (
self._problem_description is not None)
assert got_problem_description, "No problem description has been set for this alarm"
# Allow overrideable severity
if severity is None:
severity = self._severity
if parameters is not None:
assert isinstance(
parameters,
dict), "Parameters must be in the form of dictionary"
if problem_description is not None:
_problem_description = problem_description
else:
_problem_description = self._problem_description
if parameters is not None:
_parameters = parameters
else:
_parameters = self._parameters
alarm_contents = {
'action_required': self._action_required,
'problem_description': _problem_description,
'parameters': json.dumps(_parameters),
'severity': self._severity,
'alarm_name': self._alarm_name,
'node_name': self._node_name,
'clear': False,
}
if not self.different(**alarm_contents):
return
else:
self._previous_stuff = alarm_contents
alarm_msg = Alarm(header=navigator_tools.make_header(),
**alarm_contents)
self._alarm_pub.publish(alarm_msg)
return alarm_msg
def make_ray(base, direction, length, color, frame='/base_link', m_id=0, **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=navigator_tools.make_header(frame=frame),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(*color),
scale=Vector3(0.05, 0.05, 0.05),
points=map(lambda o: Point(*o), [base, direction * length]),
lifetime=rospy.Duration(),
**kwargs
)
return marker
def main(navigator):
res = navigator.fetch_result()
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = navigator_tools.point_to_numpy(buoy_field.objects[0])
#yield navigator.move.set_position(buoy_field_point).go()
circle_colors = ['blue', 'red']
color_map = {'blue': (255, 0, 0), 'red': (0, 0, 255)}
explored_ids = []
all_found = False
while not all_found:
target_totem, explored_ids = yield get_closest_buoy(navigator, explored_ids)
if target_totem is None:
fprint("No suitable totems found.", msg_color='red', title="CIRCLE_TOTEM")
continue
# Visualization
points = [target_totem.position]
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=points)
yield navigator._point_cloud_pub.publish(pc)
# Let's go there
target_distance = 7 # m
target_totem_np = navigator_tools.point_to_numpy(target_totem.position)
q = get_sun_angle()
lookat = navigator.move.set_position(target_totem_np).set_orientation(q).backward(target_distance)
yield lookat.go()
# Now that we're looking him in the eyes, aim no higher.
# Check the color and see if it's one we want.
fprint("Color request", title="CIRCLE_TOTEM")
#if target_totem is not None:
# all_found = True
defer.returnValue(res)
pattern = navigator.move.circle_point(focus, radius=5)
for p in pattern:
yield p.go(move_type='skid', focus=focus)
print "Nexting"
def clear_alarm(self):
alarm_contents = {
'alarm_name': self._alarm_name,
'node_name': self._node_name,
'severity': self._severity,
'action_required': False,
'clear': True,
}
if not self.different(**alarm_contents):
return
else:
self._previous_stuff = alarm_contents
alarm_msg = Alarm(header=navigator_tools.make_header(),
**alarm_contents)
self._alarm_pub.publish(alarm_msg)
def make_ogrid(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = navigator_tools.numpy_quat_pair_to_pose(np_origin, [0, 0, 0, 1])
grid = np.zeros((size / self.resolution, size / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame='enu')
ogrid.info.origin = origin
ogrid.info.width = size / self.resolution
ogrid.info.height = size / self.resolution
ogrid.info.resolution = self.resolution
ogrid.data = np.clip(np_arr.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def get_message(self):
if self.grid is None:
fprint("Ogrid was requested but no ogrid was found. Using blank.",
msg_color='yellow')
self.grid = np.zeros(
(self.height / self.resolution, self.width / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def make_ogrid(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = navigator_tools.numpy_quat_pair_to_pose(
np_origin, [0, 0, 0, 1])
grid = np.zeros((size / self.resolution, size / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame='enu')
ogrid.info.origin = origin
ogrid.info.width = size / self.resolution
ogrid.info.height = size / self.resolution
ogrid.info.resolution = self.resolution
ogrid.data = np.clip(np_arr.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def __init__(self, rand_size):
self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
self.odom = None
self.carrot_sub = rospy.Subscriber("/lqrrt/ref", Odometry, self.set_odom)
fprint("Shaking hands and taking names.")
rospy.sleep(1)
# We need to publish an inital odom message for lqrrt
start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
start_pos = np.append(np.random.uniform(rand_size, size=(2)), 1)
start_pose = navigator_tools.numpy_quat_pair_to_pose(start_pos, start_ori)
start_odom = Odometry()
start_odom.header = navigator_tools.make_header(frame='enu')
start_odom.child_frame_id = 'base_link'
start_odom.pose.pose = start_pose
self.odom_pub.publish(start_odom)
def raise_alarm(self, problem_description=None, parameters=None, severity=None):
'''
Arguments are override parameters
'''
got_problem_description = (problem_description != "") or (self._problem_description is not None)
assert got_problem_description, "No problem description has been set for this alarm"
# Allow overrideable severity
if severity is None:
severity = self._severity
if parameters is not None:
assert isinstance(parameters, dict), "Parameters must be in the form of dictionary"
if problem_description is not None:
_problem_description = problem_description
else:
_problem_description = self._problem_description
if parameters is not None:
_parameters = parameters
else:
_parameters = self._parameters
alarm_contents = {
'action_required': self._action_required,
'problem_description': _problem_description,
'parameters': json.dumps(_parameters),
'severity': self._severity,
'alarm_name': self._alarm_name,
'node_name': self._node_name,
'clear': False,
}
if not self.different(**alarm_contents):
return
else:
self._previous_stuff = alarm_contents
alarm_msg = Alarm(
header=navigator_tools.make_header(),
**alarm_contents
)
self._alarm_pub.publish(alarm_msg)
return alarm_msg
def clear_alarm(self):
alarm_contents = {
'alarm_name': self._alarm_name,
'node_name': self._node_name,
'severity': self._severity,
'action_required': False,
'clear': True,
}
if not self.different(**alarm_contents):
return
else:
self._previous_stuff = alarm_contents
alarm_msg = Alarm(
header=navigator_tools.make_header(),
**alarm_contents
)
self._alarm_pub.publish(alarm_msg)
def draw_sphere(position, color, scaling=(0.11, 0.11, 0.11), m_id=4, frame='/base_link'):
pose = Pose(
position=navigator_tools.numpy_to_point(position),
orientation=navigator_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=navigator_tools.make_header(frame=frame),
type=visualization_msgs.Marker.SPHERE,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(*scaling),
lifetime=rospy.Duration(),
)
rviz_pub.publish(marker)
def __init__(self, rand_size):
self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
self.odom = None
self.carrot_sub = rospy.Subscriber("/lqrrt/ref", Odometry,
self.set_odom)
fprint("Shaking hands and taking names.")
rospy.sleep(1)
# We need to publish an inital odom message for lqrrt
start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
start_pos = np.append(np.random.uniform(rand_size, size=(2)), 1)
start_pose = navigator_tools.numpy_quat_pair_to_pose(
start_pos, start_ori)
start_odom = Odometry()
start_odom.header = navigator_tools.make_header(frame='enu')
start_odom.child_frame_id = 'base_link'
start_odom.pose.pose = start_pose
self.odom_pub.publish(start_odom)
def make_ray(base,
direction,
length,
color,
frame='/base_link',
m_id=0,
**kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=navigator_tools.make_header(frame=frame),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(*color),
scale=Vector3(0.05, 0.05, 0.05),
points=map(lambda o: Point(*o), [base, direction * length]),
lifetime=rospy.Duration(),
**kwargs)
return marker
def draw_sphere(position,
color,
scaling=(0.11, 0.11, 0.11),
m_id=4,
frame='/base_link'):
pose = Pose(position=navigator_tools.numpy_to_point(position),
orientation=navigator_tools.numpy_to_quaternion(
[0.0, 0.0, 0.0, 1.0]))
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=navigator_tools.make_header(frame=frame),
type=visualization_msgs.Marker.SPHERE,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(*scaling),
lifetime=rospy.Duration(),
)
rviz_pub.publish(marker)
def __init__(self):
'''Alarm Handler
Listen for alarms, call scenarios
TODO:
- Add alarm queue
- Handle alarms in sequence (Don't get stuck in the callback)
- bag (if set) EVERY single alarm received
'''
# Queue size is large because you bet your ass we are addressing every alarm
self.alarm_sub = rospy.Subscriber('/alarm_raise',
Alarm,
self.alarm_callback,
queue_size=100)
self.alarm_pub = rospy.Publisher('/alarm', Alarm, queue_size=100)
self.alarms = {}
self.scenarios = {}
for alarm in meta_alarms_inv:
self.alarms[alarm] = Alarm(alarm_name=alarm,
clear=True,
header=navigator_tools.make_header())
self.alarm_republish_timer = rospy.Timer(rospy.Duration(0.05),
self.republish_alarms)
# Go through everything in the navigator_alarm.alarm_handlers package
for candidate_alarm_name in dir(alarm_handlers):
# Discard __* nonsense
if not candidate_alarm_name.startswith('_'):
# Verify that it is actually an alarm handler by checking if the class inherits from `HandlerBase`
handlers = inspect.getmembers(
getattr(alarm_handlers, candidate_alarm_name),
inspect.isclass)
handlers = [handler for handler in handlers if handler[0] != "HandlerBase" and \
issubclass(handler[1], HandlerBase)]
for handler_name, handler_class in handlers:
rospy.loginfo("Registered scenario with name '{}'".format(
handler_class.alarm_name))
self.scenarios[handler_class.alarm_name] = handler_class()
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
if len(buoys) == 2:
ogrid, setup, target = yield two_buoys(navigator, buoys, 10)
elif len(buoys) == 4:
ogrid, setup, target = yield four_buoys(navigator, buoys, 10)
else:
raise Exception
# Put the target into the point cloud as well
points = []
points.append(navigator_tools.numpy_to_point(target.position))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield setup.go(move_type='skid')
pose = yield navigator.tx_pose
ogrid.generate_grid(pose)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
print "START_GATE: Moving in 5 seconds!"
yield target.go(initial_plan_time=5)
return_with(result)
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
if len(buoys) == 2:
ogrid, setup, target = yield two_buoys(navigator, buoys, 10)
elif len(buoys) == 4:
ogrid, setup, target = yield four_buoys(navigator, buoys, 10)
else:
raise Exception
# Put the target into the point cloud as well
points = []
points.append(navigator_tools.numpy_to_point(target.position))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield setup.go(move_type='skid')
pose = yield navigator.tx_pose
ogrid.generate_grid(pose)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid,
msg)
print "START_GATE: Moving in 5 seconds!"
yield target.go(initial_plan_time=5)
return_with(result)
def as_MoveToGoal(self, linear=[0, 0, 0], angular=[0, 0, 0], **kwargs):
return MoveToGoal(
header=make_header(),
posetwist=self.as_PoseTwist(linear, angular),
**kwargs
)
def do_observe(self, *args):
resp = self.make_request(name='totem')
# If atleast one totem was found start observing
if resp.found:
# Time of the databse request
time_of_marker = resp.objects[0].header.stamp # - ros_t(1)
fprint("Looking for image at {}".format(time_of_marker.to_sec()),
msg_color='yellow')
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
t = self.image_history.newest_image.time
if t is None:
fprint("No images found.")
return
fprint("No valid image found for t={} ({}) dt: {}".format(
time_of_marker.to_sec(), t.to_sec(),
(rospy.Time.now() - t).to_sec()),
msg_color='red')
return
header = navigator_tools.make_header(frame='/enu',
stamp=image_holder.time)
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
try:
fprint(
"Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf,
time_of_marker, ros_t(1))
t_mat44 = self.tf_listener.asMatrix(cam_tf, header)
except tf.ExtrapolationException as e:
fprint("TF error found and excepted: {}".format(e))
return
for obj in resp.objects:
if len(obj.points) <= 1:
fprint("No points in object")
continue
fprint("{} {}".format(obj.id, "=" * 50))
# Get object position in px coordinates to determine if it's in frame
object_cam = t_mat44.dot(np.append(p2np(obj.position), 1))
object_px = map(
int, self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
fprint("Object not in frame")
continue
# Get enu points associated with this totem and remove ones that are too low
points_np = np.array(map(p2np, obj.points))
height = np.max(points_np[:, 2]) - np.min(points_np[:, 2])
if height < .1:
# If the height of the object is too small, skip (units are meters)
fprint("Object too small")
continue
threshold = np.min(points_np[:,
2]) + self.height_remove * height
points_np = points_np[points_np[:, 2] > threshold]
# Shove ones in there to make homogenous points to get points in image frame
points_np_homo = np.hstack(
(points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel,
points_cam[:, :3])
[
cv2.circle(self.debug.image, tuple(map(int, p)), 2,
(255, 255, 255), -1) for p in points_px
]
# Get color information from the points
roi = self._get_ROI_from_points(points_px)
h, s, v = self._get_hsv_from_ROI(roi, image)
# Compute parameters that go into the confidence
boat_q = self.odom[1]
target_q = self._get_solar_angle()
q_err = self._get_quaternion_error(boat_q, target_q)
dist = np.linalg.norm(self.odom[0] - p2np(obj.position))
fprint("H: {}, S: {}, V: {}".format(h, s, v))
fprint("q_err: {}, dist: {}".format(q_err, dist))
# Add to database and setup debug image
if s < self.saturation_reject or v < self.value_reject:
err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
fprint(err_msg.format(s, self.saturation_reject, v,
self.value_reject),
msg_color='red')
else:
if obj.id not in self.colored:
self.colored[obj.id] = Observation()
# Add this observation in
self.colored[obj.id] += h, v, dist, q_err
print self.colored[obj.id]
rgb = (0, 0, 0)
color = self.do_estimate(obj.id)
# Do we have a valid color estimate
if color:
fprint("COLOR IS VALID", msg_color='green')
rgb = self.database_color_map[color[0]]
cmd = '{name}={rgb[0]},{rgb[1]},{rgb[2]},{_id}'
self.make_request(
cmd=cmd.format(name=obj.name, _id=obj.id, rgb=rgb))
bgr = rgb[::-1]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id), tuple(object_px),
font, 1, bgr, 2)
def do_buoys(srv, left, right, red_seg, green_seg, tf_listener):
'''
FYI:
`left`: the left camera ImageGetter object
`right`: the right camera ImageGetter object
'''
left_point = None
right_point = None
while not rospy.is_shutdown():
# Get all the required TF links
try:
# Working copy of the current frame obtained at the same time as the tf link
tf_listener.waitForTransform("enu", "front_left_cam", rospy.Time(),
rospy.Duration(4.0))
left_image, right_image = left.frame, right.frame
cam_tf = tf_listener.lookupTransform("front_left_cam",
"front_right_cam",
left.sub.last_image_time)
cam_p, cam_q = tf_listener.lookupTransform(
"enu", "front_left_cam", left.sub.last_image_time)
cam_p = np.array([cam_p])
cam_r = tf.transformations.quaternion_matrix(cam_q)[:3, :3]
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, TypeError) as e:
print e
rospy.logwarn("TF link not found.")
time.sleep(.5)
continue
red_left_pt, rl_area = red_seg.segment(left_image)
red_right_pt, rr_area = red_seg.segment(right_image)
green_left_pt, gl_area = green_seg.segment(left_image)
green_right_pt, gr_area = green_seg.segment(right_image)
area_tol = 50
if np.linalg.norm(rl_area - rr_area) > area_tol or np.linalg.norm(
gl_area - gr_area) > area_tol:
rospy.logwarn("Unsafe segmentation")
StartGateResponse(success=False)
red_point_np = do_the_magic(red_left_pt, red_right_pt, cam_tf)
red_point_np = np.array(cam_r.dot(red_point_np) + cam_p)[0]
green_point_np = do_the_magic(green_left_pt, green_right_pt, cam_tf)
green_point_np = np.array(cam_r.dot(green_point_np) + cam_p)[0]
# Just for visualization
for i in range(5):
# Publish it 5 times so we can see it in rviz
navigator_tools.draw_ray_3d(red_left_pt,
left_cam, [1, 0, 0, 1],
m_id=0,
frame="front_left_cam")
navigator_tools.draw_ray_3d(red_right_pt,
right_cam, [1, 0, 0, 1],
m_id=1,
frame="front_right_cam")
navigator_tools.draw_ray_3d(green_left_pt,
left_cam, [0, 1, 0, 1],
m_id=2,
frame="front_left_cam")
navigator_tools.draw_ray_3d(green_right_pt,
right_cam, [0, 1, 0, 1],
m_id=3,
frame="front_right_cam")
red_point = PointStamped()
red_point.header = navigator_tools.make_header(frame="enu")
red_point.point = navigator_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = navigator_tools.make_header(frame="enu")
green_point.point = navigator_tools.numpy_to_point(green_point_np)
green_pub.publish(green_point)
time.sleep(1)
# Now we have two points, find their midpoint and calculate a target angle
midpoint = (red_point_np + green_point_np) / 2
between_vector = green_point_np - red_point_np # Red is on the left when we go out.
yaw_theta = np.arctan2(between_vector[1], between_vector[0])
# Rotate that theta by 90 deg to get the angle through the buoys
yaw_theta += np.pi / 2.0
p = midpoint
q = tf.transformations.quaternion_from_euler(0, 0, yaw_theta)
target_pose = PoseStamped()
target_pose.header = navigator_tools.make_header(frame="enu")
target_pose.pose = navigator_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
def do_update(self, *args):
resp = self.make_request(name='all')
if resp.found:
# temp
time_of_marker = rospy.Time.now() - ros_t(
.2) # header.stamp not filled out
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
return
header = navigator_tools.make_header(
frame="/enu", stamp=image_holder.time) #resp.objects[0].header
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
fprint("Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker,
ros_t(1))
t_mat44 = self.tf_listener.asMatrix(
cam_tf, header) # homogenous 4x4 transformation matrix
for obj in resp.objects:
if len(obj.points) > 0 and obj.name == "totem":
fprint("{} {}".format(obj.id, "=" * 50))
print obj.position
# Get objects position in camera frame and make sure it's in view
object_cam = t_mat44.dot(
np.append(navigator_tools.point_to_numpy(obj.position),
1))
object_px = map(
int,
self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
continue
#print object_px
points_np = np.array(
map(navigator_tools.point_to_numpy, obj.points))
# We dont want points below a certain level
points_np = points_np[points_np[:, 2] > -2.5]
# Shove ones in there to make homogenous points
points_np_homo = np.hstack(
(points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel,
points_cam[:, :3])
#print points_px
roi = self._get_ROI_from_points(points_px)
color_info = self._get_color_from_ROI(
roi, image) # hue, string_color, error
bgr = (0, 0, 0)
if color_info is not None:
hue, color, error = color_info
c = (int(hue), 255, 255)
hsv = np.array([[c]])[:, :3].astype(np.uint8)
bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0, 0]
bgr = tuple(bgr.astype(np.uint8).tolist())
if hue is not None:
if obj.id not in self.colored:
self.colored[obj.id] = []
self.colored[obj.id].append({
'color': color,
'err': error
})
if self.valid_color(obj.id):
fprint("COLOR IS VALID", msg_color='green')
self.make_request(
cmd=
'{name}={bgr[2]},{bgr[1]},{bgr[0]},{_id}'.
format(name=obj.name, _id=obj.id, bgr=bgr))
[
cv2.circle(self.debug.image, tuple(map(int, p)), 2,
bgr, -1) for p in points_px
]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id),
tuple(object_px), font, 1, bgr, 2)
print '\n' * 2
def do_update(self, *args):
resp = self.make_request(name='all')
if resp.found:
# temp
time_of_marker = rospy.Time.now() - ros_t(.2) # header.stamp not filled out
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
return
header = navigator_tools.make_header(frame="/enu", stamp=image_holder.time) #resp.objects[0].header
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
fprint("Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker, ros_t(1))
t_mat44 = self.tf_listener.asMatrix(cam_tf, header) # homogenous 4x4 transformation matrix
for obj in resp.objects:
if len(obj.points) > 0 and obj.name == "totem":
fprint("{} {}".format(obj.id, "=" * 50))
print obj.position
# Get objects position in camera frame and make sure it's in view
object_cam = t_mat44.dot(np.append(navigator_tools.point_to_numpy(obj.position), 1))
object_px = map(int, self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
continue
#print object_px
points_np = np.array(map(navigator_tools.point_to_numpy, obj.points))
# We dont want points below a certain level
points_np = points_np[points_np[:, 2] > -2.5]
# Shove ones in there to make homogenous points
points_np_homo = np.hstack((points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel, points_cam[:, :3])
#print points_px
roi = self._get_ROI_from_points(points_px)
color_info = self._get_color_from_ROI(roi, image) # hue, string_color, error
bgr = (0, 0, 0)
if color_info is not None:
hue, color, error = color_info
c = (int(hue), 255, 255)
hsv = np.array([[c]])[:, :3].astype(np.uint8)
bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0, 0]
bgr = tuple(bgr.astype(np.uint8).tolist())
if hue is not None:
if obj.id not in self.colored:
self.colored[obj.id] = []
self.colored[obj.id].append({'color':color, 'err':error})
if self.valid_color(obj.id):
fprint("COLOR IS VALID", msg_color='green')
self.make_request(cmd='{name}={bgr[2]},{bgr[1]},{bgr[0]},{_id}'.format(name=obj.name,_id= obj.id, bgr=bgr))
[cv2.circle(self.debug.image, tuple(map(int, p)), 2, bgr, -1) for p in points_px]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id), tuple(object_px), font, 1, bgr, 2)
print '\n' * 2
def do_buoys(srv, left, right, red_seg, green_seg, tf_listener):
'''
FYI:
`left`: the left camera ImageGetter object
`right`: the right camera ImageGetter object
'''
left_point = None
right_point = None
while not rospy.is_shutdown():
# Get all the required TF links
try:
# Working copy of the current frame obtained at the same time as the tf link
tf_listener.waitForTransform("enu", "stereo_left_cam", rospy.Time(), rospy.Duration(4.0))
left_image, right_image = left.frame, right.frame
cam_tf = tf_listener.lookupTransform("stereo_left_cam", "stereo_right_cam", left.sub.last_image_time)
cam_p, cam_q = tf_listener.lookupTransform("enu", "stereo_left_cam", left.sub.last_image_time)
cam_p = np.array([cam_p])
cam_r = tf.transformations.quaternion_matrix(cam_q)[:3, :3]
break
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, TypeError) as e:
print e
rospy.logwarn("TF link not found.")
time.sleep(.5)
continue
red_left_pt, rl_area = red_seg.segment(left_image)
red_right_pt, rr_area = red_seg.segment(right_image)
green_left_pt, gl_area = green_seg.segment(left_image)
green_right_pt, gr_area = green_seg.segment(right_image)
area_tol = 50
if np.linalg.norm(rl_area - rr_area) > area_tol or np.linalg.norm(gl_area - gr_area) > area_tol:
rospy.logwarn("Unsafe segmentation")
StartGateResponse(success=False)
red_point_np = do_the_magic(red_left_pt, red_right_pt, cam_tf)
red_point_np = np.array(cam_r.dot(red_point_np) + cam_p)[0]
green_point_np = do_the_magic(green_left_pt, green_right_pt, cam_tf)
green_point_np = np.array(cam_r.dot(green_point_np) + cam_p)[0]
# Just for visualization
for i in range(5):
# Publish it 5 times so we can see it in rviz
navigator_tools.draw_ray_3d(red_left_pt, left_cam, [1, 0, 0, 1], m_id=0, frame="stereo_left_cam")
navigator_tools.draw_ray_3d(red_right_pt, right_cam, [1, 0, 0, 1], m_id=1, frame="stereo_right_cam")
navigator_tools.draw_ray_3d(green_left_pt, left_cam, [0, 1, 0, 1], m_id=2, frame="stereo_left_cam")
navigator_tools.draw_ray_3d(green_right_pt, right_cam, [0, 1, 0, 1], m_id=3, frame="stereo_right_cam")
red_point = PointStamped()
red_point.header = navigator_tools.make_header(frame="enu")
red_point.point = navigator_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = navigator_tools.make_header(frame="enu")
green_point.point = navigator_tools.numpy_to_point(green_point_np)
green_pub.publish(green_point)
time.sleep(1)
# Now we have two points, find their midpoint and calculate a target angle
midpoint = (red_point_np + green_point_np) / 2
between_vector = green_point_np - red_point_np # Red is on the left when we go out.
yaw_theta = np.arctan2(between_vector[1], between_vector[0])
# Rotate that theta by 90 deg to get the angle through the buoys
yaw_theta += np.pi / 2.0
p = midpoint
q = tf.transformations.quaternion_from_euler(0, 0, yaw_theta)
target_pose = PoseStamped()
target_pose.header = navigator_tools.make_header(frame="enu")
target_pose.pose = navigator_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
if found_buoys.found:
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
else:
print "START_GATE: Error 4 - No db buoy response..."
result.success = False
result.response = result.DbObjectNotFound
result.message = "Start gates not found in the database."
return_with(result)
if len(buoys) != 4:
print "START_GATE: Error 5 - Invaild number of buoys found: {} (expecting 4)".format(
len(buoys))
result.success = False
result.message = "Invaild number of buoys found: {} (expecting 4)".format(
len(buoys))
return_with(result)
# buoys = [Buoy.from_srv(left), Buoy.from_srv(right)]
#buoys = np.array(get_buoys())
pose = yield navigator.tx_pose
# Get the ones closest to us and assume those are the front
distances = np.array([b.distance(pose[0]) for b in buoys])
back = buoys[np.argsort(distances)[-2:]]
front = buoys[np.argsort(distances)[:2]]
#print "front", front
#print "back", back
# Made it this far, make sure the red one is on the left and green on the right ================
t = txros.tf.Transform.from_Pose_message(
navigator_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = t.as_matrix()
f_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in front]
b_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in back]
# Angles are w.r.t positive x-axis. Positive is CCW around the z-axis.
angle_buoys = np.array(
[np.arctan2(buoy[1], buoy[0]) for buoy in f_bl_buoys])
#print angle_buoys, front
f_left_buoy, f_right_buoy = front[np.argmax(angle_buoys)], front[np.argmin(
angle_buoys)]
angle_buoys = np.array(
[np.arctan2(buoy[1], buoy[0]) for buoy in b_bl_buoys])
b_left_buoy, b_right_buoy = back[np.argmax(angle_buoys)], back[np.argmin(
angle_buoys)]
points = [
navigator_tools.numpy_to_point(b.position) for b in [f_left_buoy]
]
# Lets plot a course, yarrr
f_between_vector, f_direction_vector = get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
through_vector = b_mid_point - f_mid_point
through_vector = through_vector / np.linalg.norm(through_vector)
#print mid_point
setup_dist = 20 # Line up with the start gate this many meters infront of the gate.
setup = f_mid_point - f_direction_vector * setup_dist
target = b_mid_point + through_vector * setup_dist
# Put the target into the point cloud as well
#points.append(navigator_tools.numpy_to_point(b_left_buoy))
points.append(navigator_tools.numpy_to_point(target))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield navigator.move.set_position(setup).look_at(f_mid_point).go(
move_type="skid")
pose = yield navigator.tx_pose
ogrid = OgridFactory(f_left_buoy.position,
f_right_buoy.position,
pose[0],
target,
left_b_pos=b_left_buoy.position,
right_b_pos=b_right_buoy.position)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid,
msg)
yield navigator.nh.sleep(5)
print "START_GATE: Moving in 5 seconds!"
yield navigator.move.set_position(target).go(initial_plan_time=5)
return_with(result)
def do_observe(self, *args):
resp = self.make_request(name='totem')
# If atleast one totem was found start observing
if resp.found:
# Time of the databse request
time_of_marker = resp.objects[0].header.stamp# - ros_t(1)
fprint("Looking for image at {}".format(time_of_marker.to_sec()), msg_color='yellow')
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
t = self.image_history.newest_image.time
if t is None:
fprint("No images found.")
return
fprint("No valid image found for t={} ({}) dt: {}".format(time_of_marker.to_sec(), t.to_sec(), (rospy.Time.now() - t).to_sec()), msg_color='red')
return
header = navigator_tools.make_header(frame='/enu', stamp=image_holder.time)
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
try:
fprint("Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker, ros_t(1))
t_mat44 = self.tf_listener.asMatrix(cam_tf, header)
except tf.ExtrapolationException as e:
fprint("TF error found and excepted: {}".format(e))
return
for obj in resp.objects:
if len(obj.points) <= 1:
fprint("No points in object")
continue
fprint("{} {}".format(obj.id, "=" * 50))
# Get object position in px coordinates to determine if it's in frame
object_cam = t_mat44.dot(np.append(p2np(obj.position), 1))
object_px = map(int, self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
fprint("Object not in frame")
continue
# Get enu points associated with this totem and remove ones that are too low
points_np = np.array(map(p2np, obj.points))
height = np.max(points_np[:, 2]) - np.min(points_np[:, 2])
if height < .1:
# If the height of the object is too small, skip (units are meters)
fprint("Object too small")
continue
threshold = np.min(points_np[:, 2]) + self.height_remove * height
points_np = points_np[points_np[:, 2] > threshold]
# Shove ones in there to make homogenous points to get points in image frame
points_np_homo = np.hstack((points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel, points_cam[:, :3])
[cv2.circle(self.debug.image, tuple(map(int, p)), 2, (255, 255, 255), -1) for p in points_px]
# Get color information from the points
roi = self._get_ROI_from_points(points_px)
h, s, v = self._get_hsv_from_ROI(roi, image)
# Compute parameters that go into the confidence
boat_q = self.odom[1]
target_q = self._get_solar_angle()
q_err = self._get_quaternion_error(boat_q, target_q)
dist = np.linalg.norm(self.odom[0] - p2np(obj.position))
fprint("H: {}, S: {}, V: {}".format(h, s, v))
fprint("q_err: {}, dist: {}".format(q_err, dist))
# Add to database and setup debug image
if s < self.saturation_reject or v < self.value_reject:
err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
fprint(err_msg.format(s, self.saturation_reject, v, self.value_reject), msg_color='red')
else:
if obj.id not in self.colored:
self.colored[obj.id] = Observation()
# Add this observation in
self.colored[obj.id] += h, v, dist, q_err
print self.colored[obj.id]
rgb = (0, 0, 0)
color = self.do_estimate(obj.id)
# Do we have a valid color estimate
if color:
fprint("COLOR IS VALID", msg_color='green')
rgb = self.database_color_map[color[0]]
cmd = '{name}={rgb[0]},{rgb[1]},{rgb[2]},{_id}'
self.make_request(cmd=cmd.format(name=obj.name,_id=obj.id, rgb=rgb))
bgr = rgb[::-1]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id), tuple(object_px), font, 1, bgr, 2)
def keepalive_pub(self, *args):
h = navigator_tools.make_header()
self.pub.publish(h)
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
if found_buoys.found:
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
else:
print "START_GATE: Error 4 - No db buoy response..."
result.success = False
result.response = result.DbObjectNotFound
result.message = "Start gates not found in the database."
return_with(result)
if len(buoys) != 4:
print "START_GATE: Error 5 - Invaild number of buoys found: {} (expecting 4)".format(len(buoys))
result.success = False
result.message = "Invaild number of buoys found: {} (expecting 4)".format(len(buoys))
return_with(result)
# buoys = [Buoy.from_srv(left), Buoy.from_srv(right)]
#buoys = np.array(get_buoys())
pose = yield navigator.tx_pose
# Get the ones closest to us and assume those are the front
distances = np.array([b.distance(pose[0]) for b in buoys])
back = buoys[np.argsort(distances)[-2:]]
front = buoys[np.argsort(distances)[:2]]
#print "front", front
#print "back", back
# Made it this far, make sure the red one is on the left and green on the right ================
t = txros.tf.Transform.from_Pose_message(navigator_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = t.as_matrix()
f_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in front]
b_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in back]
# Angles are w.r.t positive x-axis. Positive is CCW around the z-axis.
angle_buoys = np.array([np.arctan2(buoy[1], buoy[0]) for buoy in f_bl_buoys])
#print angle_buoys, front
f_left_buoy, f_right_buoy = front[np.argmax(angle_buoys)], front[np.argmin(angle_buoys)]
angle_buoys = np.array([np.arctan2(buoy[1], buoy[0]) for buoy in b_bl_buoys])
b_left_buoy, b_right_buoy = back[np.argmax(angle_buoys)], back[np.argmin(angle_buoys)]
points = [navigator_tools.numpy_to_point(b.position) for b in [f_left_buoy]]
# Lets plot a course, yarrr
f_between_vector, f_direction_vector = get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
through_vector = b_mid_point - f_mid_point
through_vector = through_vector / np.linalg.norm(through_vector)
#print mid_point
setup_dist = 20 # Line up with the start gate this many meters infront of the gate.
setup = f_mid_point - f_direction_vector * setup_dist
target = b_mid_point + through_vector * setup_dist
# Put the target into the point cloud as well
#points.append(navigator_tools.numpy_to_point(b_left_buoy))
points.append(navigator_tools.numpy_to_point(target))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield navigator.move.set_position(setup).look_at(f_mid_point).go(move_type="skid")
pose = yield navigator.tx_pose
ogrid = OgridFactory(f_left_buoy.position, f_right_buoy.position, pose[0],
target, left_b_pos=b_left_buoy.position, right_b_pos=b_right_buoy.position)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
yield navigator.nh.sleep(5)
print "START_GATE: Moving in 5 seconds!"
yield navigator.move.set_position(target).go(initial_plan_time=5)
return_with(result)
def as_MoveToGoal(self, linear=[0, 0, 0], angular=[0, 0, 0], **kwargs):
return MoveToGoal(header=make_header(),
posetwist=self.as_PoseTwist(linear, angular),
**kwargs)
