def tx_pose(self):
'''Slighty safer to use.'''
if self.test_mode:
yield self.nh.sleep(.1)
blank = mil_ros_tools.pose_to_numpy(Odometry().pose.pose)
defer.returnValue(blank)
next_odom_msg = yield self._odom_sub.get_next_message()
pose = mil_ros_tools.pose_to_numpy(next_odom_msg.pose.pose)
defer.returnValue(pose)
def tx_pose(self):
'''Slighty safer to use.'''
if self.test_mode:
yield self.nh.sleep(.1)
blank = mil_ros_tools.pose_to_numpy(Odometry().pose.pose)
defer.returnValue(blank)
next_odom_msg = yield self._odom_sub.get_next_message()
pose = mil_ros_tools.pose_to_numpy(next_odom_msg.pose.pose)
defer.returnValue(pose)
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 sanity_check(sub, est_pos):
yield None
est_pos_np = pose_to_numpy(est_pos.pose)[0]
print(est_pos_np == np.array([5, 5, 5])).all()
if (est_pos_np == np.array([5, 5, 5])).all():
print "BUOY MISSION - Problem with guess."
defer.returnValue(None)
if est_pos_np[2] > -0.2:
print 'BUOY MISSION - Detected buoy above the water'
defer.returnValue(None)
defer.returnValue(True)
def state_cb(self, msg):
'''
Position is offset so first message is taken as zero point. (More reflective of actual sub).
Z position is absolute and so is rotation.
TODO: Add noise
'''
if self.target not in msg.name:
return
if (self.last_odom is None or self.position_offset is None):
pose = msg.pose[msg.name.index(self.target)]
position, orientation = mil_ros_tools.pose_to_numpy(pose)
position[2] = 0.0
self.position_offset = position
self.last_odom = msg
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 set_odom(msg):
return setattr(self, "odom", pose_to_numpy(msg.pose.pose))
def set_odom(msg):
return setattr(self, "odom", pose_to_numpy(msg.pose.pose))