def publish_odom(self, *args):
if self.last_odom is None or self.position_offset is None:
return
msg = self.last_odom
if self.target in msg.name:
header = mil_ros_tools.make_header(frame='/map')
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
# Add position offset to make the start position (0, 0, -depth)
position_np, orientation_np = mil_ros_tools.pose_to_numpy(
msg.pose[target_index])
pose = mil_ros_tools.numpy_quat_pair_to_pose(
position_np - self.position_offset, orientation_np)
self.state_pub.publish(header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
header = mil_ros_tools.make_header(frame='/world')
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.world_state_pub.publish(
header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
else:
# fail
return
def publish_target_pose(self, position, orientation):
self.target_pose_pub.publish(
PoseStamped(header=mil_ros_tools.make_header('/map'),
pose=Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(
orientation))))
self.distance_marker.header = mil_ros_tools.make_header('/map')
self.distance_marker.text = 'XY: ' + str(self.target_distance) + 'm\n' +\
'Z: ' + str(self.target_depth) + 'm'
self.distance_marker.pose = Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation))
self.target_distance_pub.publish(self.distance_marker)
def range_callback(self, msg):
'''Handle range data grabbed from dvl'''
# future: should be /base_link/dvl, no?
frame = '/dvl'
distance = msg.range
# Color a sharper red if we're in danger
if distance < 1.0:
color = (1.0, 0.1, 0.0, 0.9)
else:
color = (0.2, 0.8, 0.0, 0.7)
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=color, # red,
frame=frame,
up_vector=np.array([0.0, 0.0, -1.0]), # up is down in range world
id=1 # Keep these guys from overwriting eachother
)
self.depth_marker.ns = 'sub'
self.depth_marker.header = mil_ros_tools.make_header(frame='/dvl')
self.depth_marker.pose = marker.pose
self.depth_marker.text = str(round(distance, 3)) + 'm'
self.depth_marker.id = 1
self.rviz_pub_t.publish(self.depth_marker)
self.rviz_pub.publish(marker)
def range_callback(self, msg):
'''Handle range data grabbed from dvl'''
# future: should be /base_link/dvl, no?
frame = '/dvl'
distance = msg.range
# Color a sharper red if we're in danger
if distance < 1.0:
color = (1.0, 0.1, 0.0, 0.9)
else:
color = (0.2, 0.8, 0.0, 0.7)
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=color, # red,
frame=frame,
up_vector=np.array([0.0, 0.0, -1.0]), # up is down in range world
id=1 # Keep these guys from overwriting eachother
)
self.depth_marker.ns = 'sub'
self.depth_marker.header = mil_ros_tools.make_header(frame='/dvl')
self.depth_marker.pose = marker.pose
self.depth_marker.text = str(round(distance, 3)) + 'm'
self.depth_marker.id = 1
self.rviz_pub_t.publish(self.depth_marker)
self.rviz_pub.publish(marker)
def make_cylinder_marker(self,
base,
length,
color,
frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]),
**kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion(
[0.0, 0.0, 0.0, 1.0]))
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs)
return marker
def publish_target_pose(self, position, orientation):
self.target_pose_pub.publish(
PoseStamped(
header=mil_ros_tools.make_header('/map'),
pose=Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation)
)
)
)
self.distance_marker.header = mil_ros_tools.make_header('/map')
self.distance_marker.text = 'XY: ' + str(self.target_distance) + 'm\n' +\
'Z: ' + str(self.target_depth) + 'm'
self.distance_marker.pose = Pose(position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation))
self.target_distance_pub.publish(self.distance_marker)
def moveto_action(self, position, orientation):
self.client.cancel_goal()
rospy.logwarn("Going to waypoint")
rospy.logwarn("Found server")
# Stay under the water
if position[2] > 0.3:
rospy.logwarn("Waypoint set too high!")
position[2] = -0.5
goal = mil_msgs.MoveToGoal(
header=mil_ros_tools.make_header('/map'),
posetwist=mil_msgs.PoseTwist(
pose=geom_msgs.Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation)
)
),
speed=0.2,
linear_tolerance=0.1,
angular_tolerance=0.1
)
self.client.send_goal(goal)
# self.client.wait_for_result()
rospy.logwarn("Go to waypoint")
def request_bin(self, srv):
self.bin_type = srv.target_name
if (self.last_image != None):
print 'requesting', srv
response = self.find_single_bin(np.copy(self.last_image),
srv.target_name)
if response is False or response is None:
print 'did not find'
resp = VisionRequest2DResponse(
header=mil_ros_tools.make_header(frame='/down'),
found=False)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=self.last_image_time,
frame_id='/down'),
pose=Pose2D(x=center[0], y=center[1], theta=radius),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True)
return resp
def publish_pose(self):
'''TODO:
Publish velocity in body frame
'''
linear_vel = self.body.getRelPointVel((0.0, 0.0, 0.0))
# TODO: Not 100% on this transpose
angular_vel = self.orientation.dot(self.angular_vel)
# angular_vel = self.orientation.transpose().dot(self.angular_vel)
quaternion = self.body.getQuaternion()
translation = self.body.getPosition()
header = mil_ros_tools.make_header(frame='/map')
pose = geometry.Pose(
position=geometry.Point(*translation),
orientation=geometry.Quaternion(quaternion[1], quaternion[2],
quaternion[3], quaternion[0]),
)
twist = geometry.Twist(linear=geometry.Vector3(*linear_vel),
angular=geometry.Vector3(*angular_vel))
odom_msg = Odometry(header=header,
child_frame_id='/base_link',
pose=geometry.PoseWithCovariance(pose=pose),
twist=geometry.TwistWithCovariance(twist=twist))
self.truth_odom_pub.publish(odom_msg)
def moveto_action(self, position, orientation):
self.client.cancel_goal()
rospy.logwarn("Going to waypoint")
rospy.logwarn("Found server")
# Stay under the water
if position[2] > 0.3:
rospy.logwarn("Waypoint set too high!")
position[2] = -0.5
goal = mil_msgs.MoveToGoal(
header=mil_ros_tools.make_header('/map'),
posetwist=mil_msgs.PoseTwist(
pose=geom_msgs.Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation)
)
),
speed=0.2,
linear_tolerance=0.1,
angular_tolerance=0.1
)
self.client.send_goal(goal)
# self.client.wait_for_result()
rospy.logwarn("Go to waypoint")
def publish_height(self, msg):
'''Sim DVL uses laserscan message to relay height'''
self.dvl_pub.publish(
RangeStamped(
header=mil_ros_tools.make_header(),
range=msg.range
)
)
def publish_odom(self, *args):
if self.last_odom is None or self.position_offset is None:
return
msg = self.last_odom
if self.target in msg.name:
header = mil_ros_tools.make_header(frame='/map')
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
# Add position offset to make the start position (0, 0, -depth)
position_np, orientation_np = mil_ros_tools.pose_to_numpy(msg.pose[
target_index])
pose = mil_ros_tools.numpy_quat_pair_to_pose(
position_np - self.position_offset, orientation_np)
self.state_pub.publish(
header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=pose
),
twist=TwistWithCovariance(
twist=twist
)
)
header = mil_ros_tools.make_header(frame='/world')
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.world_state_pub.publish(
header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=pose
),
twist=TwistWithCovariance(
twist=twist
)
)
else:
# fail
return
def send_wrench(self, force, torque):
'''
Specify wrench in Newtons, Newton-meters
(Are you supposed to say Newtons-meter? Newtons-Meters?)
'''
wrench_msg = geometry_msgs.WrenchStamped(
header=mil_ros_tools.make_header(),
wrench=geometry_msgs.Wrench(force=geometry_msgs.Vector3(*force),
torque=geometry_msgs.Vector3(*torque)))
self.wrench_pub.publish(wrench_msg)
def set_target(self, pos, orientation=(0.0, 0.0, 0.0, 1.0)):
'''TODO:
Add linear/angular velocity
Publish a trajectory
'''
# self.target = make_pose_stamped(position=pos, orientation=orientation)
target_msg = Trajectory(
header=mil_ros_tools.make_header(frame='/body'),
trajectory=[
Waypoint(pose=geometry_msgs.Pose(
position=geometry_msgs.Vector3(*pos),
orientation=geometry_msgs.Quaternion(*orientation)))
])
self.target_pub.publish(target_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=mil_ros_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 make_ray(self, base, direction, length, color, frame='/base_link', _id=100, timestamp=None, **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
marker = visualization_msgs.Marker(
ns='sub',
id=_id,
header=mil_ros_tools.make_header(frame=frame, stamp=timestamp),
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 set_target(self, pos, orientation=(0.0, 0.0, 0.0, 1.0)):
'''TODO:
Add linear/angular velocity
Publish a trajectory
'''
# self.target = make_pose_stamped(position=pos, orientation=orientation)
target_msg = Trajectory(
header=mil_ros_tools.make_header(frame='/body'),
trajectory=[Waypoint(
pose=geometry_msgs.Pose(
position=geometry_msgs.Vector3(*pos),
orientation=geometry_msgs.Quaternion(*orientation)
)
)]
)
self.target_pub.publish(target_msg)
def visualize_pose_est(self):
marker = visualization_msgs.Marker(
ns='torpedo_board/pose_est',
id=0,
header=mil_ros_tools.make_header(frame=self.current_req.frame_id),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(0.0, 1.0, 10, 0.7),
scale=Vector3(0.05, 0.0, 0.0),
lifetime=rospy.Duration())
pts = self.generate_hom_tb_corners_from_cam(self.minimization_result.x)
marker.points.append(Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0]))
marker.points.append(Point(x=pts[0, 1], y=pts[1, 1], z=pts[2, 1]))
marker.points.append(Point(x=pts[0, 2], y=pts[1, 2], z=pts[2, 2]))
marker.points.append(Point(x=pts[0, 3], y=pts[1, 3], z=pts[2, 3]))
marker.points.append(Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0]))
self.marker_pub.publish(marker)
def draw_sphere(self, position, color, scaling=(0.11, 0.11, 0.11), _id=4, frame='/front_stereo'):
pose = Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
id=_id,
header=mil_ros_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(),
)
self.rviz_pub.publish(marker)
def draw_sphere(position, color, scaling=(0.11, 0.11, 0.11), m_id=4, frame='/base_link'):
pose = Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=mil_ros_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 visualize_pose_est(self):
marker = visualization_msgs.Marker(
ns='torpedo_board/pose_est',
id=0,
header=mil_ros_tools.make_header(frame=self.current_req.frame_id),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(0.0, 1.0, 10, 0.7),
scale=Vector3(0.05, 0.0, 0.0),
lifetime=rospy.Duration()
)
pts = self.generate_hom_tb_corners_from_cam(self.minimization_result.x)
marker.points.append(Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0]))
marker.points.append(Point(x=pts[0, 1], y=pts[1, 1], z=pts[2, 1]))
marker.points.append(Point(x=pts[0, 2], y=pts[1, 2], z=pts[2, 2]))
marker.points.append(Point(x=pts[0, 3], y=pts[1, 3], z=pts[2, 3]))
marker.points.append(Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0]))
self.marker_pub.publish(marker)
def depth_callback(self, msg):
'''Handle depth data sent from depth sensor'''
frame = '/depth'
distance = msg.depth
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=(0.0, 1.0, 0.2, 0.7), # green,
frame=frame,
id=0 # Keep these guys from overwriting eachother
)
self.surface_marker.ns = 'sub'
self.surface_marker.header = mil_ros_tools.make_header(frame='/depth')
self.surface_marker.pose = marker.pose
self.surface_marker.text = str(round(distance, 3)) + 'm'
self.surface_marker.id = 0
self.rviz_pub.publish(marker)
self.rviz_pub_t.publish(self.depth_marker)
def depth_callback(self, msg):
'''Handle depth data sent from depth sensor'''
frame = '/depth'
distance = msg.depth
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=(0.0, 1.0, 0.2, 0.7), # green,
frame=frame,
id=0 # Keep these guys from overwriting eachother
)
self.surface_marker.ns = 'sub'
self.surface_marker.header = mil_ros_tools.make_header(frame='/depth')
self.surface_marker.pose = marker.pose
self.surface_marker.text = str(round(distance, 3)) + 'm'
self.surface_marker.id = 0
self.rviz_pub.publish(marker)
self.rviz_pub_t.publish(self.depth_marker)
def publish_dvl(self):
"""Publishes dvl sensor data - twist message, and array of 4 dvl velocities based off of ray orientations"""
correlation = -1
header = mil_ros_tools.make_header(frame='/body')
vel_dvl_body = self.body.vectorFromWorld(
self.body.getRelPointVel(self.dvl_position))
velocity_measurements = []
for ray in self.dvl_ray_orientations:
velocity_measurements.append(
VelocityMeasurement(direction=geometry.Vector3(*ray),
velocity=np.dot(ray, vel_dvl_body),
correlation=correlation))
dvl_msg = VelocityMeasurements(
header=header, velocity_measurements=velocity_measurements)
self.dvl_sensor_pub.publish(dvl_msg)
def publish_imu(self, dt):
"""Publishes imu sensor information - orientation, angular velocity, and linear acceleration"""
orientation_matrix = self.pose[:3, :3]
sigma = 0.01
noise = np.random.normal(0.0, sigma, 3)
# Work on a better way to get this
# We can't get this nicely from body.getForce()
g = self.body.vectorFromWorld(self.g)
# Get current velocity of IMU in body frame
cur_vel = np.array(self.body.vectorFromWorld(self.body.getRelPointVel(self.imu_position)))
linear_acc = orientation_matrix.dot(cur_vel - self.last_vel) / dt
linear_acc += g + (noise * dt)
# TODO: Fix frame
angular_vel = orientation_matrix.dot(self.body.getAngularVel()) + (noise * dt)
header = mil_ros_tools.make_header(frame='/body')
linear = geometry.Vector3(*linear_acc)
angular = geometry.Vector3(*angular_vel)
covariance = [sigma ** 2, 0., 0.,
0., sigma ** 2, 0.,
0., 0., sigma ** 2]
orientation_covariance = [-1, 0., 0.,
0., 0., 0.,
0., 0., 0.]
imu_msg = Imu(
header=header,
angular_velocity=angular,
angular_velocity_covariance=covariance,
linear_acceleration=linear,
linear_acceleration_covariance=covariance,
orientation_covariance=orientation_covariance
)
self.imu_sensor_pub.publish(imu_msg)
def make_cylinder_marker(self, base, length, color, frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]), **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(
position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs
)
return marker
def publish_dvl(self):
"""Publishes dvl sensor data - twist message, and array of 4 dvl velocities based off of ray orientations"""
correlation = -1
header = mil_ros_tools.make_header(frame='/body')
vel_dvl_body = self.body.vectorFromWorld(self.body.getRelPointVel(self.dvl_position))
velocity_measurements = []
for ray in self.dvl_ray_orientations:
velocity_measurements.append(VelocityMeasurement(
direction=geometry.Vector3(*ray),
velocity=np.dot(ray, vel_dvl_body),
correlation=correlation
))
dvl_msg = VelocityMeasurements(
header=header,
velocity_measurements=velocity_measurements
)
self.dvl_sensor_pub.publish(dvl_msg)
def publish_imu(self, dt):
"""Publishes imu sensor information - orientation, angular velocity, and linear acceleration"""
orientation_matrix = self.pose[:3, :3]
sigma = 0.01
noise = np.random.normal(0.0, sigma, 3)
# Work on a better way to get this
# We can't get this nicely from body.getForce()
g = self.body.vectorFromWorld(self.g)
# Get current velocity of IMU in body frame
cur_vel = np.array(
self.body.vectorFromWorld(
self.body.getRelPointVel(self.imu_position)))
linear_acc = orientation_matrix.dot(cur_vel - self.last_vel) / dt
linear_acc += g + (noise * dt)
# TODO: Fix frame
angular_vel = orientation_matrix.dot(
self.body.getAngularVel()) + (noise * dt)
header = mil_ros_tools.make_header(frame='/body')
linear = geometry.Vector3(*linear_acc)
angular = geometry.Vector3(*angular_vel)
covariance = [sigma**2, 0., 0., 0., sigma**2, 0., 0., 0., sigma**2]
orientation_covariance = [-1, 0., 0., 0., 0., 0., 0., 0., 0.]
imu_msg = Imu(header=header,
angular_velocity=angular,
angular_velocity_covariance=covariance,
linear_acceleration=linear,
linear_acceleration_covariance=covariance,
orientation_covariance=orientation_covariance)
self.imu_sensor_pub.publish(imu_msg)
def publish_pose(self):
'''TODO:
Publish velocity in body frame
'''
linear_vel = self.body.getRelPointVel((0.0, 0.0, 0.0))
# TODO: Not 100% on this transpose
angular_vel = self.orientation.dot(self.angular_vel)
# angular_vel = self.orientation.transpose().dot(self.angular_vel)
quaternion = self.body.getQuaternion()
translation = self.body.getPosition()
header = mil_ros_tools.make_header(frame='/map')
pose = geometry.Pose(
position=geometry.Point(*translation),
orientation=geometry.Quaternion(quaternion[1], quaternion[2], quaternion[3], quaternion[0]),
)
twist = geometry.Twist(
linear=geometry.Vector3(*linear_vel),
angular=geometry.Vector3(*angular_vel)
)
odom_msg = Odometry(
header=header,
child_frame_id='/base_link',
pose=geometry.PoseWithCovariance(
pose=pose
),
twist=geometry.TwistWithCovariance(
twist=twist
)
)
self.truth_odom_pub.publish(odom_msg)
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 = 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(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):
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(point_to_numpy, 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] - point_to_numpy(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)
#!/usr/bin/env python
import rospy
import numpy as np
import tf
import mil_ros_tools
from sensor_msgs.msg import PointCloud
rospy.init_node("ground_finder")
pub = rospy.Publisher("ground_est", PointCloud, queue_size=1)
listener = tf.TransformListener()
pc = PointCloud()
pc.header = mil_ros_tools.make_header(frame="map")
pc.points = []
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
try:
t = listener.waitForTransform('/map', '/ground', rospy.Time.now(),
rospy.Duration(1))
(trans, rot) = listener.lookupTransform('/map', '/ground',
rospy.Time(0))
except (tf.Exception, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logwarn("TF waitForTransform timeout")
continue
pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans)))
pub.publish(pc)
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 = 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(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):
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(point_to_numpy, 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] -
point_to_numpy(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)
#!/usr/bin/env python
import rospy
import numpy as np
import tf
import mil_ros_tools
from sensor_msgs.msg import PointCloud
rospy.init_node("ground_finder")
pub = rospy.Publisher("ground_est", PointCloud, queue_size=1)
listener = tf.TransformListener()
pc = PointCloud()
pc.header = mil_ros_tools.make_header(frame="map")
pc.points = []
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
try:
t = listener.waitForTransform('/map', '/ground', rospy.Time.now(), rospy.Duration(1))
(trans, rot) = listener.lookupTransform('/map', '/ground', rospy.Time(0))
except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("TF waitForTransform timeout")
continue
pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans)))
pub.publish(pc)
rate.sleep()
