MIL COMMON: clean redundant packages and prepare for great merge & sonar OS

command/sub8_alarm/alarm_handlers/height_over_bottom.py

@@ -1,6 +1,6 @@
 import rospy
 from ros_alarms import AlarmBroadcaster, HandlerBase
-from uf_common.msg import Float64Stamped
+from mil_msgs.msg import RangeStamped
 
 class HeightOverBottom(HandlerBase):
     alarm_name = "height-over-bottom"
@@ -13,8 +13,8 @@ def __init__(self):
 
         # Keep track of the current height
         self._last_height = 100 
-        set_last_height = lambda msg: setattr(self, "_last_height", msg.data)
-        rospy.Subscriber("/dvl/range", Float64Stamped, set_last_height)
+        set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
+        rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)
 
         # Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
         rospy.Timer(rospy.Duration(5), self._update_height)

command/sub8_launch/nodes/self_check.py

@@ -1,6 +1,6 @@
 #!/usr/bin/python
 import txros
-from sub8_tools import text_effects
+from mil_misc_tools import text_effects
 from twisted.internet import defer, reactor
 import sys
 
@@ -148,16 +148,16 @@ def do_check(self):
 
 from nav_msgs.msg import Odometry
 from tf.msg import tfMessage
-from uf_common.msg import VelocityMeasurements, Float64Stamped
+from mil_msgs.msg import VelocityMeasurements, RangeStamped, DepthStamped
 from sensor_msgs.msg import Imu, MagneticField
 class StateEstChecker(TemplateChecker):
     @txros.util.cancellableInlineCallbacks
     def tx_init(self):
         self.odom = self.nh.subscribe("/odom", Odometry)
         self.tf = self.nh.subscribe("/tf", tfMessage)
         self.dvl = self.nh.subscribe("/dvl", VelocityMeasurements)
-        self.depth = self.nh.subscribe("/depth", Float64Stamped)
-        self.height = self.nh.subscribe("/dvl/range", Float64Stamped)
+        self.depth = self.nh.subscribe("/depth", DepthStamped)
+        self.height = self.nh.subscribe("/dvl/range", RangeStamped)
         self.imu = self.nh.subscribe("/imu/data_raw", Imu)
         self.mag = self.nh.subscribe("/imu/mag", MagneticField)
 

command/sub8_launch/scripts/tf_republisher.py

@@ -2,7 +2,7 @@
 
 import rospy
 import tf
-from uf_common.msg import Float64Stamped
+from mil_msgs.msg import RangeStamped
 
 
 def got_range(msg):
@@ -25,6 +25,6 @@ def got_range(msg):
     listener = tf.TransformListener()
     rospy.sleep(1)
 
-    sub = rospy.Subscriber('/dvl/range', Float64Stamped, got_range)
+    sub = rospy.Subscriber('/dvl/range', RangeStamped, got_range)
 
     rospy.spin()

command/sub8_missions/missions/align_path_marker.py

@@ -5,8 +5,8 @@
 from twisted.internet import defer
 from txros import util
 from sub8 import Searcher
-from sub8_ros_tools import pose_to_numpy, rosmsg_to_numpy
-from sub8_misc_tools import text_effects
+from mil_ros_tools import pose_to_numpy, rosmsg_to_numpy
+from mil_misc_tools import text_effects
 import numpy as np
 
 SEARCH_DEPTH = .65

command/sub8_missions/missions/autonomous.py

@@ -1,7 +1,7 @@
 import txros
 from twisted.internet import defer
 from ros_alarms import TxAlarmListener, TxAlarmBroadcaster
-from sub8_tools import text_effects 
+from mil_misc_tools import text_effects 
 
 # Import missions here
 import square

command/sub8_missions/missions/buoy.py

@@ -2,7 +2,7 @@
 from txros import util, tf
 import numpy as np
 from std_srvs.srv import SetBool, SetBoolRequest
-from sub8_ros_tools import pose_to_numpy
+from mil_ros_tools import pose_to_numpy
 
 SPEED = .3
 SEARCH = 0

command/sub8_missions/missions/start_gate.py

@@ -6,8 +6,8 @@
 
 from tf import transformations
 
-from sub8 import pose_editor
-import sub8_tools
+from mil_msgs.msg import MoveToAction, PoseTwistStamped
+from mil_misc_tools import text_effects
 from sub8_msgs.srv import VisionRequest, VisionRequestRequest, VisionRequest2DRequest, VisionRequest2D, BMatrix, BMatrixRequest
 from std_srvs.srv import SetBool, SetBoolRequest
 from nav_msgs.msg import Odometry
@@ -17,10 +17,7 @@
 import os
 import yaml
 
-from sub8 import sub_singleton
-
-
-fprint = sub8_tools.text_effects.FprintFactory(
+fprint = text_effects.FprintFactory(
 title="START_GATE", msg_color="white").fprint
 
 class StartGateMission(object):
@@ -121,4 +118,4 @@ def align_for_dummies(self, start_gate_search_res):
 @txros.util.cancellableInlineCallbacks
 def run(sub_singleton):
     start_gate_mission = StartGateMission(sub_singleton)
-    yield start_gate_mission.run_mission()
+    yield start_gate_mission.run_mission()

command/sub8_missions/nodes/move_command

@@ -3,7 +3,7 @@ from txros import util, NodeHandle
 from twisted.internet import defer, reactor
 
 import numpy as np
-import sub8_tools
+from mil_misc_tools import text_effects
 from sub8 import sub_singleton
 from geometry_msgs.msg import PoseStamped, PointStamped
 import missions
@@ -15,7 +15,7 @@ SHORTHAND = {"f": "forward", "b": "backward", "l": "left", "r": "right", "yl": "
              "d": "down", "u": "up"}
 
 ros_t = lambda d: util.genpy.Duration(d)
-fprint = sub8_tools.text_effects.FprintFactory(title="MOVE_COMMAND").fprint
+fprint = text_effects.FprintFactory(title="MOVE_COMMAND").fprint
 
 @util.cancellableInlineCallbacks
 def main(args):

command/sub8_missions/nodes/run_mission

@@ -6,12 +6,12 @@ import argparse
 from twisted.internet import defer, reactor
 import exceptions
 import txros
-import sub8_tools
+from mil_misc_tools import text_effects
 from sub8 import sub_singleton
 import missions
 
 
-fprint = sub8_tools.text_effects.FprintFactory(title="MISSION_RUNNER").fprint
+fprint = text_effects.FprintFactory(title="MISSION_RUNNER").fprint
 
 @txros.util.cancellableInlineCallbacks
 def main():

command/sub8_missions/sub8/pose_editor.py

@@ -5,10 +5,10 @@
 import rospy
 from tf import transformations
 from nav_msgs.msg import Odometry
-from uf_common.msg import PoseTwistStamped, PoseTwist, MoveToGoal
+from mil_msgs.msg import PoseTwistStamped, PoseTwist, MoveToGoal
 from std_msgs.msg import Header
 from geometry_msgs.msg import Pose as Pose, Quaternion, Point, Vector3, Twist
-from sub8_ros_tools import rosmsg_to_numpy
+from mil_ros_tools import rosmsg_to_numpy
 
 
 UP = np.array([0.0, 0.0, 1.0], np.float64)

command/sub8_missions/sub8/sub_singleton.py

@@ -6,9 +6,9 @@
 import rospkg
 from tf import transformations
 
-from uf_common.msg import MoveToAction, PoseTwistStamped, Float64Stamped
+from mil_msgs.msg import MoveToAction, PoseTwistStamped, RangeStamped
 from sub8 import pose_editor
-import sub8_tools
+import mil_ros_tools
 from sub8_msgs.srv import VisionRequest, VisionRequestRequest, VisionRequest2DRequest, VisionRequest2D
 from std_srvs.srv import SetBool, SetBoolRequest
 from nav_msgs.msg import Odometry
@@ -122,7 +122,7 @@ def sub_attr_proxy(*args, **kwargs):
     # Some special moves
     def to_height(self, height):
         dist_to_bot = self._sub._dvl_range_sub.get_last_message()
-        delta_height = dist_to_bot.data - height
+        delta_height = dist_to_bot.range - height
         return self.down(delta_height)
 
     def check_goal(self):
@@ -163,7 +163,7 @@ def _init(self):
         self._odom_sub = yield self.nh.subscribe('odom', Odometry)
         self._trajectory_sub = yield self.nh.subscribe('trajectory', PoseTwistStamped)
         self._trajectory_pub = yield self.nh.advertise('trajectory', PoseTwistStamped)
-        self._dvl_range_sub = yield self.nh.subscribe('dvl/range', Float64Stamped)
+        self._dvl_range_sub = yield self.nh.subscribe('dvl/range', RangeStamped)
         self._tf_listener = yield tf.TransformListener(self.nh)
 
         self.vision_proxies = _VisionProxies(self.nh, 'vision_proxies.yaml')
@@ -186,11 +186,11 @@ def tx_pose(self):
         '''Slighty safer to use.'''
         if self.test_mode:
             yield self.nh.sleep(.1)
-            blank = sub8_tools.pose_to_numpy(Odometry().pose.pose)
+            blank = mil_ros_tools.pose_to_numpy(Odometry().pose.pose)
             defer.returnValue(blank)
 
         next_odom_msg = yield self._odom_sub.get_next_message()
-        pose = sub8_tools.pose_to_numpy(next_odom_msg.pose.pose)
+        pose = mil_ros_tools.pose_to_numpy(next_odom_msg.pose.pose)
         defer.returnValue(pose)
 
     @property
@@ -200,7 +200,7 @@ def move(self):
     @util.cancellableInlineCallbacks
     def get_dvl_range(self):
         msg = yield self._dvl_range_sub.get_next_message()
-        defer.returnValue(msg.data)
+        defer.returnValue(msg.range)
 
     @util.cancellableInlineCallbacks
     def get_in_frame(self, pose_stamped, frame='/map'):

command/sub8_missions/tools/rviz_visualizer.py

@@ -9,10 +9,10 @@
 
 from geometry_msgs.msg import Pose, Vector3
 from std_msgs.msg import ColorRGBA, Float64
-from uf_common.msg import Float64Stamped  # This needs to be deprecated
+from mil_msgs.msg import RangeStamped, DepthStamped
 #from sub8_alarm import AlarmListener, AlarmBroadcaster
 from ros_alarms import AlarmBroadcaster, AlarmListener
-import sub8_ros_tools as sub8_utils
+import mil_ros_tools
 
 
 class RvizVisualizer(object):
@@ -78,9 +78,9 @@ def __init__(self):
         self.kill_alarm = AlarmBroadcaster("kill")
 
         # distance to bottom
-        self.range_sub = rospy.Subscriber("dvl/range", Float64Stamped, self.range_callback)
+        self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
         # distance to surface
-        self.depth_sub = rospy.Subscriber("depth", Float64Stamped, self.depth_callback)
+        self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
         # battery voltage
         self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
 
@@ -130,7 +130,7 @@ def voltage_callback(self, voltage):
     def depth_callback(self, msg):
         '''Handle depth data sent from depth sensor'''
         frame = '/depth'
-        distance = msg.data
+        distance = msg.depth
         marker = self.make_cylinder_marker(
             np.array([0.0, 0.0, 0.0]),  # place at origin
             length=distance,
@@ -139,7 +139,7 @@ def depth_callback(self, msg):
             id=0  # Keep these guys from overwriting eachother
         )
         self.surface_marker.ns='sub'
-        self.surface_marker.header = sub8_utils.make_header(frame='/depth')
+        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
@@ -151,7 +151,7 @@ def range_callback(self, msg):
         '''Handle range data grabbed from dvl'''
         # future: should be /base_link/dvl, no?
         frame = '/dvl'
-        distance = msg.data
+        distance = msg.range
 
         # Color a sharper red if we're in danger
         if distance < 1.0:
@@ -168,7 +168,7 @@ def range_callback(self, msg):
             id=1  # Keep these guys from overwriting eachother
         )
         self.depth_marker.ns='sub'
-        self.depth_marker.header = sub8_utils.make_header(frame='/dvl')
+        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
@@ -182,13 +182,13 @@ def make_cylinder_marker(self, base, length, color, frame='/base_link', up_vecto
         center = base + (up_vector * (length / 2))
 
         pose = Pose(
-            position=sub8_utils.numpy_to_point(center),
-            orientation=sub8_utils.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
+            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=sub8_utils.make_header(frame=frame),
+            header=mil_ros_tools.make_header(frame=frame),
             type=visualization_msgs.Marker.CYLINDER,
             action=visualization_msgs.Marker.ADD,
             pose=pose,

command/sub8_missions/tools/waypoint_utility.py

@@ -2,7 +2,7 @@
 import sys
 import rospy
 import nav_msgs.msg as nav_msgs
-import sub8_ros_tools as sub8_utils
+import mil_ros_tools
 import tf
 import numpy as np
 
@@ -12,7 +12,7 @@
 from scipy import linalg
 
 import actionlib
-import uf_common.msg as uf_common_msgs
+import mil_msgs.msg as mil_msgs
 import geometry_msgs.msg as geom_msgs
 
 
@@ -55,7 +55,7 @@ def __init__(self):
         self.target_orientation = np.eye(3)
         self.target_orientation_quaternion = np.array([0.0, 0.0, 0.0, 1.0])
 
-        self.client = actionlib.SimpleActionClient('/moveto', uf_common_msgs.MoveToAction)
+        self.client = actionlib.SimpleActionClient('/moveto', mil_msgs.MoveToAction)
         # self.client.wait_for_server()
 
         self.target_pose_pub = rospy.Publisher('/posegoal', PoseStamped, queue_size=1)
@@ -66,7 +66,7 @@ def __init__(self):
 
     def odom_cb(self, msg):
         '''HACK: Intermediate hack until we have tf set up'''
-        pose, twist, _, _ = sub8_utils.odometry_to_numpy(msg)
+        pose, twist, _, _ = mil_ros_tools.odometry_to_numpy(msg)
         position, orientation = pose
         self.world_to_body = self.transformer.fromTranslationRotation(position, orientation)[:3, :3]
 
@@ -77,13 +77,13 @@ def odom_cb(self, msg):
     def twist_cb(self, msg):
         if self.cur_orientation is None or self.cur_position is None:
             return
-        linear = sub8_utils.rosmsg_to_numpy(msg.linear)
-        angular = sub8_utils.rosmsg_to_numpy(msg.angular)
+        linear = mil_ros_tools.rosmsg_to_numpy(msg.linear)
+        angular = mil_ros_tools.rosmsg_to_numpy(msg.angular)
         self.target_position += self.target_orientation.dot(self._position_gain * linear)
         self.diff_position = np.subtract(self.cur_position, self.target_position)
 
         gained_angular = self._orientation_gain * angular
-        skewed = sub8_utils.skew_symmetric_cross(gained_angular)
+        skewed = mil_ros_tools.skew_symmetric_cross(gained_angular)
         rotation = linalg.expm(skewed)
 
         # TODO: Better
@@ -103,19 +103,19 @@ def twist_cb(self, msg):
     def publish_target_pose(self, position, orientation):
         self.target_pose_pub.publish(
             PoseStamped(
-                header=sub8_utils.make_header('/map'),
+                header=mil_ros_tools.make_header('/map'),
                 pose=Pose(
-                    position=sub8_utils.numpy_to_point(position),
-                    orientation=sub8_utils.numpy_to_quaternion(orientation)
+                    position=mil_ros_tools.numpy_to_point(position),
+                    orientation=mil_ros_tools.numpy_to_quaternion(orientation)
                 )
             )
         )
-        self.distance_marker.header = sub8_utils.make_header('/map')
+        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=sub8_utils.numpy_to_point(position),
-                    orientation=sub8_utils.numpy_to_quaternion(orientation)
+                    position=mil_ros_tools.numpy_to_point(position),
+                    orientation=mil_ros_tools.numpy_to_quaternion(orientation)
                 )
         self.target_distance_pub.publish(self.distance_marker)
 
@@ -140,12 +140,12 @@ def moveto_action(self, position, orientation):
             rospy.logwarn("Waypoint set too high!")
             position[2] = -0.5
 
-        goal = uf_common_msgs.MoveToGoal(
-            header=sub8_utils.make_header('/map'),
-            posetwist=uf_common_msgs.PoseTwist(
+        goal = mil_msgs.MoveToGoal(
+            header=mil_ros_tools.make_header('/map'),
+            posetwist=mil_msgs.PoseTwist(
                 pose=geom_msgs.Pose(
-                    position=sub8_utils.numpy_to_point(position),
-                    orientation=sub8_utils.numpy_to_quaternion(orientation)
+                    position=mil_ros_tools.numpy_to_point(position),
+                    orientation=mil_ros_tools.numpy_to_quaternion(orientation)
                 )
             ),
             speed=0.2,