diff --git a/command/sub8_missions/tools/rviz_visualizer.py b/command/sub8_missions/tools/rviz_visualizer.py index b8995104..691178e8 100755 --- a/command/sub8_missions/tools/rviz_visualizer.py +++ b/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, diff --git a/command/sub8_missions/tools/waypoint_utility.py b/command/sub8_missions/tools/waypoint_utility.py index f5f02339..f5b8aefc 100755 --- a/command/sub8_missions/tools/waypoint_utility.py +++ b/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 @@ -12,7 +12,7 @@ from scipy import linalg import actionlib -import mil_msgs.msg as mil_msgs_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', mil_msgs_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 = mil_msgs_msgs.MoveToGoal( - header=sub8_utils.make_header('/map'), - posetwist=mil_msgs_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, diff --git a/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config b/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config index 1dbfcdd4..66d5f4bd 100755 --- a/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config +++ b/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): diff --git a/gnc/sub8_controller/nodes/base_controller.py b/gnc/sub8_controller/nodes/base_controller.py index 89334fb0..e49a4e10 100755 --- a/gnc/sub8_controller/nodes/base_controller.py +++ b/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" diff --git a/perception/sub8_perception/nodes/torpedo_board_pose_est.py b/perception/sub8_perception/nodes/torpedo_board_pose_est.py index 93b4eaee..5e2de9fb 100755 --- a/perception/sub8_perception/nodes/torpedo_board_pose_est.py +++ b/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), diff --git a/perception/sub8_perception/sub8_vision_tools/rviz.py b/perception/sub8_perception/sub8_vision_tools/rviz.py index 74a8d8b6..b0741a5d 100644 --- a/perception/sub8_perception/sub8_vision_tools/rviz.py +++ b/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), diff --git a/simulation/sub8_gazebo/nodes/gazebo_controller.py b/simulation/sub8_gazebo/nodes/gazebo_controller.py index 8f408a2d..1f351181 100755 --- a/simulation/sub8_gazebo/nodes/gazebo_controller.py +++ b/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 diff --git a/simulation/sub8_montecarlo/sub8_montecarlo_tools/controller_verify.py b/simulation/sub8_montecarlo/sub8_montecarlo_tools/controller_verify.py index d949fd2e..a0add0c9 100644 --- a/simulation/sub8_montecarlo/sub8_montecarlo_tools/controller_verify.py +++ b/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))) diff --git a/simulation/sub8_simulation/nodes/spacenav_remap.py b/simulation/sub8_simulation/nodes/spacenav_remap.py index f5055787..f0fa4424 100755 --- a/simulation/sub8_simulation/nodes/spacenav_remap.py +++ b/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) ) diff --git a/simulation/sub8_simulation/sub8_sim_tools/physics/vehicle.py b/simulation/sub8_simulation/sub8_sim_tools/physics/vehicle.py index 09e19424..373d3ab2 100644 --- a/simulation/sub8_simulation/sub8_sim_tools/physics/vehicle.py +++ b/simulation/sub8_simulation/sub8_sim_tools/physics/vehicle.py @@ -1,6 +1,6 @@ import numpy as np import rospy -import mil_ros_tools as sub8_utils +import mil_ros_tools from sub8_sim_tools.physics.physics import Entity from sub8_simulation.srv import SimSetPose, SimSetPoseResponse from sub8_msgs.msg import Thrust, VelocityMeasurement, VelocityMeasurements @@ -134,21 +134,21 @@ def set_pose_server(self, srv): TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id ''' rospy.logwarn("Manually setting position of simulated vehicle") - position = sub8_utils.rosmsg_to_numpy(srv.pose.position) + position = mil_ros_tools.rosmsg_to_numpy(srv.pose.position) self.body.setPosition(position) - rotation_q = sub8_utils.rosmsg_to_numpy(srv.pose.orientation) + rotation_q = mil_ros_tools.rosmsg_to_numpy(srv.pose.orientation) rotation_norm = np.linalg.norm(rotation_q) - velocity = sub8_utils.rosmsg_to_numpy(srv.twist.linear) - angular = sub8_utils.rosmsg_to_numpy(srv.twist.angular) + velocity = mil_ros_tools.rosmsg_to_numpy(srv.twist.linear) + angular = mil_ros_tools.rosmsg_to_numpy(srv.twist.angular) self.body.setLinearVel(velocity) self.body.setAngularVel(angular) # If the commanded rotation is valid if np.isclose(rotation_norm, 1., atol=1e-2): - self.body.setQuaternion(sub8_utils.normalize(rotation_q)) + self.body.setQuaternion(mil_ros_tools.normalize(rotation_q)) else: rospy.logwarn("Commanded quaternion was not a unit quaternion, NOT setting rotation") @@ -167,7 +167,7 @@ def publish_pose(self): translation = self.body.getPosition() - header = sub8_utils.make_header(frame='/map') + header = mil_ros_tools.make_header(frame='/map') pose = geometry.Pose( position=geometry.Point(*translation), @@ -209,7 +209,7 @@ def publish_imu(self, dt): # TODO: Fix frame angular_vel = orientation_matrix.dot(self.body.getAngularVel()) + (noise * dt) - header = sub8_utils.make_header(frame='/body') + header = mil_ros_tools.make_header(frame='/body') linear = geometry.Vector3(*linear_acc) angular = geometry.Vector3(*angular_vel) @@ -237,7 +237,7 @@ def publish_dvl(self): """Publishes dvl sensor data - twist message, and array of 4 dvl velocities based off of ray orientations""" correlation = -1 - header = sub8_utils.make_header(frame='/body') + header = mil_ros_tools.make_header(frame='/body') vel_dvl_body = self.body.vectorFromWorld(self.body.getRelPointVel(self.dvl_position))