def run(self, parameters):
result = self.fetch_result()
found_buoys = yield self.database_query("start_gate")
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
if len(buoys) == 2:
ogrid, setup, target = yield self.two_buoys(buoys, 10)
elif len(buoys) == 4:
ogrid, setup, target = yield self.four_buoys(buoys, 10)
else:
raise Exception
# Put the target into the point cloud as well
points = []
points.append(mil_tools.numpy_to_point(target.position))
pc = PointCloud(header=mil_tools.make_header(frame='/enu'),
points=np.array(points))
yield self._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield self.nh.sleep(3)
yield setup.go(move_type='skid')
pose = yield self.tx_pose
ogrid.generate_grid(pose)
msg = ogrid.get_message()
print "publishing"
self.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
print "START_GATE: Moving in 5 seconds!"
yield target.go(initial_plan_time=5)
defer.returnValue(result)
def publish_lla(self):
'''
Publish a point message with the current LLA computed from an odom and absodom message.
'''
_, _, lla = self.enu(self.enu_pos)
lla_msg = PointStamped()
lla_msg.header.frame_id = 'lla'
lla_msg.header.stamp = self.stamp
lla_msg.point = numpy_to_point(lla)
self.lla_pub.publish(lla_msg)
def publish_lla(self):
'''
Publish a point message with the current LLA computed from an odom and absodom message.
'''
_, _, lla = self.enu(self.enu_pos)
lla_msg = PointStamped()
lla_msg.header.frame_id = 'lla'
lla_msg.header.stamp = self.stamp
lla_msg.point = numpy_to_point(lla)
self.lla_pub.publish(lla_msg)
def setUp(self):
pub = rospy.Publisher('clicked_point', PointStamped, queue_size=4)
rospy.sleep(1)
points = np.array([[0, 0, 0], [10, 0, 0], [10, 10, 0], [0, 10, 0]])
for point in points:
ps = PointStamped()
ps.header.frame_id = 'a'
ps.point = numpy_to_point(point)
rospy.loginfo(ps)
pub.publish(ps)
rospy.sleep(1)
rospy.sleep(1)
def position_to_object(self, position, color, id, name="totem"):
obj = PerceptionObject()
obj.id = int(id)
obj.header = mil_tools.make_header(frame="enu")
obj.name = name
obj.position = mil_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 = mil_tools.make_header(frame="enu")
obj.name = name
obj.position = mil_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 got_request(self, req):
to_frame = "enu" if req.to_frame == '' else req.to_frame
resp = BoundsResponse(enforce=False)
self.bounds = [self.convert(CoordinateConversionRequest(frame="lla", point=lla)) for lla in self.lla_bounds]
bounds = [numpy_to_point(getattr(response, to_frame)) for response in self.bounds]
resp.enforce = self.enforce
resp.bounds = bounds
return resp
def setUp(self):
pub = rospy.Publisher('clicked_point', PointStamped, queue_size=4)
rospy.sleep(1)
points = np.array([[0, 0, 0],
[10, 0, 0],
[10, 10, 0],
[0, 10, 0]])
for point in points:
ps = PointStamped()
ps.header.frame_id = 'a'
ps.point = numpy_to_point(point)
rospy.loginfo(ps)
pub.publish(ps)
rospy.sleep(1)
rospy.sleep(1)
def got_request(self, req):
to_frame = "enu" if req.to_frame == '' else req.to_frame
resp = BoundsResponse(enforce=False)
self.bounds = [
self.convert(CoordinateConversionRequest(frame="lla", point=lla))
for lla in self.lla_bounds
]
bounds = [
numpy_to_point(getattr(response, to_frame))
for response in self.bounds
]
resp.enforce = self.enforce
resp.bounds = bounds
return resp
def as_MoveGoal(self, move_type=MoveGoal.DRIVE, **kwargs):
if 'focus' in kwargs:
if not isinstance(kwargs['focus'], Point):
kwargs['focus'] = mil_tools.numpy_to_point(kwargs['focus'])
if 'speed_factor' in kwargs and isinstance(kwargs['speed_factor'],
float):
# User wants a uniform speed factor
sf = kwargs['speed_factor']
kwargs['speed_factor'] = [sf, sf, sf]
for key in kwargs.keys():
if not hasattr(MoveGoal, key):
fprint(
"MoveGoal msg doesn't have a field called '{}' you tried to set via kwargs."
.format(key),
title="POSE_EDITOR",
msg_color="red")
del kwargs[key]
return MoveGoal(goal=self.as_Pose(), move_type=move_type, **kwargs)
def as_MoveGoal(self, move_type='drive', **kwargs):
if 'focus' in kwargs:
if not isinstance(kwargs['focus'], Point):
kwargs['focus'] = mil_tools.numpy_to_point(kwargs['focus'])
if 'speed_factor' in kwargs and isinstance(kwargs['speed_factor'], float):
# User wants a uniform speed factor
sf = kwargs['speed_factor']
kwargs['speed_factor'] = [sf, sf, sf]
for key in kwargs.keys():
if not hasattr(MoveGoal, key):
fprint("MoveGoal msg doesn't have a field called '{}' you tried to set via kwargs.".format(key), title="POSE_EDITOR",
msg_color="red")
del kwargs[key]
return MoveGoal(
goal=self.as_Pose(),
move_type=move_type,
**kwargs
)
def _make_bounds(cls):
fprint("Constructing bounds.", title="NAVIGATOR")
if (yield cls.nh.has_param("/bounds/enforce")):
_bounds = cls.nh.get_service_client('/get_bounds',
navigator_srvs.Bounds)
yield _bounds.wait_for_service()
resp = yield _bounds(navigator_srvs.BoundsRequest())
if resp.enforce:
cls.enu_bounds = [
mil_tools.rosmsg_to_numpy(bound) for bound in resp.bounds
]
# Just for display
pc = PointCloud(header=mil_tools.make_header(frame='/enu'),
points=np.array([
mil_tools.numpy_to_point(point)
for point in cls.enu_bounds
]))
yield cls._point_cloud_pub.publish(pc)
else:
fprint("No bounds param found, defaulting to none.",
title="NAVIGATOR")
cls.enu_bounds = None
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
'''
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
mil_tools.draw_ray_3d(red_left_pt, left_cam, [1, 0, 0, 1], m_id=0, frame="front_left_cam")
mil_tools.draw_ray_3d(red_right_pt, right_cam, [1, 0, 0, 1], m_id=1, frame="front_right_cam")
mil_tools.draw_ray_3d(green_left_pt, left_cam, [0, 1, 0, 1], m_id=2, frame="front_left_cam")
mil_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 = mil_tools.make_header(frame="enu")
red_point.point = mil_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = mil_tools.make_header(frame="enu")
green_point.point = mil_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 = mil_tools.make_header(frame="enu")
target_pose.pose = mil_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
def run(self, args):
if not self.pose:
raise Exception('Cant move: No odom')
commands = args.commands
arguments = commands[1::2]
commands = commands[0::2]
self.send_feedback('Switching trajectory to lqrrt')
self.change_trajectory('lqrrt')
self.send_feedback('Switching wrench to autonomous')
yield self.change_wrench('autonomous')
for i in xrange(len(commands)):
command = commands[i]
argument = arguments[i]
action_kwargs = {
'move_type': args.movetype,
'speed_factor': args.speedfactor
}
action_kwargs['blind'] = args.blind
if args.speedfactor is not None:
if ',' in args.speedfactor:
sf = np.array(map(float,
args.speedfactor[1:-1].split(',')))
else:
sf = [float(args.speedfactor)] * 3
action_kwargs['speed_factor'] = sf
if args.plantime is not None:
action_kwargs['initial_plan_time'] = float(args.plantime)
if args.focus is not None:
focus = np.array(map(float, args.focus[1:-1].split(',')))
focus[2] = 1 # Tell lqrrt we want to look at the point
point = numpy_to_point(focus)
action_kwargs['focus'] = point
if command == 'custom':
# Let the user input custom commands, the eval may be dangerous so do away with that at some point.
self.send_feedback(
"Moving with the command: {}".format(argument))
res = yield eval(
"self.move.{}.go(move_type='{move_type}')".format(
argument, **action_kwargs))
elif command == 'rviz':
self.send_feedback('Select a 2D Nav Goal in RVIZ')
target_pose = yield util.wrap_time_notice(
self.rviz_goal.get_next_message(), 2, "Rviz goal")
self.send_feedback('RVIZ pose recieved!')
res = yield self.move.to_pose(target_pose).go(**action_kwargs)
elif command == 'circle':
self.send_feedback('Select a Publish Point in RVIZ')
target_point = yield util.wrap_time_notice(
self.rviz_point.get_next_message(), 2, "Rviz point")
self.send_feedback('RVIZ point recieved!')
target_point = rosmsg_to_numpy(target_point.point)
direction = 'cw' if argument == '-1' else 'ccw'
res = yield self.move.circle_point(
target_point, direction=direction).go(**action_kwargs)
else:
shorthand = {
"f": "forward",
"b": "backward",
"l": "left",
"r": "right",
"yl": "yaw_left",
"yr": "yaw_right"
}
command = command if command not in shorthand.keys(
) else shorthand[command]
movement = getattr(self.move, command)
trans_move = command[:3] != "yaw"
unit = "m" if trans_move else "rad"
amount = argument
# See if there's a non standard unit at the end of the argument
if not argument[-1].isdigit():
last_digit_index = [
i for i, c in enumerate(argument) if c.isdigit()
][-1] + 1
amount = float(argument[:last_digit_index])
unit = argument[last_digit_index:]
# Get the kwargs to pass to the action server
station_hold = amount == '0'
if station_hold:
action_kwargs['move_type'] = MoveGoal.HOLD
msg = "Moving {} ".format(
command) if trans_move else "Yawing {} ".format(
command[4:])
self.send_feedback(msg + "{}{}".format(amount, unit))
res = yield movement(float(amount), unit).go(**action_kwargs)
if res.failure_reason is not '':
raise Exception('Move failed. Reason: {}'.format(
res.failure_reason))
defer.returnValue('Move completed successfully!')
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
'''
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
mil_tools.draw_ray_3d(red_left_pt,
left_cam, [1, 0, 0, 1],
m_id=0,
frame="front_left_cam")
mil_tools.draw_ray_3d(red_right_pt,
right_cam, [1, 0, 0, 1],
m_id=1,
frame="front_right_cam")
mil_tools.draw_ray_3d(green_left_pt,
left_cam, [0, 1, 0, 1],
m_id=2,
frame="front_left_cam")
mil_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 = mil_tools.make_header(frame="enu")
red_point.point = mil_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = mil_tools.make_header(frame="enu")
green_point.point = mil_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 = mil_tools.make_header(frame="enu")
target_pose.pose = mil_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
def enu_position_to_geo_point(self, enu_array):
self.send_feedback('Waiting for LLA conversion')
lla_msg = yield self.to_lla(
ToLLRequest(map_point=numpy_to_point(enu_array)))
defer.returnValue(lla_msg.ll_point)
def run(self, args):
if not self.pose:
raise Exception('Cant move: No odom')
commands = args.commands
arguments = commands[1::2]
commands = commands[0::2]
self.send_feedback('Switching wrench to autonomous')
yield self.change_wrench('autonomous')
for i in xrange(len(commands)):
command = commands[i]
argument = arguments[i]
action_kwargs = {'move_type': args.movetype, 'speed_factor': args.speedfactor}
action_kwargs['blind'] = args.blind
if args.speedfactor is not None:
if ',' in args.speedfactor:
sf = np.array(map(float, args.speedfactor[1:-1].split(',')))
else:
sf = [float(args.speedfactor)] * 3
action_kwargs['speed_factor'] = sf
if args.plantime is not None:
action_kwargs['initial_plan_time'] = float(args.plantime)
if args.focus is not None:
focus = np.array(map(float, args.focus[1:-1].split(',')))
focus[2] = 1 # Tell lqrrt we want to look at the point
point = numpy_to_point(focus)
action_kwargs['focus'] = point
if command == 'custom':
# Let the user input custom commands, the eval may be dangerous so do away with that at some point.
self.send_feedback("Moving with the command: {}".format(argument))
res = yield eval("self.move.{}.go(move_type='{move_type}')".format(argument, **action_kwargs))
elif command == 'rviz':
self.send_feedback('Select a 2D Nav Goal in RVIZ')
target_pose = yield util.wrap_time_notice(self.rviz_goal.get_next_message(), 2, "Rviz goal")
self.send_feedback('RVIZ pose recieved!')
res = yield self.move.to_pose(target_pose).go(**action_kwargs)
elif command == 'circle':
self.send_feedback('Select a Publish Point in RVIZ')
target_point = yield util.wrap_time_notice(self.rviz_point.get_next_message(), 2, "Rviz point")
self.send_feedback('RVIZ point recieved!')
target_point = rosmsg_to_numpy(target_point.point)
distance = np.linalg.norm(target_point - self.pose[0])
target_point = rosmsg_to_numpy(target_point.point)
direction = 'cw' if argument == '-1' else 'ccw'
res = yield self.move.circle_point(target_point, direction=direction).go(radius=distance)
else:
shorthand = {"f": "forward", "b": "backward", "l": "left",
"r": "right", "yl": "yaw_left", "yr": "yaw_right"}
command = command if command not in shorthand.keys() else shorthand[command]
movement = getattr(self.move, command)
trans_move = command[:3] != "yaw"
unit = "m" if trans_move else "rad"
amount = argument
# See if there's a non standard unit at the end of the argument
if not argument[-1].isdigit():
last_digit_index = [i for i, c in enumerate(argument) if c.isdigit()][-1] + 1
amount = float(argument[:last_digit_index])
unit = argument[last_digit_index:]
# Get the kwargs to pass to the action server
station_hold = amount == '0'
if station_hold:
action_kwargs['move_type'] = 'hold'
msg = "Moving {} ".format(command) if trans_move else "Yawing {} ".format(command[4:])
self.send_feedback(msg + "{}{}".format(amount, unit))
res = yield movement(float(amount), unit).go(**action_kwargs)
if res.failure_reason is not '':
raise Exception('Move failed. Reason: {}'.format(res.failure_reason))
defer.returnValue('Move completed successfully!')
