MIL COMMON: fix imports of sub8_utils

command/sub8_missions/tools/rviz_visualizer.py

@@ -12,7 +12,7 @@
 from mil_msgs.msg import RangeStamped, DepthStamped
 #from sub8_alarm import AlarmListener, AlarmBroadcaster
 from ros_alarms import AlarmBroadcaster, AlarmListener
-import mil_ros_tools as sub8_utils
+import mil_ros_tools
 
 
 class RvizVisualizer(object):
@@ -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
@@ -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 mil_ros_tools as sub8_utils
+import mil_ros_tools
 import tf
 import numpy as np
 
@@ -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)
 
@@ -141,11 +141,11 @@ def moveto_action(self, position, orientation):
             position[2] = -0.5
 
         goal = mil_msgs_msgs.MoveToGoal(
-            header=sub8_utils.make_header('/map'),
+            header=mil_ros_tools.make_header('/map'),
             posetwist=mil_msgs_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,

drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config

@@ -10,7 +10,7 @@ import yaml
 import roslib
 import rosbag
 from tf import transformations
-import mil_ros_tools as sub8_utils
+import mil_ros_tools
 roslib.load_manifest('magnetic_hardsoft_compensation')
 
 
@@ -116,8 +116,8 @@ if __name__ == '__main__':
 
     with rosbag.Bag(sys.argv[1]) as bag:
         points = numpy.array([
-            sub8_utils.rosmsg_to_numpy(msg.magnetic_field) if hasattr(msg, 'magnetic_field') else
-            sub8_utils.rosmsg_to_numpy(msg.vector)
+            mil_ros_tools.rosmsg_to_numpy(msg.magnetic_field) if hasattr(msg, 'magnetic_field') else
+            mil_ros_tools.rosmsg_to_numpy(msg.vector)
             for topic, msg, t in bag.read_messages(topics=['/imu/mag_raw'])])
 
     if not len(points):

gnc/sub8_controller/nodes/base_controller.py

@@ -9,7 +9,7 @@
 from scipy import linalg
 from sub8_simulation.srv import SimSetPose
 import nav_msgs.msg as nav_msgs
-import mil_ros_tools as sub8_utils
+import mil_ros_tools
 from dynamic_reconfigure.server import Server
 from sub8_controller.cfg import GainConfig  # Configuration file
 
@@ -70,7 +70,7 @@ def start(self):
     def stop(self):
         self.control_manager.shutdown()
 
-    @sub8_utils.thread_lock(lock)
+    @mil_ros_tools.thread_lock(lock)
     def dynamic_reconfig(self, config, level):
         rospy.logwarn("Reconfiguring contoller gains at level {}".format(level))
         self.cfg = config
@@ -100,7 +100,7 @@ def trajectory_cb(self, msg):
         }
         for struct_waypoint in msg.trajectory:
             # Deserialize to numpy
-            pose, twist = sub8_utils.posetwist_to_numpy(struct_waypoint)
+            pose, twist = mil_ros_tools.posetwist_to_numpy(struct_waypoint)
             (position, orientation_q), (linear_vel, angular_vel) = pose, twist
 
             # TODO: Less repeated code
@@ -116,7 +116,7 @@ def odom_cb(self, msg):
         '''
 
         # Using ropsy.Time.now() so we can use simulated accelerated time (not CPU time)
-        pose, twist, pose_cov, twist_cov = sub8_utils.odometry_to_numpy(msg)
+        pose, twist, pose_cov, twist_cov = mil_ros_tools.odometry_to_numpy(msg)
         (position, orientation_q), (linear_vel, angular_vel) = pose, twist
 
         self.current_state = {
@@ -162,7 +162,7 @@ def send_wrench(self, force, torque):
             (Are you supposed to say Newtons-meter? Newtons-Meters?)
         '''
         wrench_msg = geometry_msgs.WrenchStamped(
-            header=sub8_utils.make_header(),
+            header=mil_ros_tools.make_header(),
             wrench=geometry_msgs.Wrench(
                 force=geometry_msgs.Vector3(*force),
                 torque=geometry_msgs.Vector3(*torque)
@@ -183,11 +183,11 @@ def orientation_error(self, q_a, q_b):
             # No error!
             return np.array([0.0, 0.0, 0.0])
 
-        angle_axis = sub8_utils.deskew(lie_alg_error)
+        angle_axis = mil_ros_tools.deskew(lie_alg_error)
         assert np.linalg.norm(angle_axis) < (2 * np.pi) + 0.01, "uh-oh, unnormalized {}".format(angle_axis)
         return angle_axis
 
-    @sub8_utils.thread_lock(lock)
+    @mil_ros_tools.thread_lock(lock)
     def run(self, time):
         '''
             Do the bookkeeping before calling "execute"

perception/sub8_perception/nodes/torpedo_board_pose_est.py

@@ -8,7 +8,7 @@
 import visualization_msgs.msg as visualization_msgs
 from geometry_msgs.msg import Point, Vector3, Pose, Quaternion
 from std_msgs.msg import ColorRGBA
-import mil_ros_tools as sub8_utils
+import mil_ros_tools
 
 
 class PoseObserver(object):
@@ -131,7 +131,7 @@ def visualize_pose_est(self):
         marker = visualization_msgs.Marker(
             ns='torpedo_board/pose_est',
             id=0,
-            header=sub8_utils.make_header(frame=self.current_req.frame_id),
+            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),

perception/sub8_perception/sub8_vision_tools/rviz.py

@@ -6,7 +6,7 @@
 import visualization_msgs.msg as visualization_msgs
 from geometry_msgs.msg import Pose, Vector3, Point
 from std_msgs.msg import ColorRGBA
-import mil_ros_tools as sub8_utils
+import mil_ros_tools
 
 
 class RvizVisualizer(object):
@@ -18,14 +18,14 @@ def __init__(self):
 
     def draw_sphere(self, position, color, scaling=(0.11, 0.11, 0.11), _id=4, frame='/front_stereo'):
         pose = Pose(
-            position=sub8_utils.numpy_to_point(position),
-            orientation=sub8_utils.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
+            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=sub8_utils.make_header(frame=frame),
+            header=mil_ros_tools.make_header(frame=frame),
             type=visualization_msgs.Marker.SPHERE,
             action=visualization_msgs.Marker.ADD,
             pose=pose,
@@ -55,7 +55,7 @@ def make_ray(self, base, direction, length, color, frame='/base_link', _id=100,
         marker = visualization_msgs.Marker(
             ns='sub',
             id=_id,
-            header=sub8_utils.make_header(frame=frame, stamp=timestamp),
+            header=mil_ros_tools.make_header(frame=frame, stamp=timestamp),
             type=visualization_msgs.Marker.LINE_STRIP,
             action=visualization_msgs.Marker.ADD,
             color=ColorRGBA(*color),

simulation/sub8_gazebo/nodes/gazebo_controller.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 import rospy
-import mil_ros_tools as sub8_utils
+import mil_ros_tools
 from tf import transformations
 from nav_msgs.msg import Odometry
 from geometry_msgs.msg import WrenchStamped, PoseWithCovariance, TwistWithCovariance, Pose, Point, Quaternion
@@ -63,7 +63,7 @@ def publish_height(self, msg):
         '''Sim DVL uses laserscan message to relay height'''
         self.dvl_pub.publish(
             RangeStamped(
-                header=sub8_utils.make_header(),
+                header=mil_ros_tools.make_header(),
                 range=float(np.mean(msg.ranges))
             )
         )
@@ -74,14 +74,14 @@ def publish_odom(self, *args):
 
         msg = self.last_odom
         if self.target in msg.name:
-            header = sub8_utils.make_header(frame='/map')
+            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 = sub8_utils.pose_to_numpy(msg.pose[target_index])
-            pose = sub8_utils.numpy_quat_pair_to_pose(position_np - self.position_offset, orientation_np)
+            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,
@@ -94,7 +94,7 @@ def publish_odom(self, *args):
                 )
             )
 
-            header = sub8_utils.make_header(frame='/world')
+            header = mil_ros_tools.make_header(frame='/world')
             twist = msg.twist[target_index]
             pose = msg.pose[target_index]
             self.world_state_pub.publish(
@@ -108,7 +108,7 @@ def publish_odom(self, *args):
                 )
             )
 
-            dist = np.linalg.norm(sub8_utils.twist_to_numpy(twist)) * self.odom_freq
+            dist = np.linalg.norm(mil_ros_tools.twist_to_numpy(twist)) * self.odom_freq
 
         else:
             # fail
@@ -126,7 +126,7 @@ def state_cb(self, msg):
 
         if (self.last_odom is None or self.position_offset is None):
             pose = msg.pose[msg.name.index(self.target)]
-            position, orientation = sub8_utils.pose_to_numpy(pose)
+            position, orientation = mil_ros_tools.pose_to_numpy(pose)
 
             self.position_offset = position
             self.position_offset[2] = 0

simulation/sub8_montecarlo/sub8_montecarlo_tools/controller_verify.py

@@ -4,7 +4,7 @@
 import rospy
 import numpy as np
 import time
-import mil_ros_tools as sub8_utils
+import mil_ros_tools
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D  # noqa
 
@@ -76,7 +76,7 @@ def set_target(self, pos, orientation=(0.0, 0.0, 0.0, 1.0)):
         '''
         # self.target = make_pose_stamped(position=pos, orientation=orientation)
         target_msg = Trajectory(
-            header=sub8_utils.make_header(frame='/body'),
+            header=mil_ros_tools.make_header(frame='/body'),
             trajectory=[Waypoint(
                 pose=geometry_msgs.Pose(
                     position=geometry_msgs.Vector3(*pos),
@@ -87,7 +87,7 @@ def set_target(self, pos, orientation=(0.0, 0.0, 0.0, 1.0)):
         self.target_pub.publish(target_msg)
 
     def traj_cb(self, msg):
-        pose, twist = sub8_utils.posetwist_to_numpy(msg.trajectory[0])
+        pose, twist = mil_ros_tools.posetwist_to_numpy(msg.trajectory[0])
         self.target_state = pose[0]
 
     def odom_cb(self, msg):
@@ -97,7 +97,7 @@ def odom_cb(self, msg):
         else:
             self.last_sample = time_of
 
-        pose, twist, _, _ = sub8_utils.odometry_to_numpy(msg)
+        pose, twist, _, _ = mil_ros_tools.odometry_to_numpy(msg)
         position, orientation = pose
         linear, angular = twist
         self.cur_state_history.append(np.hstack((position, linear)))

simulation/sub8_simulation/nodes/spacenav_remap.py

@@ -7,7 +7,7 @@
 import numpy as np
 from geometry_msgs.msg import Twist, WrenchStamped
 import nav_msgs.msg as nav_msgs
-import mil_ros_tools as sub8_utils
+import mil_ros_tools
 
 
 class Spacenav(object):
@@ -34,20 +34,20 @@ 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]
 
     def twist_cb(self, msg):
-        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)
 
         if self.behavior == 'wrench':
             # Directly transcribe linear and angular 'velocities' to torque
             # TODO: Setup actual TF!
 
             self.wrench_pub.publish(
-                sub8_utils.make_wrench_stamped(
+                mil_ros_tools.make_wrench_stamped(
                     force=self.linear_gain * self.world_to_body.dot(linear),
                     torque=self.angular_gain * self.world_to_body.dot(angular)
                 )