diff --git a/command/sub8_alarm/alarm_handlers/height_over_bottom.py b/command/sub8_alarm/alarm_handlers/height_over_bottom.py index e06c42c5..0cb81950 100644 --- a/command/sub8_alarm/alarm_handlers/height_over_bottom.py +++ b/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) diff --git a/command/sub8_launch/nodes/self_check.py b/command/sub8_launch/nodes/self_check.py index c587be96..f209b711 100755 --- a/command/sub8_launch/nodes/self_check.py +++ b/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,7 +148,7 @@ 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 @@ -156,8 +156,8 @@ 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) diff --git a/command/sub8_launch/scripts/tf_republisher.py b/command/sub8_launch/scripts/tf_republisher.py index b4051f53..85754603 100755 --- a/command/sub8_launch/scripts/tf_republisher.py +++ b/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() diff --git a/command/sub8_missions/missions/align_path_marker.py b/command/sub8_missions/missions/align_path_marker.py index a17e2e3e..1a352e97 100644 --- a/command/sub8_missions/missions/align_path_marker.py +++ b/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 diff --git a/command/sub8_missions/missions/autonomous.py b/command/sub8_missions/missions/autonomous.py index 79e90779..99a5894d 100644 --- a/command/sub8_missions/missions/autonomous.py +++ b/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 diff --git a/command/sub8_missions/missions/buoy.py b/command/sub8_missions/missions/buoy.py index 71fb7dbf..998c61c2 100644 --- a/command/sub8_missions/missions/buoy.py +++ b/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 diff --git a/command/sub8_missions/missions/start_gate.py b/command/sub8_missions/missions/start_gate.py index f73d39c7..fff4311c 100644 --- a/command/sub8_missions/missions/start_gate.py +++ b/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() \ No newline at end of file + yield start_gate_mission.run_mission() diff --git a/command/sub8_missions/nodes/move_command b/command/sub8_missions/nodes/move_command index 8d4c64a8..8216fbb5 100755 --- a/command/sub8_missions/nodes/move_command +++ b/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): diff --git a/command/sub8_missions/nodes/run_mission b/command/sub8_missions/nodes/run_mission index f16607cd..8cc893ae 100755 --- a/command/sub8_missions/nodes/run_mission +++ b/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(): diff --git a/command/sub8_missions/sub8/pose_editor.py b/command/sub8_missions/sub8/pose_editor.py index 52b9a103..29745d90 100644 --- a/command/sub8_missions/sub8/pose_editor.py +++ b/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) diff --git a/command/sub8_missions/sub8/sub_singleton.py b/command/sub8_missions/sub8/sub_singleton.py index 393959ec..37140119 100644 --- a/command/sub8_missions/sub8/sub_singleton.py +++ b/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'): diff --git a/command/sub8_missions/tools/rviz_visualizer.py b/command/sub8_missions/tools/rviz_visualizer.py index 5849ec73..691178e8 100755 --- a/command/sub8_missions/tools/rviz_visualizer.py +++ b/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, diff --git a/command/sub8_missions/tools/waypoint_utility.py b/command/sub8_missions/tools/waypoint_utility.py index 96441246..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 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, diff --git a/drivers/hydrophones/CMakeLists.txt b/drivers/hydrophones/CMakeLists.txt deleted file mode 100644 index a444b1ca..00000000 --- a/drivers/hydrophones/CMakeLists.txt +++ /dev/null @@ -1,71 +0,0 @@ -# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html -# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html -cmake_minimum_required(VERSION 2.8.3) -project(hydrophones) -# Load catkin and all dependencies required for this package -# TODO: remove all from COMPONENTS that are not catkin packages. -find_package(catkin REQUIRED COMPONENTS tf std_msgs message_runtime message_generation rospy geometry_msgs) -catkin_python_setup() - -# CATKIN_MIGRATION: removed during catkin migration -# cmake_minimum_required(VERSION 2.4.6) - -# CATKIN_MIGRATION: removed during catkin migration -# include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - - -# CATKIN_MIGRATION: removed during catkin migration -# rosbuild_init() - -#set the default path for built executables to the "bin" directory - -# CATKIN_MIGRATION: removed during catkin migration -# set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory - -# CATKIN_MIGRATION: removed during catkin migration -# set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -add_message_files( - FILES - ProcessedPing.msg - Debug.msg - Ping.msg -) -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES std_msgs geometry_msgs -) - -# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package -# TODO: fill in what other packages will need to use this package -catkin_package( - DEPENDS # TODO - CATKIN_DEPENDS tf std_msgs message_runtime message_generation rospy - INCLUDE_DIRS # TODO include - LIBRARIES # TODO -) - -include_directories( ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) - -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) - -install(PROGRAMS scripts/ping_printer scripts/ping_logger scripts/ping_plotter scripts/hydrophones DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/drivers/hydrophones/README.md b/drivers/hydrophones/README.md deleted file mode 100644 index c8f50c34..00000000 --- a/drivers/hydrophones/README.md +++ /dev/null @@ -1,21 +0,0 @@ -Conventions used: - -Incoming data is assumed to be from four hydrophones, indexed from 0 to 3, -laid out as follows: - - 1 - | x ^ - 3--0 | - | y <--0 z (out of screen) - 2 - -The resulting published position point is in the coordinate frame shown above. - -Other trivia: - -`dist_h` is the distance (in meters, of course) from hydrophone 0 to 1 (and -equivalently, 0 to 2). `dist_h4` is the distance from 0 to 3. - -The deltas (seen internally and published on the debug topic) are _delays_, so -one being more positive means that the ping was heard by that hydrophone -later. diff --git a/drivers/hydrophones/msg/Debug.msg b/drivers/hydrophones/msg/Debug.msg deleted file mode 100644 index cc0d479b..00000000 --- a/drivers/hydrophones/msg/Debug.msg +++ /dev/null @@ -1,6 +0,0 @@ -Header header -float64[] deltas -float64[] delta_errors -float64 fft_sharpness -float64 heading -float64 declination diff --git a/drivers/hydrophones/msg/Ping.msg b/drivers/hydrophones/msg/Ping.msg deleted file mode 100644 index 9389665a..00000000 --- a/drivers/hydrophones/msg/Ping.msg +++ /dev/null @@ -1,5 +0,0 @@ -Header header -int32 channels -int32 samples -int32 sample_rate -uint16[] data # C1 C2 C3 C4 C1 C2 C3 C4 ... diff --git a/drivers/hydrophones/msg/ProcessedPing.msg b/drivers/hydrophones/msg/ProcessedPing.msg deleted file mode 100644 index 4951f733..00000000 --- a/drivers/hydrophones/msg/ProcessedPing.msg +++ /dev/null @@ -1,5 +0,0 @@ -Header header -geometry_msgs/Point position -float64 freq -float64 amplitude -bool valid diff --git a/drivers/hydrophones/package.xml b/drivers/hydrophones/package.xml deleted file mode 100644 index 5ddadb7b..00000000 --- a/drivers/hydrophones/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - hydrophones - 1.0.0 - hydrophones - Matthew Thompson - - BSD - - http://ros.org/wiki/hydrophones - - - Matthew Thompson - - - catkin - - - tf - rospy - message_runtime - message_generation - std_msgs - geometry_msgs - - - tf - rospy - message_runtime - message_generation - std_msgs - geometry_msgs - - - - - - - diff --git a/drivers/hydrophones/scripts/hydrophones b/drivers/hydrophones/scripts/hydrophones deleted file mode 100755 index 13896643..00000000 --- a/drivers/hydrophones/scripts/hydrophones +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python - -import os -import numpy -import threading -import math - -import rospy - -from hydrophones import algorithms, util -from hydrophones.msg import Ping, ProcessedPing, Debug -from std_msgs.msg import Header -from geometry_msgs.msg import Point, PoseStamped, Pose - -def process_ping(ping): - samples = util.ping_to_samples(ping) - sample_rate = ping.sample_rate - - r = algorithms.run(samples, sample_rate, v_sound, dist_h, dist_h4) - - if len(r['pos']) > 0: - heading = math.atan2(r['pos'][1], r['pos'][0]) - declination = math.atan2(-r['pos'][2], numpy.linalg.norm(r['pos'][0:2])) - else: - heading = 0 - declination = 0 - - if len(r['errors']) > 0: - rospy.logwarn('Errors processing ping: ' + ", ".join(r['errors'])) - - valid = len(r['pos']) > 0 and len(r['errors']) == 0 - if valid: - pub.publish(header=Header(stamp=ping.header.stamp, - frame_id=ping.header.frame_id), - position=Point(*r['pos'].tolist()), - freq=r['freq'], - amplitude=r['amplitude'], - valid=valid) - pub_pose.publish(header=Header(stamp=ping.header.stamp, - frame_id=ping.header.frame_id), - pose=Pose(position=Point(*r['pos'].tolist()))) - pub_debug.publish(header=Header(stamp=ping.header.stamp, - frame_id=ping.header.frame_id), - deltas=r['deltas'].tolist(), - delta_errors=r['delta_errors'].tolist(), - fft_sharpness=r['fft_sharpness'], - heading=heading, - declination=declination) - -rospy.init_node('hydrophones') -dist_h = rospy.get_param('~dist_h') -dist_h4 = rospy.get_param('~dist_h4') -v_sound = rospy.get_param('~v_sound') -template_periods = rospy.get_param('~template_periods', 3) -pub = rospy.Publisher('hydrophones/processed', ProcessedPing, queue_size=10) -pub_pose = rospy.Publisher('hydrophones/pose', PoseStamped, queue_size=10) -pub_debug = rospy.Publisher('hydrophones/debug', Debug, queue_size=10) -sub = rospy.Subscriber('hydrophones/ping', Ping, process_ping) -rospy.spin() diff --git a/drivers/hydrophones/scripts/ping_logger b/drivers/hydrophones/scripts/ping_logger deleted file mode 100755 index 6747aa30..00000000 --- a/drivers/hydrophones/scripts/ping_logger +++ /dev/null @@ -1,25 +0,0 @@ -#!/usr/bin/env python - -import os -import numpy - -import roslib -roslib.load_manifest('hydrophones') -import rospy - -from hydrophones.msg import Ping -from hydrophones import util - -rospy.init_node('ping_logger') -output_dir = rospy.get_param("~output_dir", "pings") -if not os.path.exists(output_dir): - os.makedirs(output_dir) - -def save_ping(ping): - path = os.path.join(output_dir, str(ping.header.seq) + ".csv") - samples = util.ping_to_samples(ping) - numpy.savetxt(path, samples.transpose(), fmt='%d', delimiter=',') - -sub = rospy.Subscriber('hydrophones/ping', Ping, save_ping) -rospy.spin() - diff --git a/drivers/hydrophones/scripts/ping_plotter b/drivers/hydrophones/scripts/ping_plotter deleted file mode 100755 index 53ca34e1..00000000 --- a/drivers/hydrophones/scripts/ping_plotter +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python - -import os -import numpy -import threading -import matplotlib -import matplotlib.pyplot as plt - -import roslib -roslib.load_manifest('hydrophones') -import rospy - -from hydrophones import algorithms, util -from hydrophones.msg import Ping - -class Node(object): - def __init__(self): - self.fig = plt.figure() - self.ax = self.fig.add_subplot(111) - self.fig.show() - self.cond = threading.Condition() - self.samples = None - self.sub = rospy.Subscriber('hydrophones/ping', Ping, self.ping_callback) - - def ping_callback(self, ping): - with self.cond: - self.samples = util.ping_to_samples(ping) - self.cond.notify_all() - - def run(self): - while not rospy.is_shutdown(): - with self.cond: - self.cond.wait(.5) - if self.samples is not None: - self.ax.clear() - self.ax.plot(self.samples.transpose()) - self.ax.autoscale() - self.fig.canvas.draw() - self.fig.show() - self.samples = None - -rospy.init_node('ping_plotter') -Node().run() - diff --git a/drivers/hydrophones/scripts/ping_printer b/drivers/hydrophones/scripts/ping_printer deleted file mode 100755 index fe2d083c..00000000 --- a/drivers/hydrophones/scripts/ping_printer +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env python - -import math - -import roslib -roslib.load_manifest('hydrophones') -import rospy - -from hydrophones.msg import ProcessedPing - -def print_heading(ping_processed): - print '%s%dhz heading %d declination %d amplitude %d' % ('bad' if not ping_processed.valid else '', - ping_processed.freq, - ping_processed.heading / math.pi * 180, - ping_processed.declination / math.pi * 180, - ping_processed.amplitude) - -rospy.init_node('print_heading') -sub = rospy.Subscriber('hydrophones/processed', ProcessedPing, print_heading) -rospy.spin() diff --git a/drivers/hydrophones/setup.py b/drivers/hydrophones/setup.py deleted file mode 100644 index 8a4c344f..00000000 --- a/drivers/hydrophones/setup.py +++ /dev/null @@ -1,13 +0,0 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['hydrophones'], - package_dir={'': 'src'}, - requires=[], # TODO -) - -setup(**setup_args) \ No newline at end of file diff --git a/drivers/hydrophones/src/__init__.py b/drivers/hydrophones/src/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/drivers/hydrophones/src/hydrophones/__init__.py b/drivers/hydrophones/src/hydrophones/__init__.py deleted file mode 100644 index b23e43d7..00000000 --- a/drivers/hydrophones/src/hydrophones/__init__.py +++ /dev/null @@ -1 +0,0 @@ -#autogenerated by ROS python message generators \ No newline at end of file diff --git a/drivers/hydrophones/src/hydrophones/algorithms.py b/drivers/hydrophones/src/hydrophones/algorithms.py deleted file mode 100644 index 122465ff..00000000 --- a/drivers/hydrophones/src/hydrophones/algorithms.py +++ /dev/null @@ -1,256 +0,0 @@ -from __future__ import division -from hydrophones import util - -import numpy -import scipy -import math -import sys -import matplotlib.pyplot as plt - -def run(samples, sample_rate, v_sound, dist_h, dist_h4): - # Perform algorithm - samples = zero_mean(samples) - freq, amplitude, samples_fft = compute_freq(samples, sample_rate, numpy.array([5e3, 40e3]), plot=True) - fft_sharpness = amplitude**2/numpy.sum(samples_fft) - upsamples, upsample_rate = preprocess(samples, sample_rate, 3e6) - deltas, delta_errors, template_pos, template_width = \ - compute_deltas(upsamples, upsample_rate, freq, 8) - delta_errors = delta_errors / amplitude - if len(deltas) == 3: - pos = compute_pos_4hyd(deltas, upsample_rate, v_sound, dist_h, dist_h4) - else: - pos = numpy.empty(0) - - # Check for errors - errors = [] - if amplitude < 80: - errors.append('Low amplitude at maximum frequency') - if template_pos is None: - errors.append('Failed to find template') - elif numpy.max(delta_errors) > 1e-3: - errors.append('High template match error (%s)' % str(delta_errors)) - - return dict( - errors=errors, - freq=freq, - amplitude=amplitude, - fft_sharpness=fft_sharpness, - samples_fft=samples_fft, - pos=pos, - deltas=deltas, - delta_errors=delta_errors, - upsamples=upsamples, - upsample_rate=upsample_rate, - template_pos=template_pos, - template_width=template_width) - -def zero_mean(samples): - """Zero means data by assuming that the first 32 samples are zeros""" - return samples - numpy.mean(samples[:, 0:32], 1)[:, numpy.newaxis] - -def normalize(samples): - """Rescapes samples so each individual channel lies between -1 and 1""" - return samples / numpy.amax(numpy.abs(samples), 1)[:, numpy.newaxis] - -def compute_freq(samples, sample_rate, freq_range, plot=False): - """Checks whether samples are likely a solid ping and returns the frequency.""" - samples_window = samples * numpy.hamming(samples.shape[1]) - - # Compute fft, find peaks in desired range - fft_length = 2048 - samples_fft = numpy.absolute(numpy.fft.fft(samples_window, fft_length, axis=1))[:, :fft_length/2] - bin_range = freq_to_bin(freq_range, sample_rate, fft_length) - peaks = bin_range[0] + numpy.argmax(samples_fft[:, bin_range[0]:bin_range[1]], axis=1) - - # Sort peaks, take mean of the middle - middle_peaks = numpy.sort(peaks)[len(peaks)//4:len(peaks) - len(peaks)//4] - peak = numpy.mean(middle_peaks) - - freq = bin_to_freq(peak, sample_rate, fft_length) - - amplitude = math.sqrt(numpy.mean(samples_fft[:, round(peak)])) - return freq, amplitude, samples_fft - -def bin_to_freq(bin, sample_rate, fft_length): - return (sample_rate/2) / (fft_length/2) * bin - -def freq_to_bin(freq, sample_rate, fft_length): - return freq * ((fft_length/2) / (sample_rate/2)) - -def preprocess(samples, sample_rate, desired_sample_rate): - """Upsamples data to have approx. desired_sample_rate.""" - # Trim to first ms of data and bandpass - samples = bandpass(samples[:, :.001*sample_rate], sample_rate) - samples = normalize(samples) - - # Upsample each channel - upfact = round(desired_sample_rate/sample_rate) - upsamples = numpy.empty((samples.shape[0], samples.shape[1]*upfact)) - for i in xrange(samples.shape[0]): - upsamples[i, :] = util.resample(samples[i, :], upfact, 1) - return upsamples, sample_rate*upfact - -def bandpass(samples, sample_rate): - """Applies a 20-30khz bandpass FIR filter""" - # 25-40KHz is the range of the pinger for the roboboat competition - fir = scipy.signal.firwin(128, - [19e3/(sample_rate/2), 41e3/(sample_rate/2)], - window='hann', - pass_zero=False) - return scipy.signal.lfilter(fir, 1, samples) - -def compute_deltas(samples, sample_rate, ping_freq, template_periods, plot=False): - """ - Computes N-1 position deltas for N channels, by making a template - for the first channel and matching to all subsequent channels. - """ - period = int(round(sample_rate / ping_freq)) - template_width = period*template_periods+1 - template, template_pos = make_template(samples[0, :], - .2, - template_width) - if template_pos is None: - return numpy.empty(0), numpy.empty(0), None, template_width - start = template_pos - period//2 - stop = template_pos + period//2 - - deltas = numpy.empty(samples.shape[0]-1) - errors = numpy.empty(samples.shape[0]-1) - for i in xrange(deltas.shape[0]): - pos, error = match_template(samples[i+1, :], start, stop, template) - deltas[i] = pos - template_pos - errors[i] = error - - return deltas, errors, template_pos, template_width - -def make_template(channel, thresh, width): - """ - Returns a template of the specified width, centered at the first - point of the channel to exceed thresh. - """ - for pos in xrange(0, channel.shape[0]-width): - if abs(channel[pos+width//4]) >= thresh: - return channel[pos:pos+width], pos - return None, None - -def match_template(channel, start, stop, template): - """ - Matches template to channel, returning the point where the start - of the template should be placed. - """ - start = max(start, 0) - stop = min(stop, channel.shape[0] - template.shape[0]) - mad = mean_absolute_difference(channel, start, stop, template) - min_pt = find_zero(mad) - - return start + min_pt, mad[round(min_pt)] - -def mean_absolute_difference(channel, start, stop, template): - """ - Slides the template along channel from start to stop, giving the - mean absolute difference of the template and channel at each - location. - """ - width = template.shape[0] - mad = numpy.zeros(stop-start) - for i in xrange(start, stop): - mad[i-start] = numpy.mean(numpy.abs(channel[i:i+width] - template)) - return mad - -def find_zero(data): - """ - Finds the sub-sample position of the first zero in a continuous signal, - by first finding the lowest absolute value, then taking the gradient - and performing a linear interpolation near the lowest absolute value. - """ - approx = numpy.argmin(numpy.abs(data)) - d_data = numpy.gradient(data) - - for pos in xrange(max(approx-3,0), min(approx+3, d_data.shape[0]-1)): - if numpy.sign(d_data[pos]) != numpy.sign(d_data[pos+1]): - y2 = d_data[pos+1] - y1 = d_data[pos] - x2 = pos+1 - x1 = pos - return -(x2-x1)/(y2-y1)*y1 + x1 - return approx - -def compute_pos_4hyd(deltas, sample_rate, v_sound, dist_h, dist_h4): - """ - Solves the 4 hydrophone case (3 deltas) for heading, declination, - and sph_dist using a bunch of trig. Future work would be to phrase - this as a NLLSQ problem or something, and adapt it to more - hydrophones. - """ - assert(len(deltas) == 3) - - y1 = deltas[0]/sample_rate*v_sound - y2 = deltas[1]/sample_rate*v_sound - y3 = deltas[2]/sample_rate*v_sound - - dist = abs((y1**2 + y2**2 - 2*dist_h**2)/(2*y1 + 2*y2)) - cos_alpha1 = (2*dist*y1 + y1**2 - dist_h**2)/(-2*dist*dist_h); - cos_alpha2 = -(2*dist*y2 + y2**2 - dist_h**2)/(-2*dist*dist_h); - cos_alpha = (cos_alpha1 + cos_alpha2)/2; - - cos_beta = (2*dist*y3 + y3**2 - dist_h4**2)/(-2*dist*dist_h4); - - dist_x = cos_alpha*dist - dist_y = cos_beta*dist - if dist**2 - (dist_x**2 + dist_y**2) < 0: - dist_z = 0 - else: - dist_z = -math.sqrt(dist**2 - (dist_x**2 + dist_y**2)) - return numpy.array([dist_x, dist_y, dist_z]) - -if __name__ == '__main__': - sample_rate = 300e3 - if len(sys.argv) > 1: - samples = numpy.loadtxt(sys.argv[1], delimiter=',').transpose() - else: - samples = util.make_ping([0, .25, 1.234, -5], {'freq': 23e3, 'sample_rate': sample_rate}) - - r = run(samples, sample_rate, 1497, 2.286000e-02, 2.286000e-02) - - if len(r['errors']) > 0: - print 'ERRORS', r['errors'] - print 'freq', r['freq'], 'amplitude', r['amplitude'] - print 'sharpness', r['fft_sharpness'] - print 'deltas', r['deltas'] - print 'delta errors', r['delta_errors']/r['amplitude'] - print 'pos (hyd coordinates)', r['pos'] - - plt.figure() - plt.plot(samples.transpose()) - plt.title('Raw ping') - - plt.figure() - fft_length = r['samples_fft'].shape[1]*2 - plt.plot(bin_to_freq(numpy.arange(0, fft_length//2), sample_rate, fft_length), - r['samples_fft'].transpose()) - plt.title('FFT') - - plt.figure() - plt.plot(r['upsamples'].transpose()) - plt.title('Upsampled ping') - - if r['template_pos'] is not None: - period = int(round(r['upsample_rate'] / r['freq'])) - template = r['upsamples'][0,r['template_pos']:r['template_pos']+r['template_width']] - plot_start = r['template_pos'] - 2*period - plot_stop = r['template_pos'] + r['template_width'] + 2*period - plt.ioff() - plt.figure() - plt.plot(template) - plt.title('Template') - - for i in xrange(r['deltas'].shape[0]): - plt.figure() - plt.plot(numpy.arange(plot_start, plot_stop), r['upsamples'][i+1, plot_start:plot_stop]) - pos = r['template_pos'] + int(round(r['deltas'][i])) - plt.plot(numpy.arange(pos, pos+r['template_width']), template) - plt.title('Channel %d' % (i+1)) - - plt.show() - - diff --git a/drivers/hydrophones/src/hydrophones/util.py b/drivers/hydrophones/src/hydrophones/util.py deleted file mode 100644 index f19e1d4c..00000000 --- a/drivers/hydrophones/src/hydrophones/util.py +++ /dev/null @@ -1,73 +0,0 @@ -from __future__ import division - -import numpy -import scipy -import scipy.signal -import math - -from hydrophones.msg import Ping - -def resample(x, p, q): - """Polyphase filter resample, based on MATLAB's resample.m""" - bta = 5 - N = 10 - pqmax = max(p, q) - fc = (1/2)/pqmax - L = 2*N*pqmax + 1 - h = p*scipy.signal.firwin(L-1, 2*fc, window=('kaiser', bta)) - Lhalf = (L-1)/2 - Lx = len(x) - - nz = math.floor(q-(Lhalf % q)) - z = numpy.zeros(nz) - Lhalf += nz - - delay = math.floor(math.ceil(Lhalf)/q) - nz1 = 0 - while math.ceil(((Lx - 1)*p + len(h) + nz1)/q) - delay < math.ceil(Lx*p/q): - nz1 = nz1+1; - h = numpy.hstack([h, numpy.zeros(nz1)]); - y = upfirdn(x,h,p,q) - Ly = math.ceil(Lx*p/q) - y = y[delay:] - y = y[:Ly] - return y - -def upfirdn(x, h, p, q): - # Allocate an expanded array to hold x interspersed with p-1 zeros, - # padded with enough zeros for the fir filter - x_expanded = numpy.zeros((x.shape[0] - 1)*p + h.shape[0]) - - # Insert x values every p elements - x_expanded[:x.shape[0]*p:p] = x - - # Run filter - x_filt = scipy.signal.lfilter(h, 1, x_expanded) - return x_filt - -def make_ping_channel(delay=0, - freq=25e3, - offset=0x7FFF, - ampl=200, - zeros=64, - count=1024, - sample_rate=300e3): - w = 2*math.pi*freq/sample_rate - sinwave = ampl*numpy.sin(w*(numpy.arange(count)-delay)) - - delay_i = round(delay) - window = numpy.zeros(count) - window[zeros+delay_i:] = numpy.minimum(numpy.exp(numpy.arange(count - zeros - delay_i)/10), 2)-1 - - noise = numpy.random.normal(0, .01, count) - - return offset + sinwave * window + noise - -def make_ping(delays=[0, 0, 0, 0], args={}): - return numpy.vstack(make_ping_channel(delay=delay, **args) for delay in delays) - -def samples_to_list(samples): - return samples.transpose().flatten().tolist() - -def ping_to_samples(ping): - return numpy.array(ping.data, dtype=numpy.float64).reshape((ping.samples, ping.channels)).transpose() diff --git a/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/CMakeLists.txt b/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/CMakeLists.txt index c02c3b5c..beda35b4 100644 --- a/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/CMakeLists.txt +++ b/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(magnetic_hardsoft_compensation) -find_package(catkin REQUIRED COMPONENTS tf geometry_msgs nodelet roscpp uf_common eigen_conversions) +find_package(catkin REQUIRED COMPONENTS tf geometry_msgs nodelet roscpp mil_tools eigen_conversions) catkin_package( DEPENDS - CATKIN_DEPENDS tf geometry_msgs nodelet roscpp uf_common eigen_conversions + CATKIN_DEPENDS tf geometry_msgs nodelet roscpp mil_tools eigen_conversions INCLUDE_DIRS LIBRARIES ) diff --git a/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/package.xml b/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/package.xml index a83c0a28..c55a9a10 100644 --- a/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/package.xml +++ b/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/package.xml @@ -16,14 +16,14 @@ nodelet geometry_msgs roscpp - uf_common + mil_tools eigen_conversions tf nodelet geometry_msgs roscpp - uf_common + mil_tools eigen_conversions 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 71c7c397..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 sub8_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/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/src/nodelet.cpp b/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/src/nodelet.cpp index 9de8a07d..a2245990 100644 --- a/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/src/nodelet.cpp +++ b/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/src/nodelet.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include namespace magnetic_hardsoft_compensation { class Nodelet : public nodelet::Nodelet { @@ -42,9 +42,9 @@ class Nodelet : public nodelet::Nodelet { virtual void onInit() { ros::NodeHandle& private_nh = getPrivateNodeHandle(); - frame_id = uf_common::getParam(private_nh, "frame_id"); - scale_inverse = uf_common::getParam(private_nh, "scale").inverse(); - shift = uf_common::getParam(private_nh, "shift"); + frame_id = mil_tools::getParam(private_nh, "frame_id"); + scale_inverse = mil_tools::getParam(private_nh, "scale").inverse(); + shift = mil_tools::getParam(private_nh, "shift"); ros::NodeHandle& nh = getNodeHandle(); diff --git a/drivers/paulboard_driver/CMakeLists.txt b/drivers/paulboard_driver/CMakeLists.txt deleted file mode 100644 index 6ffedf4f..00000000 --- a/drivers/paulboard_driver/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html -# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html -cmake_minimum_required(VERSION 2.8.3) -project(paulboard_driver) -# Load catkin and all dependencies required for this package -# TODO: remove all from COMPONENTS that are not catkin packages. -find_package(catkin REQUIRED COMPONENTS rospy hydrophones) -catkin_python_setup() - -# CATKIN_MIGRATION: removed during catkin migration -# cmake_minimum_required(VERSION 2.4.6) - -# CATKIN_MIGRATION: removed during catkin migration -# include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - - -# CATKIN_MIGRATION: removed during catkin migration -# rosbuild_init() - -#set the default path for built executables to the "bin" directory - -# CATKIN_MIGRATION: removed during catkin migration -# set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory - -# CATKIN_MIGRATION: removed during catkin migration -# set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) -## Generate added messages and services with any dependencies listed here -#generate_messages( -# DEPENDENCIES -#) - -# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package -# TODO: fill in what other packages will need to use this package -catkin_package( - DEPENDS # TODO - CATKIN_DEPENDS rospy hydrophones - INCLUDE_DIRS # TODO include - LIBRARIES # TODO -) - -include_directories( ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) - - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) - -install(PROGRAMS scripts/paulboard_driver DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/drivers/paulboard_driver/README.md b/drivers/paulboard_driver/README.md deleted file mode 100644 index 3db6c494..00000000 --- a/drivers/paulboard_driver/README.md +++ /dev/null @@ -1,6 +0,0 @@ -Explanation of `permute` parameter ----------------------------------- - -A `permute` value of `w x y z` means that the connector labeled Aw on the PCB -maps to hydrophone 0 as the hydrophones package wants it, Ax maps to 1, Ay -maps to 2, and Az maps to 3. See `README.md` in the hydrophones package. diff --git a/drivers/paulboard_driver/blobs/SimpleHyd2013.bin b/drivers/paulboard_driver/blobs/SimpleHyd2013.bin deleted file mode 100644 index 4b4c50a0..00000000 Binary files a/drivers/paulboard_driver/blobs/SimpleHyd2013.bin and /dev/null differ diff --git a/drivers/paulboard_driver/package.xml b/drivers/paulboard_driver/package.xml deleted file mode 100644 index 00de2fc6..00000000 --- a/drivers/paulboard_driver/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - paulboard_driver - 1.0.0 - paulboard_driver - Matthew Thompson - - BSD - - http://ros.org/wiki/paulboard_driver - - - Matthew Thompson - - - catkin - - - rospy - hydrophones - - - rospy - hydrophones - - - - - - \ No newline at end of file diff --git a/drivers/paulboard_driver/scripts/paulboard_driver b/drivers/paulboard_driver/scripts/paulboard_driver deleted file mode 100755 index d5dcbafd..00000000 --- a/drivers/paulboard_driver/scripts/paulboard_driver +++ /dev/null @@ -1,149 +0,0 @@ -#!/usr/bin/env python - -import serial -import numpy - -import roslib -roslib.load_manifest('paulboard_driver') -import rospy - -from std_msgs.msg import Header - -from paulboard_driver import ais_bootloader -from hydrophones.msg import Ping -from hydrophones import util - -class Node(object): - def __init__(self, ser): - self.sample_rate = rospy.get_param('~sample_rate', 300e3) - self.thresh = rospy.get_param('~thresh', 500) - self.frame = rospy.get_param('~frame', '/hydrophones') - permute_str = rospy.get_param('~permute', '1 2 3 4') - self.permute = [int(v)-1 for v in permute_str.split(' ')] - self.ser = ser - self.pub = rospy.Publisher('hydrophones/ping', Ping) - self.buf = "" - self.last_packet_num = None - - self.ser.baudrate = 1152000 - - def run_command(self, cmd): - self.ser.timeout = .2 - self.ser.write(cmd) - response = self.ser.read(128) - if 'OK' not in response: - raise RuntimeError('Invalid response to command "%s"\n got: "%s"' % (cmd, response)) - - def run(self): - self.run_command('samplerate set\r%d\r' % (self.sample_rate)) - self.run_command('thresh set\r%d\r' % (self.thresh)) - self.ser.write('go\r') - self.ser.read(99999) - self.ser.timeout = .1 - rospy.loginfo('Paulboard running') - - buf = "" - while not rospy.is_shutdown(): - got = self.ser.read(10240) - if len(got) > 0: - buf += got - elif len(buf) > 0: - self.parse_publish_ping(buf) - buf = "" - - def parse_publish_ping(self, buf): - result = self.try_parse_ping(buf) - if result is None: - rospy.logerr('Got bad ping packet') - return - - packet_num, samples = result - if self.last_packet_num is not None and packet_num != self.last_packet_num+1: - rospy.logwarn('Dropped %d ping packet(s)' % (packet_num - (self.last_packet_num+1))) - self.last_packet_num = packet_num - - samples = samples[self.permute, :] - - self.pub.publish(Ping( - header=Header(stamp=rospy.Time.now(), - frame_id=self.frame), - channels=samples.shape[0], - samples=samples.shape[1], - data=util.samples_to_list(samples), - sample_rate=self.sample_rate)) - - def try_parse_ping(self, buf): - # Verify we got a header - if len(buf) < 8: - return None - header = word16(buf[0:2]) - if header != 0x1234: - return None - - # Parse header, verify we have all data - packet_num = word16(buf[2:4]) - sample_rate = word16(buf[4:6])*1000 - sample_count = word16(buf[6:8]) - if len(buf) < 8 + 8*sample_count + 4: - return None - - # Parse all samples into an array of column vectors - pos = 8 - samples = numpy.zeros((4, sample_count), numpy.uint16) - for sample in xrange(sample_count): - for chan in xrange(4): - samples[chan, sample] = word16(buf[pos:pos+2]) - pos += 2 - - # Parse footer - checksum = word16(buf[pos:pos+2]) - footer = word16(buf[pos+2:pos+4]) - if footer != 0x4321: - return None - - return (packet_num, samples) - -def word16(str): - return ord(str[0]) + (ord(str[1]) << 8) - -def check_board_bootloaded(ser): - ser.timeout = .5 - ser.write('\r') - got = ser.read(99999) - if got.endswith('> '): - return True - return False - -def bootload_board(ser): - path = roslib.packages.resource_file('paulboard_driver', 'blobs', 'SimpleHyd2013.bin') - print 'Bin path: ', path - ser.timeout = 1 - ser.baudrate = 115200 - with open(path, 'rb') as file: - ais_bootloader.boot(ser, file) - rospy.sleep(0.5) # Give program time to start - -if __name__ == '__main__': - rospy.init_node('paulboard_driver') - port = rospy.get_param('~port') - print 'Port: ', port - - with serial.Serial(port, 115200, timeout=.3) as ser: - if check_board_bootloaded(ser): - rospy.loginfo('PaulBoard already bootloaded') - else: - rospy.loginfo('Bootloading PaulBoard') - while True: - try: - bootload_board(ser) - rospy.loginfo('Bootloading complete') - break - except ais_bootloader.Exception, ex: - rospy.logerr('Failed to bootload: %s' % ex) - if rospy.is_shutdown(): - break - rospy.sleep(1) - - Node(ser).run() - - diff --git a/drivers/paulboard_driver/setup.py b/drivers/paulboard_driver/setup.py deleted file mode 100644 index bc1213b0..00000000 --- a/drivers/paulboard_driver/setup.py +++ /dev/null @@ -1,13 +0,0 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['paulboard_driver'], - package_dir={'': 'src'}, - requires=[], # TODO -) - -setup(**setup_args) \ No newline at end of file diff --git a/drivers/paulboard_driver/src/paulboard_driver/SimpleHyd2013.bin b/drivers/paulboard_driver/src/paulboard_driver/SimpleHyd2013.bin deleted file mode 100644 index 4b4c50a0..00000000 Binary files a/drivers/paulboard_driver/src/paulboard_driver/SimpleHyd2013.bin and /dev/null differ diff --git a/drivers/paulboard_driver/src/paulboard_driver/__init__.py b/drivers/paulboard_driver/src/paulboard_driver/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/drivers/paulboard_driver/src/paulboard_driver/ais_bootloader.py b/drivers/paulboard_driver/src/paulboard_driver/ais_bootloader.py deleted file mode 100644 index 1ac1dcd8..00000000 --- a/drivers/paulboard_driver/src/paulboard_driver/ais_bootloader.py +++ /dev/null @@ -1,147 +0,0 @@ -import logging -import serial -import time - -MAGICWORD = 0x41504954; -XMT_START_WORD = 0x58535441; -RCV_START_WORD = 0x52535454; - -OP_BOOTTABLE = 0x58535907; -OP_SEQREADEN = 0x58535963; -OP_SECTIONLOAD = 0x58535901; -OP_CSECTIONLOAD = 0x58535909; -OP_SECTIONFILL = 0x5853590A; -OP_FXNEXEC = 0x5853590D; -OP_JUMP = 0x58535905; -OP_JUMPCLOSE = 0x58535906; -OP_CRCEN = 0x58535903; -OP_CRCDIS = 0x58535904; -OP_CRCREQ = 0x58535902; -OP_READWAIT = 0x58535914; -OP_STARTOVER = 0x58535908; -OP_PINGDEVICE = 0x5853590B; - -def op2ack(op): - return (op & ~0x0F000000) | 0x02000000 - -class Exception(RuntimeError): - pass - -def delay(): - time.sleep(5/1000) - -def word2str(word): - return chr(word & 0xFF) + \ - chr((word >> 8) & 0xFF) + \ - chr((word >> 16) & 0xFF) + \ - chr((word >> 24) & 0xFF) - -def str2word(s): - vals = map(ord, s) - return vals[0] + (vals[1] << 8) + (vals[2] << 16) + (vals[3] << 24) - -def read_word_timeout(ser): - s = ser.read(4) - if len(s) < 4: - raise Exception('Timeout while reading word') - return str2word(s) - -def sws(ser): - """Start word sync""" - logging.debug('Performing SWS') - ser.write(chr(XMT_START_WORD >> 24)) - while True: - response = ser.read(1) - if len(response) == 0: - raise Exception('Timeout during start word sync') - if ord(response[0]) == (RCV_START_WORD >> 24): - return True - -def pos(ser, N=10): - """Ping opcode sync""" - logging.debug('Performing POS') - ser.write(word2str(OP_PINGDEVICE)) - if read_word_timeout(ser) != op2ack(OP_PINGDEVICE): - raise Exception('Invalid response to ping opcode') - delay() - - ser.write(word2str(N)) - if read_word_timeout(ser) != N: - raise Exception('Invalid response to ping count') - - for i in xrange(1, N+1): - ser.write(word2str(i)) - if read_word_timeout(ser) != i: - raise Exception('Invalid response to ping %d' % i) - return True - -def os(ser, op): - ser.write(word2str(op)) - response = read_word_timeout(ser) - if response != op2ack(op): - raise Exception('Invalid response to opcode (%x, expected %x)' % (response, op2ack(op))) - -def boot(ser, file): - magic = str2word(file.read(4)) - if magic != MAGICWORD: - raise Exception('Invalid magic word in file') - - sws(ser) - pos(ser) - - while True: - delay() - op = str2word(file.read(4)) - os(ser, op) - - if op == OP_SECTIONLOAD or op == OP_CSECTIONLOAD: - addr = str2word(file.read(4)) - ser.write(word2str(addr)) - size = str2word(file.read(4)) - ser.write(word2str(size)) - logging.debug('SECTIONLOAD of %d bytes to 0x%x' % (size, addr)) - ser.write(file.read(size)) - elif op == OP_FXNEXEC: - args = str2word(file.read(4)) - ser.write(word2str(args)) - words = (args >> 16)*4 - logging.debug('FXNEXEC of %d bytes' % words) - ser.write(file.read(words)) - elif op == OP_JUMPCLOSE: - addr = str2word(file.read(4)) - logging.debug('JUMPLOAD to 0x%x' % addr) - ser.write(word2str(addr)) - break - elif op == OP_CRCEN: - logging.debug('Enabled CRC') - elif op == OP_CRCDIS: - logging.debug('Disabled CRC') - elif op == OP_CRCREQ: - calc_crc = read_word_timeout(ser) - real_crc = str2word(file.read(4)) - seek = str2word(file.read(4)) - if seek > 0x7FFFFFFF: - seek = (seek-1) - 0xFFFFFFFF - if real_crc == calc_crc: - logging.debug('CRC passed') - else: - logging.debug('CRC failed') - file.seek(seek, 1) - else: - raise Exception('Unknown opcode 0x%x' % op) - - donestr = ser.read(8) - if donestr != " DONE\0": - raise Exception('Invalid done string: %s' % donestr) - - return True - -def main(): - ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1) - with open('SimpleHyd2013.bin') as file: - boot(ser, file) - -if __name__ == '__main__': - logging.basicConfig(level=logging.DEBUG) - main() - diff --git a/drivers/sub8_actuator_driver/nodes/actuator_driver.py b/drivers/sub8_actuator_driver/nodes/actuator_driver.py index 30045360..b9c7deec 100755 --- a/drivers/sub8_actuator_driver/nodes/actuator_driver.py +++ b/drivers/sub8_actuator_driver/nodes/actuator_driver.py @@ -4,7 +4,7 @@ import rospkg from sub8_msgs.srv import SetValve -from sub8_ros_tools import thread_lock +from mil_ros_tools import thread_lock from sub8_alarm import AlarmBroadcaster import threading diff --git a/drivers/sub8_adis16400_imu/CMakeLists.txt b/drivers/sub8_adis16400_imu/CMakeLists.txt index 46b79395..07dc1b43 100644 --- a/drivers/sub8_adis16400_imu/CMakeLists.txt +++ b/drivers/sub8_adis16400_imu/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) project(adis16400_imu) -find_package(catkin REQUIRED COMPONENTS sensor_msgs geometry_msgs nodelet roscpp uf_common) +find_package(catkin REQUIRED COMPONENTS sensor_msgs geometry_msgs nodelet roscpp mil_tools) catkin_package( DEPENDS - CATKIN_DEPENDS sensor_msgs geometry_msgs nodelet roscpp uf_common + CATKIN_DEPENDS sensor_msgs geometry_msgs nodelet roscpp mil_tools INCLUDE_DIRS include LIBRARIES ) diff --git a/drivers/sub8_adis16400_imu/package.xml b/drivers/sub8_adis16400_imu/package.xml index bd2b57fe..335c8b28 100644 --- a/drivers/sub8_adis16400_imu/package.xml +++ b/drivers/sub8_adis16400_imu/package.xml @@ -14,13 +14,13 @@ sensor_msgs roscpp nodelet - uf_common + mil_tools geometry_msgs sensor_msgs roscpp nodelet - uf_common + mil_tools diff --git a/drivers/sub8_adis16400_imu/src/nodelet.cpp b/drivers/sub8_adis16400_imu/src/nodelet.cpp index 8a13402a..3e533552 100644 --- a/drivers/sub8_adis16400_imu/src/nodelet.cpp +++ b/drivers/sub8_adis16400_imu/src/nodelet.cpp @@ -5,7 +5,7 @@ #include #include -#include +#include #include "adis16400_imu/driver.h" @@ -20,9 +20,9 @@ class Nodelet : public nodelet::Nodelet { } virtual void onInit() { - std::string port = uf_common::getParam(getPrivateNodeHandle(), "port"); - frame_id = uf_common::getParam(getPrivateNodeHandle(), "frame_id"); - drop_every_ = uf_common::getParam(getPrivateNodeHandle(), "divide", 1); + std::string port = mil_tools::getParam(getPrivateNodeHandle(), "port"); + frame_id = mil_tools::getParam(getPrivateNodeHandle(), "frame_id"); + drop_every_ = mil_tools::getParam(getPrivateNodeHandle(), "divide", 1); ros::NodeHandle& nh = getNodeHandle(); pub = nh.advertise("imu/data_raw", 10); diff --git a/drivers/sub8_depth_driver/CMakeLists.txt b/drivers/sub8_depth_driver/CMakeLists.txt index 7b832105..2f5d81ca 100644 --- a/drivers/sub8_depth_driver/CMakeLists.txt +++ b/drivers/sub8_depth_driver/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(depth_driver) -find_package(catkin REQUIRED COMPONENTS nodelet roscpp uf_common) +find_package(catkin REQUIRED COMPONENTS nodelet roscpp mil_msgs mil_tools) catkin_package( DEPENDS - CATKIN_DEPENDS nodelet roscpp uf_common + CATKIN_DEPENDS nodelet roscpp mil_msgs mil_tools INCLUDE_DIRS include LIBRARIES ) diff --git a/drivers/sub8_depth_driver/package.xml b/drivers/sub8_depth_driver/package.xml index 79129b2d..3d4a0f40 100644 --- a/drivers/sub8_depth_driver/package.xml +++ b/drivers/sub8_depth_driver/package.xml @@ -14,14 +14,16 @@ nodelet roscpp - uf_common + mil_msgs + mil_tools nodelet roscpp - uf_common + mil_msgs + mil_tools - \ No newline at end of file + diff --git a/drivers/sub8_depth_driver/scripts/fake_depth b/drivers/sub8_depth_driver/scripts/fake_depth index 56f8b82f..41eb0e6a 100755 --- a/drivers/sub8_depth_driver/scripts/fake_depth +++ b/drivers/sub8_depth_driver/scripts/fake_depth @@ -3,20 +3,20 @@ import roslib import rospy from std_msgs.msg import Header -from uf_common.msg import Float64Stamped +from mil_msgs.msg import DepthStamped roslib.load_manifest('depth_driver') rospy.init_node('fake_depth') -pub = rospy.Publisher('/depth', Float64Stamped) +pub = rospy.Publisher('/depth', DepthStamped) while not rospy.is_shutdown(): - pub.publish(Float64Stamped( + pub.publish(DepthStamped( header=Header( stamp=rospy.Time.now(), frame_id='/map', ), - data=0, + depth=0, )) rospy.sleep(rospy.Duration(.2)) diff --git a/drivers/sub8_depth_driver/src/nodelet.cpp b/drivers/sub8_depth_driver/src/nodelet.cpp index 75a9ab53..4560a79c 100644 --- a/drivers/sub8_depth_driver/src/nodelet.cpp +++ b/drivers/sub8_depth_driver/src/nodelet.cpp @@ -5,8 +5,8 @@ #include #include -#include -#include +#include +#include #include @@ -23,11 +23,11 @@ class Nodelet : public nodelet::Nodelet { } virtual void onInit() { - std::string port = uf_common::getParam(getPrivateNodeHandle(), "port"); - int baudrate = uf_common::getParam(getPrivateNodeHandle(), "baudrate", 115200); - frame_id = uf_common::getParam(getPrivateNodeHandle(), "frame_id"); + std::string port = mil_tools::getParam(getPrivateNodeHandle(), "port"); + int baudrate = mil_tools::getParam(getPrivateNodeHandle(), "baudrate", 115200); + frame_id = mil_tools::getParam(getPrivateNodeHandle(), "frame_id"); - pub = getNodeHandle().advertise("depth", 10); + pub = getNodeHandle().advertise("depth", 10); device = boost::make_shared(port, baudrate); heartbeat_timer = getNodeHandle().createTimer( @@ -41,8 +41,8 @@ class Nodelet : public nodelet::Nodelet { void polling_thread() { while (running) { - uf_common::Float64Stamped msg; - if (!device->read(msg.data)) continue; + mil_msgs::DepthStamped msg; + if (!device->read(msg.depth)) continue; msg.header.stamp = ros::Time::now(); msg.header.frame_id = frame_id; pub.publish(msg); diff --git a/drivers/sub8_rdi_dvl/CMakeLists.txt b/drivers/sub8_rdi_dvl/CMakeLists.txt index afe8880f..8b3bd7d5 100644 --- a/drivers/sub8_rdi_dvl/CMakeLists.txt +++ b/drivers/sub8_rdi_dvl/CMakeLists.txt @@ -1,13 +1,13 @@ cmake_minimum_required(VERSION 2.8.3) project(rdi_explorer_dvl) -find_package(catkin REQUIRED COMPONENTS nodelet roscpp uf_common) +find_package(catkin REQUIRED COMPONENTS nodelet roscpp mil_msgs mil_tools) catkin_package( DEPENDS - CATKIN_DEPENDS nodelet roscpp uf_common + CATKIN_DEPENDS nodelet roscpp mil_msgs mil_tools INCLUDE_DIRS include LIBRARIES ) -include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) +include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} $(mil_tools_INCLUDE_DIRS)) add_library(rdi_explorer_dvl_nodelet src/nodelet.cpp) target_link_libraries(rdi_explorer_dvl_nodelet ${catkin_LIBRARIES}) add_dependencies(rdi_explorer_dvl_nodelet ${catkin_EXPORTED_TARGETS}) diff --git a/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h b/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h index 8524c280..18981cea 100644 --- a/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h +++ b/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h @@ -9,10 +9,10 @@ #include #include -#include +#include -#include -#include +#include +#include namespace rdi_explorer_dvl { static uint16_t getu16le(uint8_t *i) { return *i | (*(i + 1) << 8); } @@ -67,8 +67,8 @@ class Device { // open is called on first read() in the _polling_ thread } - void read(boost::optional &res, - boost::optional &height_res) { + void read(boost::optional &res, + boost::optional &height_res) { res = boost::none; height_res = boost::none; @@ -116,7 +116,7 @@ class Device { std::vector correlations(4, nan("")); if (section_id == 0x5803) { // Bottom Track High Resolution Velocity if (ensemble.size() - offset < 2 + 4 * 4) continue; - res = boost::make_optional(uf_common::VelocityMeasurements()); + res = boost::make_optional(mil_msgs::VelocityMeasurements()); res->header.stamp = stamp; std::vector dirs; @@ -124,13 +124,13 @@ class Device { double tilt = 30 * boost::math::constants::pi() / 180; double x = sin(tilt); double z = cos(tilt); - dirs.push_back(uf_common::make_xyz(-x, 0, -z)); - dirs.push_back(uf_common::make_xyz(+x, 0, -z)); - dirs.push_back(uf_common::make_xyz(0, +x, -z)); - dirs.push_back(uf_common::make_xyz(0, -x, -z)); + dirs.push_back(mil_tools::make_xyz(-x, 0, -z)); + dirs.push_back(mil_tools::make_xyz(+x, 0, -z)); + dirs.push_back(mil_tools::make_xyz(0, +x, -z)); + dirs.push_back(mil_tools::make_xyz(0, -x, -z)); } for (int i = 0; i < 4; i++) { - uf_common::VelocityMeasurement m; + mil_msgs::VelocityMeasurement m; m.direction = dirs[i]; int32_t vel = gets32le(ensemble.data() + offset + 2 + 4 * i); m.velocity = -vel * .01e-3; @@ -150,9 +150,9 @@ class Device { ROS_ERROR("DVL didn't return height over bottom"); continue; } - height_res = boost::make_optional(uf_common::Float64Stamped()); + height_res = boost::make_optional(mil_msgs::RangeStamped()); height_res->header.stamp = stamp; - height_res->data = gets32le(ensemble.data() + offset + 10) * 0.1e-3; + height_res->range = gets32le(ensemble.data() + offset + 10) * 0.1e-3; } if (res) { for (int i = 0; i < 4; i++) { diff --git a/drivers/sub8_rdi_dvl/package.xml b/drivers/sub8_rdi_dvl/package.xml index 9cd821e6..b785a96a 100644 --- a/drivers/sub8_rdi_dvl/package.xml +++ b/drivers/sub8_rdi_dvl/package.xml @@ -11,12 +11,14 @@ catkin nodelet roscpp - uf_common + mil_msgs + mil_tools nodelet roscpp - uf_common + mil_msgs + mil_tools - \ No newline at end of file + diff --git a/drivers/sub8_rdi_dvl/scripts/fake_dvl b/drivers/sub8_rdi_dvl/scripts/fake_dvl index af30a3cf..146c9a70 100755 --- a/drivers/sub8_rdi_dvl/scripts/fake_dvl +++ b/drivers/sub8_rdi_dvl/scripts/fake_dvl @@ -5,7 +5,7 @@ import math import rospy from std_msgs.msg import Header from geometry_msgs.msg import Vector3 -from uf_common.msg import VelocityMeasurements, VelocityMeasurement +from mil_msgs.msg import VelocityMeasurements, VelocityMeasurement rospy.init_node('fake_dvl') diff --git a/drivers/sub8_rdi_dvl/src/nodelet.cpp b/drivers/sub8_rdi_dvl/src/nodelet.cpp index 672d8eac..f3256e97 100644 --- a/drivers/sub8_rdi_dvl/src/nodelet.cpp +++ b/drivers/sub8_rdi_dvl/src/nodelet.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include "rdi_explorer_dvl/driver.h" @@ -23,14 +23,14 @@ class Nodelet : public nodelet::Nodelet { } virtual void onInit() { - std::string port = uf_common::getParam(getPrivateNodeHandle(), "port"); + std::string port = mil_tools::getParam(getPrivateNodeHandle(), "port"); - int baudrate = uf_common::getParam(getPrivateNodeHandle(), "baudrate", 115200); + int baudrate = mil_tools::getParam(getPrivateNodeHandle(), "baudrate", 115200); - frame_id = uf_common::getParam(getPrivateNodeHandle(), "frame_id"); + frame_id = mil_tools::getParam(getPrivateNodeHandle(), "frame_id"); - pub = getNodeHandle().advertise("dvl", 10); - range_pub = getNodeHandle().advertise("dvl/range", 10); + pub = getNodeHandle().advertise("dvl", 10); + range_pub = getNodeHandle().advertise("dvl/range", 10); device = boost::make_shared(port, baudrate); heartbeat_timer = getNodeHandle().createTimer( @@ -44,8 +44,8 @@ class Nodelet : public nodelet::Nodelet { void polling_thread() { while (running) { - boost::optional msg; - boost::optional range_msg; + boost::optional msg; + boost::optional range_msg; device->read(msg, range_msg); if (msg) { msg->header.frame_id = frame_id; diff --git a/drivers/sub8_sonar/CMakeLists.txt b/drivers/sub8_sonar/CMakeLists.txt deleted file mode 100644 index 15603797..00000000 --- a/drivers/sub8_sonar/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sub8_sonar) - -find_package(catkin REQUIRED COMPONENTS - rospy -) - -catkin_python_setup() - -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES sub8_sonar -# CATKIN_DEPENDS rospy -# DEPENDS system_lib -) - -include_directories( - ${catkin_INCLUDE_DIRS} -) diff --git a/drivers/sub8_sonar/README.md b/drivers/sub8_sonar/README.md deleted file mode 100644 index 7e808471..00000000 --- a/drivers/sub8_sonar/README.md +++ /dev/null @@ -1,16 +0,0 @@ -# Sonar Driver - -This script interfaces with Jake's sonar board and provides a ROS service that returns the location and time of emission of the last heard pinger pulse. - -### How to run and use -To start the driver, run: - - roslaunch sub8_sonar sonar.launch - -*Note: In /launch/sonar.launch, make sure that port and baud are set correctly, and that the hydrophones' coordinates in the sonar frame are accurate.* - -In order to ask for hydrophone information: - - rosservice call /sonar/get_pinger_pulse *double_tab* - -The service should respond with the x, y, z, and t of the emission of the last heard pinger pulse. diff --git a/drivers/sub8_sonar/launch/test.launch b/drivers/sub8_sonar/launch/test.launch deleted file mode 100644 index 7eff7b7f..00000000 --- a/drivers/sub8_sonar/launch/test.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - { hydro0: {x: 0, y: 0, z: 0}, - hydro1: {x: 0, y: 25.4, z: 0}, - hydro2: {x: -22, y: -12.7, z: 0}, - hydro3: {x: 22, y: -12.7, z: 0}} - - - \ No newline at end of file diff --git a/drivers/sub8_sonar/multilateration/__init__.py b/drivers/sub8_sonar/multilateration/__init__.py deleted file mode 100644 index 87a0c3ea..00000000 --- a/drivers/sub8_sonar/multilateration/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from multilateration import Multilaterator, ReceiverArraySim, Pulse diff --git a/drivers/sub8_sonar/multilateration/multilateration.py b/drivers/sub8_sonar/multilateration/multilateration.py deleted file mode 100644 index 1ef865c4..00000000 --- a/drivers/sub8_sonar/multilateration/multilateration.py +++ /dev/null @@ -1,212 +0,0 @@ -#!/usr/bin/python -from __future__ import division -import numpy as np -import numpy.linalg as la -from scipy import optimize -from itertools import combinations - -from sub8_msgs.srv import Sonar, SonarResponse - - -class Multilaterator(object): - ''' - Finds the origin location of a pulse given differential times of - arrival to the individual sensors. c is the wave speed in the medium of operation. - Units: - Hydrohone coordinates are expected in millimeters, pulse location will be given in millimeters. - Timestamps are expected in microseconds. c is expected in millimeters per microsecond - Note: - hydrophone locations should be the dict returned by rospy.get_param('~//hydrophones - ''' - def __init__(self, hydrophone_locations, c, method): # speed in millimeters/microsecond - self.hydrophone_locations = [] - for key in hydrophone_locations: - sensor_location = np.array([hydrophone_locations[key]['x'], hydrophone_locations[key]['y'], hydrophone_locations[key]['z']]) - self.hydrophone_locations += [sensor_location] - self.pairs = list(combinations(range(len(hydrophone_locations)),2)) - self.c = c - self.method = method - print "\x1b[32mSpeed of Sound (c):", self.c, "millimeter/microsecond\x1b[0m" - - def getPulseLocation(self, timestamps, method=None): - ''' - Returns a ros message with the location and time of emission of a pinger pulse. - ''' - try: - if method == None: - method = self.method - # print "\x1b[32mMultilateration algorithm:", method, "\x1b[0m" - assert len(self.hydrophone_locations) == len(timestamps) - source = None - if method == 'bancroft': - source = self.estimate_pos_bancroft(timestamps) - elif method == 'LS': - source = self.estimate_pos_LS(timestamps) - else: - print method, "is not an available multilateration algorithm" - return - response = SonarResponse() - response.x = source[0] - response.y = source[1] - response.z = source[2] - print "Reconstructed Pulse:\n\t" + "x: " + str(response.x) + " y: " + str(response.y) \ - + " z: " + str(response.z) + " (mm)" - return response - except KeyboardInterrupt: - print "Source localization interupted, returning all zeroes." - response = SonarResponse() - response.x, response.y, response.z = (0, 0, 0) - - - def estimate_pos_bancroft(self, reception_times): - N = len(reception_times) - assert N >= 4 - - L = lambda a, b: a[0]*b[0] + a[1]*b[1] + a[2]*b[2] - a[3]*b[3] - - def get_B(delta): - B = np.zeros((N, 4)) - for i in xrange(N): - B[i] = np.concatenate([self.hydrophone_locations[i]/(self.c), [-reception_times[i]]]) + delta - return B - - delta = min([.1*np.random.randn(4) for i in xrange(10)], key=lambda delta: np.linalg.cond(get_B(delta))) - # delta = np.zeros(4) # gives very good heading for noisy timestamps, although range is completely unreliable - - B = get_B(delta) - a = np.array([0.5 * L(B[i], B[i]) for i in xrange(N)]) - e = np.ones(N) - - Bpe = np.linalg.lstsq(B, e)[0] - Bpa = np.linalg.lstsq(B, a)[0] - - Lambdas = quadratic( - L(Bpe, Bpe), - 2*(L(Bpa, Bpe) - 1), - L(Bpa, Bpa)) - if not Lambdas: - return [0, 0, 0] - - res = [] - for Lambda in Lambdas: - u = Bpa + Lambda * Bpe - position = u[:3] - delta[:3] - time = u[3] + delta[3] - if any(reception_times[i] < time for i in xrange(N)): continue - res.append(position*self.c) - if len(res) == 1: - source = res[0] - elif len(res) == 2: - source = [x for x in res if x[2] < 0] # Assume that the source is below us - if not source: - source = res[0] - else: - source = source[0] - else: - source = [0, 0, 0] - return source - - def estimate_pos_LS(self, timestamps): - ''' - Returns a ros message with the location and time of emission of a pinger pulse. - ''' - self.timestamps = timestamps - init_guess = np.random.normal(0,100,3) - opt = {'disp': 0} - opt_method = 'Powell' - result = optimize.minimize(self.cost_LS, init_guess, method=opt_method, options=opt, tol=1e-15) - if(result.success): - source = [result.x[0], result.x[1], result.x[2]] - else: - source = [0, 0, 0] - return source - - def cost_LS(self, potential_pulse): - ''' - Compares the difference in observed and theoretical difference in time of arrival - between tevery unique pair of hydrophones. - - Note: the result will be along the direction of the heading but not at the right distance. - ''' - cost = 0 - t = self.timestamps - c = self.c - x = np.array(potential_pulse) - for pair in self.pairs: - h0 = self.hydrophone_locations[pair[0]] - h1 = self.hydrophone_locations[pair[1]] - residual = la.norm(x-h0) - la.norm(x-h1) - c*(t[pair[0]] - t[pair[1]]) - cost += residual**2 - return cost - - def cost_LS2(self, potential_pulse): - """ - Slightly less accurate than the one above in terms of heading but much faster. - """ - cost = 0 - t = self.timestamps - x0 = self.hydrophone_locations[0][0] - y0 = self.hydrophone_locations[0][1] - z0 = self.hydrophone_locations[0][2] - x = potential_pulse[0] - y = potential_pulse[1] - z = potential_pulse[2] - d0 = np.sqrt((x0 - x)**2 + (y0 - x)**2 + (z0 - x)**2) - for i in xrange(1, len(self.hydrophone_locations)): - xi = self.hydrophone_locations[i][0] - yi = self.hydrophone_locations[i][1] - zi = self.hydrophone_locations[i][2] - di = np.sqrt((xi - x)**2 + (yi - x)**2 + (zi - x)**2) - hydro_i_cost = (di - d0 - self.c * t[i])**2 - cost = cost + hydro_i_cost - return cost - - -class ReceiverArraySim(object): - """ - Simulates an array of receivers that listens to point sources and returns the DTOA. - (difference in time of arrival) - Base Units: - time - microseconds - length - millimeters - """ - def __init__(self, hydrophone_locations, wave_propagation_speed_mps): - self.c = wave_propagation_speed_mps - self.hydrophone_locations = np.array([0, 0, 0]) - for key in hydrophone_locations: - sensor_location = np.array([hydrophone_locations[key]['x'], hydrophone_locations[key]['y'], hydrophone_locations[key]['z']]) - self.hydrophone_locations = np.vstack((self.hydrophone_locations, sensor_location)) - self.hydrophone_locations = self.hydrophone_locations[1:] - - def listen(self, pulse): - timestamps = [] - for idx in range(4): - src_range = np.sqrt(sum(np.square(pulse.position() - self.hydrophone_locations[idx]))) - timestamps += [pulse.t + src_range / self.c] - return np.array(timestamps) - -class Pulse(object): - """ - Represents an omnidirectional wave or impulse emmited from a point source - """ - def __init__(self, x, y, z, t): - self.x = x - self.y = y - self.z = z - self.t = t - - def position(self): - return np.array([self.x, self.y, self.z]) - - def __repr__(self): - return "Pulse:\t" + "x: " + str(self.x) + " y: " + str(self.y) + " z: " \ - + str(self.z) + " (mm)" - - -def quadratic(a, b, c): - discriminant = b*b - 4*a*c - if discriminant >= 0: - first_times_a = (-b+math.copysign(math.sqrt(discriminant), -b))/2 - return [first_times_a/a, c/first_times_a] - else: - return [] \ No newline at end of file diff --git a/drivers/sub8_sonar/nodes/plotter.py b/drivers/sub8_sonar/nodes/plotter.py deleted file mode 100755 index 93d8f7ad..00000000 --- a/drivers/sub8_sonar/nodes/plotter.py +++ /dev/null @@ -1,34 +0,0 @@ -#!/usr/bin/env python -import rospy -import rosparam - -import numpy as np -from scipy import optimize - -from sub8_msgs.srv import Sonar, SonarResponse - -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -fig = plt.figure() -ax = fig.add_subplot(111, projection='3d') - -print "Waiting for sonar service" -rospy.wait_for_service('~/sonar/get_pinger_pulse') -sonar = rospy.ServiceProxy('~/sonar/get_pinger_pulse', Sonar) -print "sonar serv proxy created" - -try: - while(True): - try: - pinger_pose = sonar() - except Exception: - plt.close('all') - print "\nSonar driver is down, shutting down plotter" - break - print "x:", str(pinger_pose.x).rjust(15), "y:", str(pinger_pose.y).rjust(15), "z:", str(pinger_pose.z).rjust(15) - ax.scatter(pinger_pose.x, pinger_pose.y, pinger_pose.z) - plt.draw() - plt.pause(0.1) -except KeyboardInterrupt: - plt.close('all') - print "\nshutting down plotter" \ No newline at end of file diff --git a/drivers/sub8_sonar/nodes/sonar_test.py b/drivers/sub8_sonar/nodes/sonar_test.py deleted file mode 100755 index d4aa9108..00000000 --- a/drivers/sub8_sonar/nodes/sonar_test.py +++ /dev/null @@ -1,82 +0,0 @@ -#!/usr/bin/env python -from __future__ import division -import numpy as np -import numpy.linalg as la -from scipy import optimize -import rospy -import rosparam -import random -from multilateration import Multilaterator, ReceiverArraySim, Pulse -import sys - - -if __name__ == '__main__': - def print_green(str): - print '\x1b[32m' + str + '\x1b[0m' - - def error(obs, exp): - # Interesting, faster, but not as accurate - # alpha = np.arccos(np.clip(np.dot(obs/la.norm(obs),exp/la.norm(exp)),-1,1))*180/np.pi - alpha = 2*np.arctan2(la.norm(la.norm(exp)*obs-la.norm(obs)*exp),la.norm(la.norm(exp)*obs+la.norm(obs)*exp)) - mag_error = 100 * (la.norm(obs) - la.norm(exp)) / la.norm(exp) - return ('\x1b[31m' if (mag_error == -100) else "") + ("Errors: directional=" + str(alpha) + "deg").ljust(42) \ - + ("magnitude=" + str(mag_error) + "%").ljust(20) - - def delete_last_lines(n=0): - CURSOR_UP_ONE = '\x1b[1A' - ERASE_LINE = '\x1b[2K' - for _ in range(n): - sys.stdout.write(CURSOR_UP_ONE) - sys.stdout.write(ERASE_LINE) - - c = 1.484 # millimeters/microsecond - hydrophone_locations = rospy.get_param('~/sonar_test/hydrophones') - hydrophone_array = ReceiverArraySim(hydrophone_locations, c) - sonar = Multilaterator(hydrophone_locations, c, 'LS') - - # # Simulate individual pulses (Debugging Jakes Board) - # pulse = Pulse(-5251, -7620, 1470, 0) - # tstamps = hydrophone_array.listen(pulse) - # tstamps = tstamps - tstamps[0] - # print_green(pulse.__repr__()) - # print "Perfect timestamps: (microseconds)\n\t", tstamps - # res_msg = sonar.getPulseLocation(np.array(tstamps)) - # res = np.array([res_msg.x, res_msg.y, res_msg.z]) - # print "\t\x1b[33m".ljust(22) + error(res, pulse.position()) + "\x1b[0m" - - # pulses will be generated with inside a cube with side-length $(pulse_range) (mm) - try: - for h in range(3,8): - # smallest cube will be a meter wide, largest will be 10 km wide - pulse_range = 10**h # in mm - rand_args = [-pulse_range, pulse_range + 1] - num_pulses = 10 - print "\n\x1b[1mGenerating " + str(num_pulses) + " pulses within a " \ - + str(2*pulse_range/1000) + " meters wide cube\x1b[0m\n" - - for i in range(num_pulses): - pulse = Pulse(random.randrange(*rand_args), - random.randrange(*rand_args), - random.randrange(*rand_args), 0) - tstamps = hydrophone_array.listen(pulse) - tstamps = tstamps - tstamps[0] - print_green(str(i).ljust(2) + str(pulse)) - print "Perfect timestamps: (microseconds)\n\t", tstamps - res_msg = sonar.getPulseLocation(np.array(tstamps)) - delete_last_lines(4) # more concise output - res = np.array([res_msg.x, res_msg.y, res_msg.z]) - print "\t\x1b[33m".ljust(22) + error(res, pulse.position()) + "\x1b[0m" - print "Progressively adding noise to timestamps..." - - for j in range(-5, 2): - sigma = 10**j - noisy_tstamps = [x + np.random.normal(0, sigma) for x in tstamps] - noisy_tstamps[0] = 0 - print "Noisy timestamps:\n\t", noisy_tstamps - res_msg = sonar.getPulseLocation(np.array(noisy_tstamps)) - res = np.array([res_msg.x, res_msg.y, res_msg.z]) - delete_last_lines(4) # more concise output - print "\t\x1b[33m" + ("sigma: " + str(sigma)).ljust(16) \ - + error(res, pulse.position()) + "\x1b[0m" - except KeyboardInterrupt: - print "\nAborting mutilateration tests prematurely" diff --git a/drivers/sub8_sonar/package.xml b/drivers/sub8_sonar/package.xml deleted file mode 100644 index aaa335ae..00000000 --- a/drivers/sub8_sonar/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - sub8_sonar - 1.0.0 - The sub8_sonar package - - Matthew Langford - David Soto - - MIT - - catkin - rospy - rospy - - - - diff --git a/drivers/sub8_sonar/setup.py b/drivers/sub8_sonar/setup.py deleted file mode 100644 index 2486c904..00000000 --- a/drivers/sub8_sonar/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['sub8_sonar', 'multilateration'], -) - -setup(**setup_args) diff --git a/drivers/sub8_sonar/sub8_sonar/__init__.py b/drivers/sub8_sonar/sub8_sonar/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/drivers/sub8_sonar/sub8_sonar/sonar_driver.py b/drivers/sub8_sonar/sub8_sonar/sonar_driver.py deleted file mode 100755 index ddfb3f6a..00000000 --- a/drivers/sub8_sonar/sub8_sonar/sonar_driver.py +++ /dev/null @@ -1,305 +0,0 @@ -#!/usr/bin/python -from __future__ import division -import numpy as np -import numpy.linalg as la -from scipy.signal import resample, periodogram - -from itertools import product - -import rospy -import rosparam - -from sub8_msgs.srv import Sonar, SonarResponse -from sub8_ros_tools import thread_lock, make_header -from sub8_alarm import AlarmBroadcaster -from multilateration import Multilaterator - -import threading -import serial -import binascii -import struct -import time -import crc16 -import sys - -# temp -import matplotlib -matplotlib.use("WX") -import matplotlib.patches as mpatches -import matplotlib.pyplot as plt -fig = plt.figure(0) -fig2 =plt.figure(1) -plt.ion() -plt.show() - -lock = threading.Lock() - - -class Sub8Sonar(): - ''' - Smart sensor that provides high level ROS code with the location of pinger pulses detected with - Jake Easterling's Hydrophone board. - - TODO: Add a function to try and reconnect to the serial port if we lose connection. - TODO: Express pulse location in map frame - ''' - def __init__(self, method, c, hydrophone_locations, port, baud=19200): - rospy.init_node("sonar") - - alarm_broadcaster = AlarmBroadcaster() - self.disconnection_alarm = alarm_broadcaster.add_alarm( - name='sonar_disconnect', - action_required=True, - severity=0 - ) - self.packet_error_alarm = alarm_broadcaster.add_alarm( - name='sonar_packet_error', - action_required=False, - severity=2 - ) - - try: - self.ser = serial.Serial(port=port, baudrate=baud, timeout=None) - self.ser.flushInput() - except Exception, e: - print "\x1b[31mSonar serial connection error:\n\t", e, "\x1b[0m" - return None - - self.c = c - self.hydrophone_locations = [] - for key in hydrophone_locations: - sensor_location = np.array([hydrophone_locations[key]['x'], hydrophone_locations[key]['y'], hydrophone_locations[key]['z']]) - self.hydrophone_locations += [sensor_location] - self.multilaterator = Multilaterator(hydrophone_locations, self.c, method) # speed of sound in m/s - self.est_signal_freq_kHz = 0 - self._freq_sum = 0 - self._freq_samples = 0 - - rospy.Service('~/sonar/get_pinger_pulse', Sonar, self.get_pulse_from_raw_data) - print "\x1b[32mSub8 sonar driver initialized\x1b[0m" - rospy.spin() - - @thread_lock(lock) - def get_pulse_from_timestamps(self, srv): - self.ser.flushInput() - - try: - self.ser.write('A') - print "Sent timestamp request..." - except: # Except only serial errors in the future. - self.disconnection_alarm.raise_alarm( - problem_description="Sonar board serial connection has been terminated." - ) - return False - return self.multilaterator.getPulseLocation(self.timestamp_listener()) - - def timestamp_listener(self): - ''' - Parse the response of the board. - ''' - print "Listening..." - - # We've found a packet now disect it. - response = self.ser.read(19) - # rospy.loginfo("Received: %s" % response) #uncomment for debugging - if self.check_CRC(response): - delete_last_lines(0) # Heard response! - # The checksum matches the data so split the response into each piece of data. - # For info on what these letters mean: https://docs.python.org/2/library/struct.html#format-characters - data = struct.unpack('>BffffH', response) - timestamps = [data[4], data[1], data[2], data[3] ] - print "timestamps:", timestamps - return timestamps - else: - self.packet_error_alarm.raise_alarm( - problem_description="Sonar board checksum error.", - parameters={ - 'fault_info': {'data': response} - } - ) - return None - - @thread_lock(lock) - def get_pulse_from_raw_data(self, srv): - # request signals from hydrophone board - self.ser.flushInput() - try: - self.ser.write('B') - print "Sent raw data request..." - except: # Except only serial errors in the future. - self.disconnection_alarm.raise_alarm( - problem_description="Sonar board serial connection has been terminated." - ) - return False - return self.multilaterator.getPulseLocation(self.raw_data_listener()) - - def raw_data_listener(self): - ''' - Parse the response of the board. - ''' - print "Listening..." - - # prepare arrays for storing signals - signal_bias = 32767 - waves_recorded = 3 - samples_per_wave = 17.24 - upsample_scale = 3 - exp_recording_size = np.floor(samples_per_wave) * waves_recorded * upsample_scale - 1 - raw_signals = np.zeros([4, exp_recording_size], dtype=float) - - try: - # read in start bit of packet - timeout = 0 - start_bit = self.ser.read(1) - # If start bit not read, keep trying - while start_bit != '\xBB': - start_bit = self.ser.read(1) - timeout += 1 - if timeout > 600: - raise BufferError("Timeout: failed to read a start bit from sonar board") - for elem in np.nditer(raw_signals, op_flags=['readwrite']): - elem[...] = float(self.ser.read(5)) - signal_bias # ... is idx to current elem - except BufferError as e: - print e - - non_zero_end_idx = 57 - up_factor = 8 - sampling_freq = 430E3 * 0.8 # Hz - samp_period = 1E6 / sampling_freq # microseconds - upsamp_step = samp_period / up_factor - - # Set how much of each signal to actually use for processing - raw_signals = raw_signals[:, 0:non_zero_end_idx] - - # upsample the signals for successful cross-correlation - upsampled_signals = [resample(x, up_factor*len(x)) for x in raw_signals] - - # scale waves so they all have the same variance - equalized_signals = [x / np.std(x) for x in upsampled_signals] - raw_signals = [x / np.std(x) for x in raw_signals] - w0_upsamp, w1_upsamp, w2_upsamp, w3_upsamp = equalized_signals - t_up = np.arange(0, non_zero_end_idx*samp_period, upsamp_step) - - zero_crossings = np.where(np.diff(np.sign(w0_upsamp)))[0] - num_crossings = len(zero_crossings) - if num_crossings % 2 == 0: # we want an odd number of zero crossings - zero_crossings = zero_crossings[1:] - num_crossings -= 1 - waves_between_first_and_last_crossing = (num_crossings - 1) / 2 - time_between_first_and_last_crossing = t_up[zero_crossings[-1]] - t_up[zero_crossings[0]] - curr_signal_freq_kHz = 1E3 * waves_between_first_and_last_crossing / time_between_first_and_last_crossing # kHz - self._freq_sum += curr_signal_freq_kHz - self._freq_samples += 1 - self.est_signal_freq_kHz = self._freq_sum / self._freq_samples - - - # frequencies, spectrum = periodogram(w0_upsamp, sampling_freq * up_factor) - # signal_freq = frequencies[np.argmax(spectrum)] # Hz - # print "fft source frequency:", signal_freq, "Hz" - print "current source frequency:", curr_signal_freq_kHz, "kHz" - print "est source frequency:", self.est_signal_freq_kHz, "kHz" - # ax = fig2.add_subplot(111) - # ax.semilogy(frequencies,spectrum) - # ax.set_ylim([1e-7, 1e2]) - # ax.set_xlim([0, 6e4]) - # ax.set_xlabel('frequency [Hz]') - signal_period = 1E3 / self.est_signal_freq_kHz # microseconds - upsamples_recorded = len(w0_upsamp) - - waves_ref = 3.5 - waves_non_ref = 2.5 - samples_per_wave = signal_period / samp_period - ref_upsamples = int(round(waves_ref * samples_per_wave * up_factor)) - nonref_upsamples = int(np.ceil(waves_non_ref * samples_per_wave * up_factor)) - - ref_start_idx = None - if (len(w0_upsamp) % 2 == ref_upsamples % 2): - ref_start_idx = int(round((upsamples_recorded / 2) - (ref_upsamples / 2))) - else: - ref_start_idx = int(np.ceil((upsamples_recorded / 2) - (ref_upsamples / 2))) - ref_end_idx = ref_start_idx + ref_upsamples - 1 - nonref_end_idx = ref_start_idx + nonref_upsamples - 1 - - w0_select = w0_upsamp[ref_start_idx : ref_end_idx + 1] - w1_select = w1_upsamp[ref_start_idx : nonref_end_idx + 1] - w2_select = w2_upsamp[ref_start_idx : nonref_end_idx + 1] - w3_select = w3_upsamp[ref_start_idx : nonref_end_idx + 1] - t_ref_select = t_up[ref_start_idx : ref_end_idx + 1] - t_nonref_select = t_up[ref_start_idx : nonref_end_idx + 1] - - cc1 = np.correlate(w0_select, w1_select, mode='full') - cc2 = np.correlate(w0_select, w2_select, mode='full') - cc3 = np.correlate(w0_select, w3_select, mode='full') - corr_start = t_ref_select[0] - t_nonref_select[-1] - corr_end = t_ref_select[-1] - t_ref_select[0] + upsamp_step - t_corr = np.arange(corr_start, corr_end, upsamp_step) - print len(cc1), len(t_corr) - - fig.clf() - ax1 = fig.add_subplot(511) - ax2 = fig.add_subplot(512) - ax3 = fig.add_subplot(513) - ax4 = fig.add_subplot(514) - ax5 = fig.add_subplot(515) - axarr = [ax1, ax2, ax3, ax4, ax5] - - axarr[0].plot(t_up,w1_upsamp,'r',t_up,w2_upsamp,'g',t_up,w3_upsamp,'b',t_up,w0_upsamp,'k') - axarr[0].axis([0,60*samp_period,-3,3]) - axarr[0].set_title('Signals') - - axarr[1].plot(t_nonref_select, w1_select, 'r', - t_nonref_select, w2_select, 'g', - t_nonref_select, w3_select, 'b', - t_ref_select, w0_select, 'k') - axarr[1].set_title('Selected portions') - - axarr[2].plot(t_corr, cc1) - axarr[2].set_title('Hydrophone 1 cross-correlation') - axarr[3].plot(t_corr, cc2) - axarr[3].set_title('Hydrophone 2 cross-correlation') - axarr[4].plot(t_corr, cc3) - axarr[4].set_title('Hydrophone 3 cross-correlation') - - plt.draw() - plt.pause(0.1) - - return [0,0,0,0] #DBG - - def max_delta_t(hydrophone_idx_a, hydrophone_idx_b): - a = self.hydrophone_locations[hydrophone_idx_a] - b = self.hydrophone_locations[hydrophone_idx_b] - dist = la.norm(a - b) - return dist / self.c - - def CRC(self, message): - # You may have to change the checksum type. - # Check the crc16 module online to see how to do that. - crc = crc16.crc16xmodem(message, 0xFFFF) - return struct.pack('>H', crc) - - def check_CRC(self, message): - ''' - Given a message with a checksum as the last two bytes, this will return True or False - if the checksum matches the given message. - ''' - msg_checksum = message[-2:] - raw_message = message[:-2] - crc = crc16.crc16xmodem(raw_message, 0xFFFF) - - # If the two match the message was correct - if crc == struct.unpack('>H', msg_checksum)[0]: - return True - else: - return False - -def delete_last_lines(n=1): - CURSOR_UP_ONE = '\x1b[1A' - ERASE_LINE = '\x1b[2K' - for _ in range(n): - sys.stdout.write(CURSOR_UP_ONE) - sys.stdout.write(ERASE_LINE) - -if __name__ == "__main__": - d = Sub8Sonar('LS', 1.484, rospy.get_param('~/sonar_driver/hydrophones'), - "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_AH02X4IE-if00-port0", - 19200) \ No newline at end of file diff --git a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py index 827e6d51..dc33a626 100755 --- a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py +++ b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py @@ -7,7 +7,7 @@ import argparse from std_msgs.msg import Header, Float64 from sub8_msgs.msg import Thrust, ThrusterStatus -from sub8_ros_tools import wait_for_param, thread_lock +from mil_ros_tools import wait_for_param, thread_lock from sub8_msgs.srv import ThrusterInfo, ThrusterInfoResponse, FailThruster, FailThrusterResponse from sub8_thruster_comm import thruster_comm_factory from ros_alarms import AlarmBroadcaster, AlarmListener diff --git a/gnc/sub8_controller/nodes/base_controller.py b/gnc/sub8_controller/nodes/base_controller.py index e0ab5f5a..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 sub8_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/gnc/sub8_thruster_mapper/nodes/mapper.py b/gnc/sub8_thruster_mapper/nodes/mapper.py index fe41434d..d3c5451f 100755 --- a/gnc/sub8_thruster_mapper/nodes/mapper.py +++ b/gnc/sub8_thruster_mapper/nodes/mapper.py @@ -2,7 +2,7 @@ import rospy from scipy.optimize import minimize import numpy as np -from sub8_ros_tools import wait_for_param, thread_lock, rosmsg_to_numpy, msg_helpers +from mil_ros_tools import wait_for_param, thread_lock, rosmsg_to_numpy, msg_helpers import threading from sub8_msgs.msg import Thrust, ThrusterCmd from sub8_msgs.srv import (ThrusterInfo, UpdateThrusterLayout, UpdateThrusterLayoutResponse, diff --git a/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py index 2954a6d0..c994f2a9 100755 --- a/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py +++ b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py @@ -9,7 +9,7 @@ from sub8_msgs.msg import Thrust, ThrusterCmd from sub8_msgs.srv import ThrusterInfo, ThrusterInfoResponse from geometry_msgs.msg import WrenchStamped, Wrench, Vector3 -from sub8_ros_tools import wait_for_subscriber +from mil_ros_tools import wait_for_subscriber import rospy import rostest import time diff --git a/legacy/c3_trajectory_generator/CMakeLists.txt b/legacy/c3_trajectory_generator/CMakeLists.txt index 0905e93b..7167c184 100644 --- a/legacy/c3_trajectory_generator/CMakeLists.txt +++ b/legacy/c3_trajectory_generator/CMakeLists.txt @@ -8,10 +8,11 @@ find_package( actionlib message_runtime message_generation - uf_common + mil_msgs tf cmake_modules ros_alarms + mil_tools ) add_service_files( @@ -19,29 +20,32 @@ add_service_files( SetDisabled.srv ) -find_package(Eigen3 REQUIRED) -include_directories(${Eigen_INCLUDE_DIRS}) +find_package(Eigen 3 REQUIRED) + generate_messages( DEPENDENCIES ) catkin_package( DEPENDS - uf_common + mil_msgs + Eigen3 CATKIN_DEPENDS nav_msgs actionlib message_runtime message_generation - uf_common + mil_msgs tf ros_alarms + mil_tools INCLUDE_DIRS include + ${Eigen_INCLUDE_DIRS} LIBRARIES ) -include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) +include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS}) add_executable(c3_trajectory_generator src/C3Trajectory.cpp src/node.cpp src/AttitudeHelpers.cpp) target_link_libraries(c3_trajectory_generator ${catkin_LIBRARIES}) @@ -49,7 +53,7 @@ add_dependencies(c3_trajectory_generator ${catkin_EXPORTED_TARGETS}) add_dependencies(c3_trajectory_generator ${PROJECT_NAME}_generate_messages_cpp) add_dependencies( ${PROJECT_NAME} - uf_common_generate_messages_cpp + mil_msgs_generate_messages_cpp ${catkin_EXPORTED_TARGETS} ) set_target_properties(c3_trajectory_generator PROPERTIES COMPILE_FLAGS "-std=c++11 -O2") diff --git a/legacy/c3_trajectory_generator/package.xml b/legacy/c3_trajectory_generator/package.xml index c922eb68..573bde52 100644 --- a/legacy/c3_trajectory_generator/package.xml +++ b/legacy/c3_trajectory_generator/package.xml @@ -17,18 +17,20 @@ actionlib message_runtime message_generation - uf_common + mil_msgs tf ros_alarms cmake_modules + mil_tools nav_msgs actionlib message_runtime message_generation - uf_common + mil_msgs tf ros_alarms + mil_tools diff --git a/legacy/c3_trajectory_generator/src/node.cpp b/legacy/c3_trajectory_generator/src/node.cpp index 17091029..6d7b6658 100644 --- a/legacy/c3_trajectory_generator/src/node.cpp +++ b/legacy/c3_trajectory_generator/src/node.cpp @@ -4,19 +4,20 @@ #include #include -#include -#include -#include +#include +#include +#include #include -#include "uf_common/MoveToAction.h" +#include #include "c3_trajectory_generator/SetDisabled.h" #include "C3Trajectory.h" using namespace std; using namespace geometry_msgs; using namespace nav_msgs; -using namespace uf_common; +using namespace mil_msgs; +using namespace mil_tools; using namespace c3_trajectory_generator; subjugator::C3Trajectory::Point Point_from_PoseTwist(const Pose &pose, const Twist &twist) { @@ -81,7 +82,7 @@ struct Node { ros::Duration traj_dt; ros::Subscriber odom_sub; - actionlib::SimpleActionServer actionserver; + actionlib::SimpleActionServer actionserver; ros::Publisher trajectory_pub; ros::Publisher trajectory_vis_pub; ros::Publisher waypoint_pose_pub; @@ -122,16 +123,16 @@ struct Node { throw std::runtime_error("The kill listener isn't connected to the alarm server"); kill_listener.start(); // Fuck. - fixed_frame = uf_common::getParam(private_nh, "fixed_frame"); - body_frame = uf_common::getParam(private_nh, "body_frame"); + fixed_frame = mil_tools::getParam(private_nh, "fixed_frame"); + body_frame = mil_tools::getParam(private_nh, "body_frame"); - limits.vmin_b = uf_common::getParam(private_nh, "vmin_b"); - limits.vmax_b = uf_common::getParam(private_nh, "vmax_b"); - limits.amin_b = uf_common::getParam(private_nh, "amin_b"); - limits.amax_b = uf_common::getParam(private_nh, "amax_b"); - limits.arevoffset_b = uf_common::getParam(private_nh, "arevoffset_b"); - limits.umax_b = uf_common::getParam(private_nh, "umax_b"); - traj_dt = uf_common::getParam(private_nh, "traj_dt", ros::Duration(0.0001)); + limits.vmin_b = mil_tools::getParam(private_nh, "vmin_b"); + limits.vmax_b = mil_tools::getParam(private_nh, "vmax_b"); + limits.amin_b = mil_tools::getParam(private_nh, "amin_b"); + limits.amax_b = mil_tools::getParam(private_nh, "amax_b"); + limits.arevoffset_b = mil_tools::getParam(private_nh, "arevoffset_b"); + limits.umax_b = mil_tools::getParam(private_nh, "umax_b"); + traj_dt = mil_tools::getParam(private_nh, "traj_dt", ros::Duration(0.0001)); odom_sub = nh.subscribe("odom", 1, boost::bind(&Node::odom_callback, this, _1)); @@ -172,7 +173,7 @@ struct Node { ros::Time now = ros::Time::now(); if (actionserver.isNewGoalAvailable()) { - boost::shared_ptr goal = actionserver.acceptNewGoal(); + boost::shared_ptr goal = actionserver.acceptNewGoal(); current_waypoint = subjugator::C3Trajectory::Waypoint( Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist), goal->speed, !goal->uncoordinated); diff --git a/legacy/rise_6dof/CMakeLists.txt b/legacy/rise_6dof/CMakeLists.txt index cbd293d2..15deae27 100644 --- a/legacy/rise_6dof/CMakeLists.txt +++ b/legacy/rise_6dof/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(rise_6dof) -find_package(catkin REQUIRED COMPONENTS nav_msgs dynamic_reconfigure message_runtime message_generation rospy uf_common tf geometry_msgs) +find_package(catkin REQUIRED COMPONENTS nav_msgs dynamic_reconfigure message_runtime message_generation rospy mil_msgs tf geometry_msgs) catkin_python_setup() add_service_files( @@ -18,7 +18,7 @@ generate_messages( catkin_package( DEPENDS - CATKIN_DEPENDS nav_msgs dynamic_reconfigure message_runtime message_generation rospy uf_common tf geometry_msgs + CATKIN_DEPENDS nav_msgs dynamic_reconfigure message_runtime message_generation rospy mil_msgs tf geometry_msgs INCLUDE_DIRS LIBRARIES ) diff --git a/legacy/rise_6dof/package.xml b/legacy/rise_6dof/package.xml index d8cbaf12..356b0674 100644 --- a/legacy/rise_6dof/package.xml +++ b/legacy/rise_6dof/package.xml @@ -18,7 +18,7 @@ dynamic_reconfigure message_runtime rospy - uf_common + mil_msgs tf message_generation geometry_msgs @@ -28,7 +28,7 @@ dynamic_reconfigure message_runtime rospy - uf_common + mil_msgs tf message_generation geometry_msgs diff --git a/legacy/rise_6dof/scripts/do_spiral b/legacy/rise_6dof/scripts/do_spiral index 64e510ad..7ce0680a 100755 --- a/legacy/rise_6dof/scripts/do_spiral +++ b/legacy/rise_6dof/scripts/do_spiral @@ -14,8 +14,8 @@ from std_msgs.msg import Header from geometry_msgs.msg import Pose, Twist, Vector3, Quaternion, Point from tf import transformations -from uf_common import orientation_helpers -from uf_common.msg import PoseTwist, PoseTwistStamped +from mil_msgs import orientation_helpers +from mil_msgs.msg import PoseTwist, PoseTwistStamped rospy.init_node('do_spiral') diff --git a/legacy/rise_6dof/scripts/joy_to_posetwist b/legacy/rise_6dof/scripts/joy_to_posetwist index a12fd04e..5231fe0f 100755 --- a/legacy/rise_6dof/scripts/joy_to_posetwist +++ b/legacy/rise_6dof/scripts/joy_to_posetwist @@ -13,8 +13,8 @@ from std_msgs.msg import Header from nav_msgs.msg import Odometry from tf import transformations -from uf_common.msg import PoseTwist, PoseTwistStamped -from uf_common.orientation_helpers import xyz_array, xyzw_array, PoseEditor +from mil_msgs.msg import PoseTwist, PoseTwistStamped +from mil_msgs.orientation_helpers import xyz_array, xyzw_array, PoseEditor roslib.load_manifest('c3_trajectory_generator') from c3_trajectory_generator.srv import SetDisabled diff --git a/legacy/rise_6dof/scripts/rise_6dof b/legacy/rise_6dof/scripts/rise_6dof index 4a13fb12..81ee8069 100755 --- a/legacy/rise_6dof/scripts/rise_6dof +++ b/legacy/rise_6dof/scripts/rise_6dof @@ -12,8 +12,8 @@ from geometry_msgs.msg import Vector3, Wrench, WrenchStamped from nav_msgs.msg import Odometry from std_msgs.msg import Header -from uf_common.msg import PoseTwist, PoseTwistStamped -from sub8_ros_tools import pose_to_numpy, twist_to_numpy +from mil_msgs.msg import PoseTwist, PoseTwistStamped +from mil_ros_tools import pose_to_numpy, twist_to_numpy from ros_alarms import AlarmListener diff --git a/legacy/rise_6dof/scripts/test b/legacy/rise_6dof/scripts/test index 54ddaaf2..7528be68 100755 --- a/legacy/rise_6dof/scripts/test +++ b/legacy/rise_6dof/scripts/test @@ -11,7 +11,7 @@ roslib.load_manifest("rise_6dof") from geometry_msgs.msg import Point, Vector3, Quaternion, Pose, Twist from tf import transformations -from uf_common.msg import PoseTwist +from mil_msgs.msg import PoseTwist from rise_6dof.controller import Controller diff --git a/legacy/rise_6dof/scripts/test_rviz b/legacy/rise_6dof/scripts/test_rviz index d68536fc..f2afe1e8 100755 --- a/legacy/rise_6dof/scripts/test_rviz +++ b/legacy/rise_6dof/scripts/test_rviz @@ -14,8 +14,8 @@ from nav_msgs.msg import Odometry from std_msgs.msg import Header, ColorRGBA from tf import transformations -from uf_common.msg import PoseTwist, PoseTwistStamped -from uf_common.orientation_helpers import xyz_array, xyzw_array +from mil_msgs.msg import PoseTwist, PoseTwistStamped +from mil_msgs.orientation_helpers import xyz_array, xyzw_array def makeBoxControl(): diff --git a/perception/sub8_perception/CMakeLists.txt b/perception/sub8_perception/CMakeLists.txt index 69b1fb9e..3d0ae71e 100644 --- a/perception/sub8_perception/CMakeLists.txt +++ b/perception/sub8_perception/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(catkin message_generation std_msgs geometry_msgs + mil_vision ) catkin_python_setup() @@ -58,7 +59,6 @@ include_directories( add_library(sub8_vision_lib # src/sub8_vision_lib/align.cpp - src/sub8_vision_lib/cv_utils.cpp # src/sub8_vision_lib/cv_param_helpers.cpp src/sub8_vision_lib/visualization.cpp # src/sub8_vision_lib/object_finder.cpp @@ -188,7 +188,6 @@ target_link_libraries( # target_link_libraries( # pcl_buoy -# sub8_vision_lib # ${catkin_LIBRARIES} # ${PCL_COMMON_LIBRARIES} # ${PCL_IO_LIBRARIES} diff --git a/perception/sub8_perception/include/sub8_perception/buoy.hpp b/perception/sub8_perception/include/sub8_perception/buoy.hpp index 7fe0a9bb..3c7ce146 100644 --- a/perception/sub8_perception/include/sub8_perception/buoy.hpp +++ b/perception/sub8_perception/include/sub8_perception/buoy.hpp @@ -23,8 +23,8 @@ #include #include -#include -#include +#include +#include #include "ros/ros.h" diff --git a/perception/sub8_perception/include/sub8_vision_lib/cv_tools.hpp b/perception/sub8_perception/include/sub8_vision_lib/cv_tools.hpp deleted file mode 100644 index 9d625037..00000000 --- a/perception/sub8_perception/include/sub8_vision_lib/cv_tools.hpp +++ /dev/null @@ -1,177 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include - -#include -#include -#include -#include - -// #define SEGMENTATION_DEBUG - -namespace sub { - -/// UTILS - -//template -//bool pseudoInverse( -// const _Matrix_Type_ &a, _Matrix_Type_ &result, -// double epsilon = -// std::numeric_limits::epsilon()); - -typedef std::vector Contour; - -// Compute the centroid of an OpenCV contour (Not templated) -cv::Point contour_centroid(Contour &contour); - -// Used as a comparison function for std::sort(), sorting in order of decreasing -// perimeters -// returns true if the contourArea(c1) > contourArea(c2) -bool larger_contour(const Contour &c1, const Contour &c2); - -// Filter a histogram of type cv::MatND generated by cv::calcHist using a -// gaussian kernel -cv::MatND smooth_histogram(const cv::MatND &histogram, - size_t filter_kernel_size = 3, float sigma = 1.0); - -// Generate a one-dimensional gaussian kernel given a kernel size and it's -// standard deviation -// (sigma) -std::vector generate_gaussian_kernel_1D(size_t kernel_size = 3, - float sigma = 1.0); - -// Finds positive local maxima greater than (global maximum * thresh_multiplier) -std::vector find_local_maxima(const cv::MatND &histogram, - float thresh_multiplier); - -// Finds negative local minima less than (global minimum * thresh_multiplier) -std::vector find_local_minima(const cv::MatND &histogram, - float thresh_multiplier); - -// Selects the mode of a multi-modal distribution closest to a given target -// value -unsigned int select_hist_mode(std::vector &histogram_modes, - unsigned int target); - -// Takes in a grayscale image and segments out a semi-homogenous foreground -// object with pixel intensities close to . Tuning of last three -// parameters may imrove results but default values should work well in -// most cases. -void statistical_image_segmentation(const cv::Mat &src, cv::Mat &dest, - cv::Mat &debug_img, const int hist_size, - const float **ranges, const int target, - std::string image_name = "Unnamed Image", - bool ret_dbg_img = false, - const float sigma = 1.5, - const float low_thresh_gain = 0.5, - const float high_thresh_gain = 0.5); - -cv::Mat triangulate_Linear_LS(cv::Mat mat_P_l, cv::Mat mat_P_r, - cv::Mat undistorted_l, cv::Mat undistorted_r); - -Eigen::Vector3d kanatani_triangulation(const cv::Point2d &pt1, - const cv::Point2d &pt2, - const Eigen::Matrix3d &essential, - const Eigen::Matrix3d &R); - -Eigen::Vector3d lindstrom_triangulation(const cv::Point2d &pt1, - const cv::Point2d &pt2, - const Eigen::Matrix3d &essential, - const Eigen::Matrix3d &R); - -struct ImageWithCameraInfo { - /** - Packages corresponding sensor_msgs::ImageConstPtr and - sensor_msgs::CameraInfoConstPtr - info_msg - into one object. Containers of these objects can be sorted by their - image_time attribute - */ -public: - ImageWithCameraInfo() {} - ImageWithCameraInfo(sensor_msgs::ImageConstPtr _image_msg_ptr, - sensor_msgs::CameraInfoConstPtr _info_msg_ptr); - sensor_msgs::ImageConstPtr image_msg_ptr; - sensor_msgs::CameraInfoConstPtr info_msg_ptr; - ros::Time image_time; - bool operator<(const ImageWithCameraInfo &right) const { - return this->image_time < right.image_time; - } -}; - -class FrameHistory { - /** - Object that subscribes itself to an image topic and stores up to a - user defined - number of ImageWithCameraInfo objects. The frame history can then be - retrieved - in whole or just a portion. - */ -public: - FrameHistory(std::string img_topic, unsigned int hist_size); - ~FrameHistory(); - void image_callback(const sensor_msgs::ImageConstPtr &image_msg, - const sensor_msgs::CameraInfoConstPtr &info_msg); - std::vector - get_frame_history(unsigned int frames_requested); - int frames_available(); - - const std::string topic_name; - const size_t history_size; - -private: - ros::NodeHandle nh; - image_transport::CameraSubscriber _image_sub; - image_transport::ImageTransport _image_transport; - std::vector _frame_history_ring_buffer; - size_t frame_count; -}; - -/// Param Helpers - -struct Range { - cv::Scalar lower; - cv::Scalar upper; -}; - -void range_from_param(std::string ¶m_root, Range &range); - -void inParamRange(cv::Mat &src, Range &range, cv::Mat &dest); - -/// Templated pseudoinverse function implementation -template -bool pseudoInverse( - const _Matrix_Type_ &a, _Matrix_Type_ &result, - double epsilon = - std::numeric_limits::epsilon()) { - if (a.rows() < a.cols()) - return false; - - Eigen::JacobiSVD<_Matrix_Type_> svd = a.jacobiSvd(); - - typename _Matrix_Type_::Scalar tolerance = - epsilon * std::max(a.cols(), a.rows()) * - svd.singularValues().array().abs().maxCoeff(); - - result = svd.matrixV() * - _Matrix_Type_( - _Matrix_Type_((svd.singularValues().array().abs() > tolerance) - .select(svd.singularValues().array().inverse(), - 0)).diagonal()) * - svd.matrixU().adjoint(); -} -} // namespace sub diff --git a/perception/sub8_perception/include/sub8_vision_lib/object_finder.hpp b/perception/sub8_perception/include/sub8_vision_lib/object_finder.hpp index 878d13cd..335403df 100644 --- a/perception/sub8_perception/include/sub8_vision_lib/object_finder.hpp +++ b/perception/sub8_perception/include/sub8_vision_lib/object_finder.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/perception/sub8_perception/include/sub8_vision_lib/torpedo_board.hpp b/perception/sub8_perception/include/sub8_vision_lib/torpedo_board.hpp index 8b543b57..94e2ce99 100644 --- a/perception/sub8_perception/include/sub8_vision_lib/torpedo_board.hpp +++ b/perception/sub8_perception/include/sub8_vision_lib/torpedo_board.hpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include #include @@ -57,8 +57,8 @@ class Sub8TorpedoBoardDetector { // Public Variables double image_proc_scale, feature_min_distance; int diffusion_time, max_features, feature_block_size; - sub::Contour left_corners, right_corners; - sub::ImageWithCameraInfo left_most_recent, right_most_recent; + mil_vision::Contour left_corners, right_corners; + mil_vision::ImageWithCameraInfo left_most_recent, right_most_recent; private: // Callbacks @@ -175,4 +175,4 @@ void calc_plane_coeffs(Eigen::Vector3d &pt1, Eigen::Vector3d &pt2, Eigen::Vector // Calculate distance of point to plane defined by coefficients of the plane equation of the // form a*x + b*y + c*z + d = 0 -double point_to_plane_distance(double a, double b, double c, double d, Eigen::Vector3d pt); \ No newline at end of file +double point_to_plane_distance(double a, double b, double c, double d, Eigen::Vector3d pt); diff --git a/perception/sub8_perception/nodes/bins_2d.py b/perception/sub8_perception/nodes/bins_2d.py index f1b4aec9..43b67b2a 100755 --- a/perception/sub8_perception/nodes/bins_2d.py +++ b/perception/sub8_perception/nodes/bins_2d.py @@ -4,11 +4,11 @@ import sys import rospy import image_geometry -import sub8_ros_tools +import mil_ros_tools from sub8_msgs.srv import VisionRequest2DResponse, VisionRequest2D from std_msgs.msg import Header from geometry_msgs.msg import Pose2D -from uf_common.msg import Float64Stamped # This needs to be deprecated +from mil_msgs.msg import RangeStamped def contour_sort(l): @@ -54,10 +54,10 @@ def __init__(self): self.last_image_time = None self.camera_model = None self.pose_service = rospy.Service('vision/bin/2D', VisionRequest2D, self.request_bin) - self.image_sub = sub8_ros_tools.Image_Subscriber('/down/left/image_rect_color', self.image_cb) - self.image_pub = sub8_ros_tools.Image_Publisher('/vision/bin_2d/target_info') + self.image_sub = mil_ros_tools.Image_Subscriber('/down/left/image_rect_color', self.image_cb) + self.image_pub = mil_ros_tools.Image_Publisher('/vision/bin_2d/target_info') self.range = None - self.range_sub = rospy.Subscriber("dvl/range", Float64Stamped, self.range_callback) + self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback) # Occasional status publisher self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_target_info) @@ -76,7 +76,7 @@ def request_bin(self, srv): if response is False or response is None: print 'did not find' resp = VisionRequest2DResponse( - header=sub8_ros_tools.make_header(frame='/down'), + header=mil_ros_tools.make_header(frame='/down'), found=False ) @@ -177,7 +177,7 @@ def find_single_bin(self, img, bin_type): def range_callback(self, msg): '''Handle range data grabbed from dvl''' frame = '/dvl' - self.range = msg.data + self.range = msg.range def find_bins(self, img, srv): draw_image = np.copy(img) diff --git a/perception/sub8_perception/nodes/buoy_2d.py b/perception/sub8_perception/nodes/buoy_2d.py index 0cc2b700..9f6789e6 100755 --- a/perception/sub8_perception/nodes/buoy_2d.py +++ b/perception/sub8_perception/nodes/buoy_2d.py @@ -4,7 +4,7 @@ import sys import rospy import image_geometry -import sub8_ros_tools +import mil_ros_tools import tf from sub8_vision_tools import machine_learning import rospkg @@ -297,7 +297,7 @@ def find_single_buoy(self, img, timestamp, buoy_type): (t, rot_q) = self.transformer.lookupTransform('/map', '/front_left_cam', timestamp) trans = np.array(t) - R = sub8_ros_tools.geometry_helpers.quaternion_matrix(rot_q) + R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q) self.rviz.draw_ray_3d(tuple_center, self.camera_model, buoy.draw_colors, frame='/front_left_cam', _id=self._id + 100, timestamp=timestamp) diff --git a/perception/sub8_perception/nodes/ground_finder.py b/perception/sub8_perception/nodes/ground_finder.py index 9b0e8fd0..b71accce 100755 --- a/perception/sub8_perception/nodes/ground_finder.py +++ b/perception/sub8_perception/nodes/ground_finder.py @@ -2,7 +2,7 @@ import rospy import numpy as np import tf -import sub8_ros_tools +import mil_ros_tools from sensor_msgs.msg import PointCloud @@ -12,7 +12,7 @@ listener = tf.TransformListener() pc = PointCloud() -pc.header = sub8_ros_tools.make_header(frame="map") +pc.header = mil_ros_tools.make_header(frame="map") pc.points = [] rate = rospy.Rate(1.0) @@ -24,7 +24,7 @@ rospy.logwarn("TF waitForTransform timeout") continue - pc.points.append(sub8_ros_tools.numpy_to_point(np.array(trans))) + pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans))) pub.publish(pc) rate.sleep() diff --git a/perception/sub8_perception/nodes/ocr_server.py b/perception/sub8_perception/nodes/ocr_server.py index 408c5b3c..5fbfad55 100755 --- a/perception/sub8_perception/nodes/ocr_server.py +++ b/perception/sub8_perception/nodes/ocr_server.py @@ -5,7 +5,7 @@ import sys import rospy import image_geometry -import sub8_ros_tools +import mil_ros_tools from sub8_msgs.srv import OcrRequest from std_msgs.msg import Header, String from geometry_msgs.msg import Pose2D diff --git a/perception/sub8_perception/nodes/path_marker_finder.py b/perception/sub8_perception/nodes/path_marker_finder.py index a8c4d223..d053d4b2 100755 --- a/perception/sub8_perception/nodes/path_marker_finder.py +++ b/perception/sub8_perception/nodes/path_marker_finder.py @@ -7,8 +7,8 @@ from std_srvs.srv import SetBool, SetBoolResponse from sub8_msgs.srv import VisionRequestResponse, VisionRequest, VisionRequest2D, VisionRequest2DResponse, VisionRequest, VisionRequestResponse from geometry_msgs.msg import PoseStamped -from sub8_ros_tools import numpy_quat_pair_to_pose, numpy_to_point, numpy_matrix_to_quaternion, numpy_to_quaternion -from sub8_ros_tools import Image_Publisher, Image_Subscriber +from mil_ros_tools import numpy_quat_pair_to_pose, numpy_to_point, numpy_matrix_to_quaternion, numpy_to_quaternion +from mil_ros_tools import Image_Publisher, Image_Subscriber from image_geometry import PinholeCameraModel from visualization_msgs.msg import Marker diff --git a/perception/sub8_perception/nodes/perception_server.cpp b/perception/sub8_perception/nodes/perception_server.cpp index f40d7802..4190c1b1 100644 --- a/perception/sub8_perception/nodes/perception_server.cpp +++ b/perception/sub8_perception/nodes/perception_server.cpp @@ -1,4 +1,4 @@ -#include +#include /////////////////////////////////////////////////////////////////////////////////////////////////// // Main /////////////////////////////////////////////////////////////////////////////////////////// diff --git a/perception/sub8_perception/nodes/torpedo_board.cpp b/perception/sub8_perception/nodes/torpedo_board.cpp index b309f4fc..0c6ec714 100644 --- a/perception/sub8_perception/nodes/torpedo_board.cpp +++ b/perception/sub8_perception/nodes/torpedo_board.cpp @@ -368,7 +368,7 @@ void Sub8TorpedoBoardDetector::determine_torpedo_board_position() { Matx31d pt_L_hom(pt_L.x, pt_L.y, 1); Matx31d pt_R_hom(pt_R.x, pt_R.y, 1); - Mat X_hom = sub::triangulate_Linear_LS(Mat(left_cam_mat), + Mat X_hom = mil_vision::triangulate_Linear_LS(Mat(left_cam_mat), Mat(right_cam_mat), Mat(pt_L_hom), Mat(pt_R_hom)); X_hom = X_hom / X_hom.at(3, 0); @@ -787,7 +787,7 @@ void Sub8TorpedoBoardDetector::determine_torpedo_board_position() { Point2d pt_R_ = right_corners[i]; Matx31d pt_L(pt_L_.x, pt_L_.y, 1); Matx31d pt_R(pt_R_.x, pt_R_.y, 1); - Mat X_hom = sub::triangulate_Linear_LS(Mat(left_cam_mat), + Mat X_hom = mil_vision::triangulate_Linear_LS(Mat(left_cam_mat), Mat(right_cam_mat), Mat(pt_L), Mat(pt_R)); X_hom = X_hom / X_hom.at(3, 0); @@ -997,10 +997,10 @@ void Sub8TorpedoBoardDetector::segment_board(const Mat &src, Mat &dest, Mat threshed_hue, threshed_sat, segmented_board; int target_yellow = 20; int target_saturation = 180; - sub::statistical_image_segmentation( + mil_vision::statistical_image_segmentation( hsv_channels[0], threshed_hue, hue_segment_dbg_img, hist_size, ranges, target_yellow, "Hue", draw_dbg_img, 6, 3.0, 3.0); - sub::statistical_image_segmentation( + mil_vision::statistical_image_segmentation( hsv_channels[1], threshed_sat, sat_segment_dbg_img, hist_size, ranges, target_saturation, "Saturation", draw_dbg_img, 6, 0.1, 0.1); #ifdef SEGMENTATION_DEBUG @@ -1052,7 +1052,7 @@ bool Sub8TorpedoBoardDetector::find_board_corners( // Put longest 2 contours at beginning of "contours" vector if (contours.size() > 2) { partial_sort(contours.begin(), contours.begin() + 2, contours.end(), - sub::larger_contour); + mil_vision::larger_contour); } if (contours.size() < 2) { // prevent out of range access of contours corners_success = false; @@ -1075,8 +1075,8 @@ bool Sub8TorpedoBoardDetector::find_board_corners( } // Connect yellow pannels - Point pt1 = sub::contour_centroid(contours[0]); - Point pt2 = sub::contour_centroid(contours[1]); + Point pt1 = mil_vision::contour_centroid(contours[0]); + Point pt2 = mil_vision::contour_centroid(contours[1]); convex_hull_working_img = Mat::zeros(convex_hull_working_img.size(), CV_8UC1); drawContours(convex_hull_working_img, contours, 0, Scalar(255)); @@ -1089,7 +1089,7 @@ bool Sub8TorpedoBoardDetector::find_board_corners( if (contours.size() > 1) { partial_sort(connected_contours.begin(), connected_contours.begin() + 1, connected_contours.end(), - sub::larger_contour); + mil_vision::larger_contour); } // Find convex hull of connected panels diff --git a/perception/sub8_perception/nodes/torpedo_board_pose_est.py b/perception/sub8_perception/nodes/torpedo_board_pose_est.py index 4ae6bb9d..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 sub8_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/package.xml b/perception/sub8_perception/package.xml index e8cb88cd..dcbbdd69 100644 --- a/perception/sub8_perception/package.xml +++ b/perception/sub8_perception/package.xml @@ -28,6 +28,7 @@ geometry_msgs geometry_msgs rostest + mil_vision diff --git a/perception/sub8_perception/ros_tools/easy_thresh.py b/perception/sub8_perception/ros_tools/easy_thresh.py deleted file mode 100755 index 5fb36d6e..00000000 --- a/perception/sub8_perception/ros_tools/easy_thresh.py +++ /dev/null @@ -1,162 +0,0 @@ -#!/usr/bin/python -# PYTHON_ARGCOMPLETE_OK - -import argcomplete -import sys -import argparse -import rospy - -all_topics = rospy.get_published_topics() -topics = [topic[0] for topic in all_topics if topic[1] == 'sensor_msgs/Image'] - -usage_msg = ("Name an image topic, we'll subscribe to it and grab the first image we can. " + - "Then click a rectangle around the area of interest") -desc_msg = "A tool for making threshold determination fun!" - -parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg) -parser.add_argument(dest='topic_name', - help="The topic name you'd like to listen to", - choices=topics) -parser.add_argument('--hsv', action='store_true', - help="Would you like to look at hsv instead of bgr?" - ) - -argcomplete.autocomplete(parser) -args = parser.parse_args(sys.argv[1:]) -if args.hsv: - print 'Using HSV instead of bgr' -prefix = 'hsv' if args.hsv else 'bgr' - -# Importing these late so that argcomplete can run quickly -from sub8_vision_tools import visual_threshold_tools # noqa -from sub8_ros_tools.image_helpers import Image_Subscriber # noqa -import cv2 # noqa -import numpy as np # noqa -import os # noqa -from sklearn import cluster # noqa -os.system("export ETS_TOOLKIT=qt4") - - -class Segmenter(object): - def __init__(self): - self.is_done = False - self.corners = [] - self.state = 0 - - def mouse_cb(self, event, x, y, flags, param): - if event == cv2.EVENT_LBUTTONDOWN: - if not self.is_done: - print 'click' - self.corners.append(np.array([x, y])) - self.state += 1 - if self.state >= 4: - print 'done' - self.is_done = True - self.state = 0 - - if event == cv2.EVENT_RBUTTONDOWN: - pass - - def segment(self): - while(not self.is_done and not rospy.is_shutdown()): - if cv2.waitKey(50) & 0xFF == ord('q'): - break - - self.is_done = False - rect = cv2.minAreaRect(np.array([np.array(self.corners)])) - box = cv2.cv.BoxPoints(rect) - box = np.int0(box) - return box - - -class ImageGetter(object): - def __init__(self, topic_name='/down/left/image_raw'): - self.sub = Image_Subscriber(topic_name, self.get_image) - - print 'getting topic', topic_name - self.frame = None - self.done = False - - def get_image(self, msg): - self.frame = msg - self.done = True - - def wait_for_image(self): - while not self.done and not rospy.is_shutdown(): - if cv2.waitKey(50) & 0xFF == ord('q'): - exit() - - -if __name__ == '__main__': - rospy.init_node('easy_thresh') - - # Do the import after arg parse - - ig = ImageGetter(args.topic_name) - ig.wait_for_image() - print 'Got image' - frame_initial = np.copy(ig.frame) - - cv2.namedWindow("color") - seg = Segmenter() - cv2.setMouseCallback("color", seg.mouse_cb) - - frame_unblurred = frame_initial[::2, ::2, :] - frame = frame_unblurred - - if args.hsv: - analysis_image = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - else: - analysis_image = frame - - draw_image = np.copy(frame_unblurred) - seg_image = np.zeros_like(frame_unblurred[:, :, 0]) - - cv2.imshow("color", frame_unblurred) - - box = seg.segment() - print 'finished' - - cv2.drawContours(seg_image, [box], 0, 1, -2) - - hsv_in_box = analysis_image[seg_image.astype(np.bool)] - hsv_list = np.reshape(hsv_in_box, (-1, 3)) - - clust = cluster.KMeans(n_clusters=2) - print 'done clustering' - - clust.fit(hsv_list) - - hsv_dsamp = hsv_list - labels_dsamp = clust.labels_ - - visual_threshold_tools.points_with_labels( - hsv_dsamp[:, 0], - hsv_dsamp[:, 1], - hsv_dsamp[:, 2], - labels_dsamp, - scale_factor=5.0, - ) - - thresholder = visual_threshold_tools.make_extent_dialog( - ranges=visual_threshold_tools.color_ranges[prefix], - image=analysis_image - ) - - while not rospy.is_shutdown(): - if cv2.waitKey(50) & 0xFF == ord('q'): - break - - ranges = thresholder.ranges - labels = visual_threshold_tools.np_inrange(hsv_dsamp, ranges[:, 0], ranges[:, 1]) - - # Print out thresholds that can be put in the configuration yaml - low = ranges[:, 0] - print ' {}_low: [{}, {}, {}]'.format(prefix, low[0], low[1], low[2]) - - high = ranges[:, 1] - print ' {}_high: [{}, {}, {}]'.format(prefix, high[0], high[1], high[2]) - - print 'np.' + repr(ranges) - - cv2.destroyAllWindows() diff --git a/perception/sub8_perception/ros_tools/image_dumper.py b/perception/sub8_perception/ros_tools/image_dumper.py deleted file mode 100755 index 317db72a..00000000 --- a/perception/sub8_perception/ros_tools/image_dumper.py +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/python - -import os -import re -import sys -import fnmatch -import argparse -from tqdm import tqdm - -import cv2 -import rospy -import roslib -import rosbag - -from cv_bridge import CvBridge, CvBridgeError - -# encoding=utf8 -reload(sys) -sys.setdefaultencoding('utf8') - - -class ImageHandler: - def __init__(self, filepath, savepath, bag): - self.bag = bag - self.msgs = 0 # Used to show remaining frames - self.image_index = 0 - self.image_topics = [] - self.bridge = CvBridge() - self.bagname = filepath.split('/')[-1] # Returns ../../bagname.bag - - if savepath is not None: - # Alternative save path (i.e. want to save to ext. drive) - if savepath[-1] != '/': - savepath += '/' - self.working_dir = savepath + self.bagname.split('.')[0]\ - + '_images' - else: - # Save path defaults to launch path - self.working_dir = filepath.split('.')[0] + '_images' - - if not os.path.exists(self.working_dir): - os.makedirs(self.working_dir) - - print '\033[1m' + self.bagname + ':\033[0m' - print 'Saving images to: ', self.working_dir - - self.parse_bag() - self.save_images() - - def parse_bag(self): - types = [] - bag_msg_cnt = [] - topic_status = False - topics = bag.get_type_and_topic_info()[1].keys() - - for i in range(0, len(bag.get_type_and_topic_info()[1].values())): - types.append(bag.get_type_and_topic_info()[1].values()[i][0]) - bag_msg_cnt.append(bag.get_type_and_topic_info()[1].values()[i][1]) - - topics = zip(topics, types, bag_msg_cnt) - - for topic, type, count in topics: - # Want to ignore image topics other than /image_raw - # TODO: Make this changeable - match = re.search(r'\mono|rect|theora|color\b', topic) - if match: - pass - elif type == 'sensor_msgs/Image': - if topic_status is False: - print 'Topic(s):' - topic_status = True - print ' ' + topic - self.image_topics.append(topic) - self.msgs = self.msgs + count - - def save_images(self): - # TODO: Add ability to pickup where it last left off - with tqdm(total=self.msgs) as pbar: - for topic, msg, t in self.bag.read_messages( - topics=self.image_topics): - try: - cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") - cv2.imwrite(self.working_dir + '/' + - str(self.image_index) + '.png', cv_image) - self.image_index = self.image_index + 1 - except CvBridgeError, e: - print e - pbar.update(1) - self.bag.close() - print self.image_index + 1, 'images saved to', self.working_dir, '\n' - -if __name__ == '__main__': - - desc_msg = ('Automated tool that traverses a directory dumping all of' + - 'the images from all non-rectificed, non-mono, and non-' + - 'theora topics found in all of the ROS bag files encountered.') - - parser = argparse.ArgumentParser(description=desc_msg) - - parser.add_argument('-f', '--filepath', - help='File path containing the ROS bags') - parser.add_argument('-s', '--savepath', - help='Path where the images will be saved') - parser.add_argument('-r', '--resize', - help='Resolution to resize images to') - - args = parser.parse_args() - - # TODO: Path validation can be moved to ImageHandler class - if args.filepath is not None: - filepath = args.filepath - else: - print 'No bag source provided' - filepath = sys.path[0] - - if args.savepath is not None: - savepath = args.savepath - print 'Setting save path to:', savepath - else: - savepath = None - - if args.resize: - # We're not ready for the future yet - pass - - print '\033[1mFilepath:', filepath + '\033[0m' # Print working directory - - matches = [] - bag_count = 0 - - if filepath[-4:] == '.bag': - bag = rosbag.Bag(filepath) - ImageHandler(filepath, savepath, bag) - else: - - # Generate list of file paths containing bag files - for root, dirnames, filenames in os.walk(filepath): - for filename in fnmatch.filter(filenames, '*.bag'): - if not filename.startswith('.'): - matches.append(os.path.join(root, filename)) - - print '\033[1m' + str(len(matches)) + ' bags found\033[0m\n' - - # Iterate through found bags - for bag_dir in matches: - bag = rosbag.Bag(bag_dir) - try: - ImageHandler(bag_dir, savepath, bag) - bag_count = bag_count + 1 - except rospy.ROSInterruptException: - pass - - print 'Processed', bag_count, 'bags.\n' - - print "Done!" diff --git a/perception/sub8_perception/src/sub8_vision_lib/align.cpp b/perception/sub8_perception/src/sub8_vision_lib/align.cpp index 6ab859b2..6c542de7 100644 --- a/perception/sub8_perception/src/sub8_vision_lib/align.cpp +++ b/perception/sub8_perception/src/sub8_vision_lib/align.cpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include #define _USE_MATH_DEFINES #include diff --git a/perception/sub8_perception/src/sub8_vision_lib/cv_param_helpers.cpp b/perception/sub8_perception/src/sub8_vision_lib/cv_param_helpers.cpp index b2ace3f4..74af4a3a 100644 --- a/perception/sub8_perception/src/sub8_vision_lib/cv_param_helpers.cpp +++ b/perception/sub8_perception/src/sub8_vision_lib/cv_param_helpers.cpp @@ -1,4 +1,4 @@ -#include +#include #include namespace sub { diff --git a/perception/sub8_perception/src/sub8_vision_lib/cv_utils.cpp b/perception/sub8_perception/src/sub8_vision_lib/cv_utils.cpp deleted file mode 100644 index 688a0cec..00000000 --- a/perception/sub8_perception/src/sub8_vision_lib/cv_utils.cpp +++ /dev/null @@ -1,656 +0,0 @@ -#include - -namespace sub { - -cv::Point contour_centroid(Contour &contour) { - cv::Moments m = cv::moments(contour, true); - cv::Point center(m.m10 / m.m00, m.m01 / m.m00); - return center; -} - -bool larger_contour(const Contour &c1, const Contour &c2) { - if (cv::contourArea(c1) > cv::contourArea(c2)) - return true; - else - return false; -} - -cv::MatND smooth_histogram(const cv::MatND &histogram, - size_t filter_kernel_size, float sigma) { - cv::MatND hist = histogram.clone(); - std::vector gauss_kernel = - generate_gaussian_kernel_1D(filter_kernel_size, sigma); - size_t histSize = hist.total(); - int offset = (filter_kernel_size - 1) / 2; - for (size_t i = offset; i < histSize - offset; - i++) // Convolve histogram values with gaussian kernel - { - int sum = 0; - int kernel_idx = 0; - for (int j = i - offset; j <= int(i + offset); j++) { - sum += (hist.at(j) * gauss_kernel[kernel_idx++]); - } - hist.at(i) = sum; - } - for (int i = 0; i < offset; ++i) // Pad filtered result with zeroes - { - hist.at(i) = 0; - hist.at(histSize - 1 - i) = 0; - } - return hist; -} - -std::vector generate_gaussian_kernel_1D(size_t kernel_size, - float sigma) { - std::vector kernel; - int middle_index = (kernel_size - 1) / 2; - int first_discrete_sample_x = -(middle_index); - for (int i = first_discrete_sample_x; i <= 0; i++) { - float power = -0.5 * (float(i) / sigma) * (float(i) / sigma); - kernel.push_back( - exp(power)); // From definition of Standard Normal Distribution - } - for (int i = 1; i <= middle_index; i++) { // Kernel is symmetric - kernel.push_back(kernel[middle_index - i]); - } - // Normalize kernel (sum of values should equal 1.0) - float sum = 0; - for (size_t i = 0; i < kernel_size; i++) { - sum += kernel[i]; - } - for (size_t i = 0; i < kernel_size; i++) { - kernel[i] /= sum; - } - return kernel; -} - -std::vector find_local_maxima(const cv::MatND &histogram, - float thresh_multiplier) { - - std::stringstream ros_log; - ros_log << "\x1b[1;31m" - << "find_local_maxima" - << "\x1b[0m" << std::endl; - - std::vector local_maxima, threshed_local_maxima; - float global_maximum = -std::numeric_limits::infinity(); - - // Locate local maxima and find global maximum - for (size_t idx = 1; idx < histogram.total() - 1; idx++) { - float current_value = histogram.at(idx); - if ((histogram.at(idx - 1) < current_value) && - (histogram.at(idx + 1) <= current_value)) { - local_maxima.push_back(cv::Point(idx, current_value)); - if (global_maximum < current_value) - global_maximum = current_value; - } - } - ros_log << "Maxima [x, y]:"; - for (size_t i = 0; i < local_maxima.size(); i++) { - if (local_maxima[i].y > global_maximum * thresh_multiplier) - threshed_local_maxima.push_back(local_maxima[i]); - if (i % 4 == 0) - ros_log << std::endl - << "\t"; - ros_log << "[" << std::setw(5) << local_maxima[i].x << "," << std::setw(5) - << local_maxima[i].y << "] "; - } - ros_log << std::endl - << "thresh: > global_maximum(" << global_maximum - << ") * thresh_multiplier(" << thresh_multiplier - << ") = " << (global_maximum * thresh_multiplier); - ros_log << std::endl - << "Threshed Maxima (x): "; - if (threshed_local_maxima.size() != local_maxima.size()) { - BOOST_FOREACH (cv::Point pt, threshed_local_maxima) { - ros_log << " " << pt.x << " "; - } - } else - ros_log << "same as 'Maxima'"; - ros_log << std::endl; -#ifdef SEGMENTATION_DEBUG - ROS_INFO(ros_log.str().c_str()); -#endif - return threshed_local_maxima; -} - -std::vector find_local_minima(const cv::MatND &histogram, - float thresh_multiplier) { - - std::stringstream ros_log; - ros_log << "\x1b[1;31m" - << "find_local_minima" - << "\x1b[0m" << std::endl; - - std::vector local_minima, threshed_local_minima; - float global_minimum = std::numeric_limits::infinity(); - ; - - // Locate local minima and find global minimum - for (size_t idx = 1; idx < histogram.total() - 1; idx++) { - float current_value = histogram.at(idx); - if ((histogram.at(idx - 1) >= current_value) && - (histogram.at(idx + 1) > current_value)) { - local_minima.push_back(cv::Point(idx, current_value)); - if (global_minimum > current_value) - global_minimum = current_value; - } - } - ros_log << "Minima [x, y]:"; - for (size_t i = 0; i < local_minima.size(); i++) { - if (local_minima[i].y < global_minimum * thresh_multiplier) - threshed_local_minima.push_back(local_minima[i]); - if (i % 4 == 0) - ros_log << std::endl - << "\t"; - ros_log << "[" << std::setw(5) << local_minima[i].x << "," << std::setw(5) - << local_minima[i].y << "] "; - } - ros_log << std::endl - << "thresh: < global_minimum(" << global_minimum - << ") * thresh_multiplier(" << thresh_multiplier - << ") = " << (global_minimum * thresh_multiplier); - ros_log << std::endl - << "Threshed Minima (x): "; - if (threshed_local_minima.size() != local_minima.size()) { - BOOST_FOREACH (cv::Point pt, threshed_local_minima) { - ros_log << " " << pt.x << " "; - } - } else - ros_log << "same as 'Minima'"; - ros_log << std::endl; -#ifdef SEGMENTATION_DEBUG - ROS_INFO(ros_log.str().c_str()); -#endif - return threshed_local_minima; -} - -unsigned int select_hist_mode(std::vector &histogram_modes, - int target) { - - std::stringstream ros_log; - ros_log << "\x1b[1;31m" - << "select_hist_mode" - << "\x1b[0m" << std::endl; - - std::vector distances; - BOOST_FOREACH (cv::Point mode, histogram_modes) { - distances.push_back(mode.x - target); - } - int min_idx = 0; - for (size_t i = 0; i < distances.size(); i++) { - if (std::abs(distances[i]) <= std::abs(distances[min_idx])) - min_idx = i; - } - if (histogram_modes.size() == 0) { - ros_log << "No modes could be generated" << std::endl; - ROS_INFO(ros_log.str().c_str()); - return 0; - } else - return histogram_modes[min_idx].x; -} - -void statistical_image_segmentation(const cv::Mat &src, cv::Mat &dest, - cv::Mat &debug_img, const int hist_size, - const float **ranges, const int target, - std::string image_name, bool ret_dbg_img, - const float sigma, - const float low_thresh_gain, - const float high_thresh_gain) { - std::stringstream ros_log; - ros_log << "\x1b[1;31m" - << "statistical_image_segmentation" - << "\x1b[0m" << std::endl; - - // Calculate histogram - cv::MatND hist, hist_smooth, hist_derivative; - cv::calcHist(&src, 1, 0, cv::Mat(), hist, 1, &hist_size, ranges, true, false); - - // Smooth histogram - const int kernel_size = 11; - hist_smooth = sub::smooth_histogram(hist, kernel_size, sigma); - - // Calculate histogram derivative (central finite difference) - hist_derivative = hist_smooth.clone(); - hist_derivative.at(0) = 0; - hist_derivative.at(hist_size - 1) = 0; - for (int i = 1; i < hist_size - 1; ++i) { - hist_derivative.at(i) = - (hist_smooth.at(i + 1) - hist_smooth.at(i - 1)) / 2.0; - } - hist_derivative = sub::smooth_histogram(hist_derivative, kernel_size, sigma); - - // Find target mode - std::vector histogram_modes = - sub::find_local_maxima(hist_smooth, 0.1); - int target_mode = sub::select_hist_mode(histogram_modes, target); - ros_log << "Target: " << target << std::endl; - ros_log << "Mode Selected: " << target_mode << std::endl; - - // Calculate std dev of histogram slopes - cv::Scalar hist_deriv_mean, hist_deriv_stddev; - cv::meanStdDev(hist_derivative, hist_deriv_mean, hist_deriv_stddev); - - // Determine thresholds for cv::inRange() using the std dev of histogram - // slopes times a gain as a cutoff heuristic - int high_abs_derivative_thresh = - std::abs(hist_deriv_stddev[0] * high_thresh_gain); - int low_abs_derivative_thresh = - std::abs(hist_deriv_stddev[0] * low_thresh_gain); - std::vector derivative_maxima = - sub::find_local_maxima(hist_derivative, 0.01); - std::vector derivative_minima = - sub::find_local_minima(hist_derivative, 0.01); - int high_thresh_search_start = target_mode; - int low_thresh_search_start = target_mode; - ros_log << "high_thresh_search_start: " << target_mode << std::endl; - ros_log << "Looking for the local minimum of the derivative of the histogram " - "immediately to the right of the selected mode." << std::endl; - - for (size_t i = 0; i < derivative_minima.size(); i++) { - ros_log << "\tderivative_minima[" << i << "].x = " << derivative_minima[i].x - << std::endl; - if (derivative_minima[i].x > target_mode) { - high_thresh_search_start = derivative_minima[i].x; - ros_log << "Done: The upper threshold will be no less than " - << high_thresh_search_start << std::endl; - break; - } - } - ros_log << "low_thresh_search_start: " << target_mode << std::endl; - ros_log << "Looking for the local maximum of the derivative of the histogram " - "immediately to the left of the selected mode." << std::endl; - for (int i = derivative_maxima.size() - 1; i >= 0; i--) { - ros_log << "\tderivative_maxima[" << i << "].x = " << derivative_maxima[i].x - << std::endl; - if (derivative_maxima[i].x < target_mode) { - low_thresh_search_start = derivative_maxima[i].x; - ros_log << "Done: The lower threshold will be no greater than " - << low_thresh_search_start << std::endl; - break; - } - } - int high_thresh = high_thresh_search_start; - int low_thresh = low_thresh_search_start; - ros_log << "high_deriv_thresh: " << hist_deriv_stddev[0] << " * " - << high_thresh_gain << " = " << high_abs_derivative_thresh - << std::endl; - ros_log << "abs(high_deriv_thresh) - abs(slope) = slope_error" << std::endl; - ros_log << "i: potential upper threshold" << std::endl; - ros_log << "Looking for first i such that slope_error >= 0" << std::endl; - for (int i = high_thresh_search_start; i < hist_size; i++) { - int abs_slope = std::abs(hist_derivative.at(i)); - ros_log << "\ti = " << i << " : " << high_abs_derivative_thresh << " - " - << abs_slope << " = " << high_abs_derivative_thresh - abs_slope - << std::endl; - if (abs_slope <= high_abs_derivative_thresh) { - high_thresh = i; - break; - } - } - ros_log << "high_thresh = " << high_thresh << std::endl; - ros_log << "low_deriv_thresh: " << hist_deriv_stddev[0] << " * " - << low_thresh_gain << " = " << low_abs_derivative_thresh << std::endl; - ros_log << "abs(low_deriv_thresh) - abs(slope) = slope_error" << std::endl; - ros_log << "i: potential lower threshold" << std::endl; - ros_log << "Looking for first i such that slope_error <= 0" << std::endl; - for (int i = low_thresh_search_start; i > 0; i--) { - int abs_slope = std::abs(hist_derivative.at(i)); - ros_log << "\ti = " << i << " : " << low_abs_derivative_thresh << " - " - << abs_slope << " = " << high_abs_derivative_thresh - abs_slope - << std::endl; - - if (abs_slope <= low_abs_derivative_thresh) { - low_thresh = i; - break; - } - } - - ros_log << "low_thresh = " << low_thresh << std::endl; - ros_log << "\x1b[1;37mTarget: " << target - << "\nClosest distribution mode: " << target_mode - << "\nThresholds selected: low=" << low_thresh - << " high=" << high_thresh << "\x1b[0m" << std::endl; - - // Threshold image - cv::inRange(src, low_thresh, high_thresh, dest); - -#ifdef SEGMENTATION_DEBUG - ROS_INFO(ros_log.str().c_str()); - cv::imshow("src" + image_name, src); - cv::waitKey(1); -#endif - - // Closing Morphology operation - int dilation_size = 2; - cv::Mat structuring_element = cv::getStructuringElement( - cv::MORPH_ELLIPSE, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), - cv::Point(dilation_size, dilation_size)); - cv::dilate(dest, dest, structuring_element); - cv::erode(dest, dest, structuring_element); - - if (ret_dbg_img) { - - try { - // Prepare to draw graph of histogram and derivative - int hist_w = src.cols; - int hist_h = src.rows; - int bin_w = hist_w / hist_size; - cv::Mat histImage(hist_h, hist_w, CV_8UC1, cv::Scalar(0, 0, 0)); - cv::Mat histDerivImage(hist_h, hist_w, CV_8UC1, cv::Scalar(0, 0, 0)); - cv::normalize(hist_smooth, hist_smooth, 0, histImage.rows, - cv::NORM_MINMAX, -1, cv::Mat()); - cv::normalize(hist_derivative, hist_derivative, 0, histImage.rows, - cv::NORM_MINMAX, -1, cv::Mat()); - - // Draw Graphs - for (int i = 1; i < hist_size; i++) { - // Plot image histogram - cv::line( - histImage, - cv::Point(bin_w * (i - 1), - hist_h - cvRound(hist_smooth.at(i - 1))), - cv::Point(bin_w * (i), hist_h - cvRound(hist_smooth.at(i))), - cv::Scalar(255, 0, 0), 2, 8, 0); - // Plot image histogram derivative - cv::line(histDerivImage, - cv::Point(bin_w * (i - 1), - hist_h - cvRound(hist_derivative.at(i - 1))), - cv::Point(bin_w * (i), - hist_h - cvRound(hist_derivative.at(i))), - cv::Scalar(122, 0, 0), 1, 8, 0); - } - - // Shade in area being segmented under histogram curve - cv::line(histImage, - cv::Point(bin_w * low_thresh, - hist_h - cvRound(hist_smooth.at(low_thresh))), - cv::Point(bin_w * low_thresh, hist_h), cv::Scalar(255, 255, 0), - 2); - cv::line(histImage, - cv::Point(bin_w * high_thresh, - hist_h - cvRound(hist_smooth.at(high_thresh))), - cv::Point(bin_w * high_thresh, hist_h), cv::Scalar(255, 255, 0), - 2); - cv::floodFill( - histImage, - cv::Point(bin_w * cvRound(float(low_thresh + high_thresh) / 2.0), - hist_h - 1), - cv::Scalar(155)); - - // Combine graphs into one image and display results - cv::Mat segmentation_channel = cv::Mat::zeros(histImage.size(), CV_8UC1); - std::vector debug_img_channels; - debug_img_channels.push_back(histImage); - debug_img_channels.push_back(histDerivImage); - debug_img_channels.push_back(dest * 0.25); - cv::merge(debug_img_channels, debug_img); - cv::Point text_upper_left(debug_img.cols / 2.0, debug_img.rows / 10.0); - std::string text_ln1 = image_name; - std::stringstream text_ln2; - text_ln2 << "low = " << low_thresh; - std::stringstream text_ln3; - text_ln3 << "high = " << high_thresh; - int font = cv::FONT_HERSHEY_SIMPLEX; - double font_scale = 0.0015 * debug_img.rows; - cv::Point vert_offset = cv::Point(0, debug_img.rows / 15.0); - cv::Scalar text_color(255, 255, 0); - cv::putText(debug_img, text_ln1, text_upper_left, font, font_scale, - text_color); - cv::putText(debug_img, text_ln2.str(), text_upper_left + vert_offset, - font, font_scale, text_color); - cv::putText(debug_img, text_ln3.str(), - text_upper_left + vert_offset + vert_offset, font, font_scale, - text_color); - } catch (std::exception &e) { - ROS_INFO(e.what()); - } - } -} - -cv::Mat triangulate_Linear_LS(cv::Mat mat_P_l, cv::Mat mat_P_r, - cv::Mat undistorted_l, cv::Mat undistorted_r) { - cv::Mat A(4, 3, CV_64FC1), b(4, 1, CV_64FC1), X(3, 1, CV_64FC1), - X_homogeneous(4, 1, CV_64FC1), W(1, 1, CV_64FC1); - W.at(0, 0) = 1.0; - A.at(0, 0) = - (undistorted_l.at(0, 0) / undistorted_l.at(2, 0)) * - mat_P_l.at(2, 0) - - mat_P_l.at(0, 0); - A.at(0, 1) = - (undistorted_l.at(0, 0) / undistorted_l.at(2, 0)) * - mat_P_l.at(2, 1) - - mat_P_l.at(0, 1); - A.at(0, 2) = - (undistorted_l.at(0, 0) / undistorted_l.at(2, 0)) * - mat_P_l.at(2, 2) - - mat_P_l.at(0, 2); - A.at(1, 0) = - (undistorted_l.at(1, 0) / undistorted_l.at(2, 0)) * - mat_P_l.at(2, 0) - - mat_P_l.at(1, 0); - A.at(1, 1) = - (undistorted_l.at(1, 0) / undistorted_l.at(2, 0)) * - mat_P_l.at(2, 1) - - mat_P_l.at(1, 1); - A.at(1, 2) = - (undistorted_l.at(1, 0) / undistorted_l.at(2, 0)) * - mat_P_l.at(2, 2) - - mat_P_l.at(1, 2); - A.at(2, 0) = - (undistorted_r.at(0, 0) / undistorted_r.at(2, 0)) * - mat_P_r.at(2, 0) - - mat_P_r.at(0, 0); - A.at(2, 1) = - (undistorted_r.at(0, 0) / undistorted_r.at(2, 0)) * - mat_P_r.at(2, 1) - - mat_P_r.at(0, 1); - A.at(2, 2) = - (undistorted_r.at(0, 0) / undistorted_r.at(2, 0)) * - mat_P_r.at(2, 2) - - mat_P_r.at(0, 2); - A.at(3, 0) = - (undistorted_r.at(1, 0) / undistorted_r.at(2, 0)) * - mat_P_r.at(2, 0) - - mat_P_r.at(1, 0); - A.at(3, 1) = - (undistorted_r.at(1, 0) / undistorted_r.at(2, 0)) * - mat_P_r.at(2, 1) - - mat_P_r.at(1, 1); - A.at(3, 2) = - (undistorted_r.at(1, 0) / undistorted_r.at(2, 0)) * - mat_P_r.at(2, 2) - - mat_P_r.at(1, 2); - b.at(0, 0) = - -((undistorted_l.at(0, 0) / undistorted_l.at(2, 0)) * - mat_P_l.at(2, 3) - - mat_P_l.at(0, 3)); - b.at(1, 0) = - -((undistorted_l.at(1, 0) / undistorted_l.at(2, 0)) * - mat_P_l.at(2, 3) - - mat_P_l.at(1, 3)); - b.at(2, 0) = - -((undistorted_r.at(0, 0) / undistorted_r.at(2, 0)) * - mat_P_r.at(2, 3) - - mat_P_r.at(0, 3)); - b.at(3, 0) = - -((undistorted_r.at(1, 0) / undistorted_r.at(2, 0)) * - mat_P_r.at(2, 3) - - mat_P_r.at(1, 3)); - solve(A, b, X, cv::DECOMP_SVD); - vconcat(X, W, X_homogeneous); - return X_homogeneous; -} - -Eigen::Vector3d kanatani_triangulation(const cv::Point2d &pt1, - const cv::Point2d &pt2, - const Eigen::Matrix3d &essential, - const Eigen::Matrix3d &R) { - /* - K. Kanatani, Y. Sugaya, and H. Niitsuma. Triangulation from two views - revisited: Hartley-Sturm vs. optimal - correction. In British Machine Vision Conference, page 55, 2008. - */ - // std::cout << "ptL_noisy: " << pt1 << " ptR_noisy: " << pt2 << std::endl; - const unsigned int max_iterations = 7; - Eigen::Vector3d p1_old(pt1.x, pt1.y, 1.0); - Eigen::Vector3d p2_old(pt2.x, pt2.y, 1.0); - const Eigen::Vector3d p1_0(pt1.x, pt1.y, 1.0); - const Eigen::Vector3d p2_0(pt2.x, pt2.y, 1.0); - Eigen::Vector3d p1, p2; - Eigen::Vector2d n1, n2, delta_p1, delta_p2, delta_p1_old, delta_p2_old; - delta_p1_old << 0.0, 0.0; - delta_p2_old << 0.0, 0.0; - Eigen::Matrix S; - S << 1, 0, 0, 0, 1, 0; - Eigen::Matrix2d essential_bar = essential.topLeftCorner(2, 2); - double lambda; - for (unsigned int i = 0; i < max_iterations; i++) { - n1 = S * (essential * p2_old); - n2 = S * (essential.transpose() * p1_old); - lambda = ((p1_0.transpose() * essential * p2_0)(0) - - (delta_p1_old.transpose() * essential_bar * delta_p2_old)(0)) / - (n1.transpose() * n1 + n2.transpose() * n2)(0); - delta_p1 = lambda * n1; - delta_p2 = lambda * n2; - p1 = p1_0 - (S.transpose() * delta_p1); - p2 = p2_0 - (S.transpose() * delta_p2); - p1_old = p1; - p2_old = p2; - delta_p1_old = delta_p1; - delta_p2_old = delta_p2; - // std::cout << "ptL_est: [" << p1.transpose() << "] ptR_est: [" << - // p2.transpose() << "]" << std::endl; - } - Eigen::Vector3d z = p1.cross(R * p2); - Eigen::Vector3d X = - ((z.transpose() * (essential * p2))(0) / (z.transpose() * z)(0)) * p1; - return X; -} - -Eigen::Vector3d lindstrom_triangulation(const cv::Point2d &pt1, - const cv::Point2d &pt2, - const Eigen::Matrix3d &essential, - const Eigen::Matrix3d &R) { - /* - Optimal triangulation method for two cameras with parallel principal - axes - Based of off this paper by Peter Lindstrom: - https://e-reports-ext.llnl.gov/pdf/384387.pdf **Listing 2** - */ - // std::cout << "ptL_noisy: " << pt1 << " ptR_noisy: " << pt2 << std::endl; - const unsigned int max_iterations = 7; - Eigen::Vector3d p1_old(pt1.x, pt1.y, 1.0); - Eigen::Vector3d p2_old(pt2.x, pt2.y, 1.0); - const Eigen::Vector3d p1_0(pt1.x, pt1.y, 1.0); - const Eigen::Vector3d p2_0(pt2.x, pt2.y, 1.0); - Eigen::Vector3d p1, p2; - Eigen::Vector2d n1, n2, delta_p1, delta_p2; - Eigen::Matrix S; - S << 1, 0, 0, 0, 1, 0; - Eigen::Matrix2d essential_bar = essential.topLeftCorner(2, 2); - double a, b, c, d, lambda; - c = p1_0.transpose() * (essential * p2_0); - for (unsigned int i = 0; i < max_iterations; i++) { - n1 = S * (essential * p2_old); - n2 = S * (essential.transpose() * p1_old); - a = n1.transpose() * (essential_bar * n2); - b = (0.5 * ((n1.transpose() * n1) + (n2.transpose() * n2)))(0); - d = sqrt(b * b - a * c); - double signum_b = (b > 0) ? 1 : ((b < 0) ? -1 : 0); - lambda = c / (b + signum_b * d); - delta_p1 = lambda * n1; - delta_p2 = lambda * n2; - p1 = p1_0 - (S.transpose() * delta_p1); - p2 = p2_0 - (S.transpose() * delta_p2); - p1_old = p1; - p2_old = p2; - // std::cout << "ptL_est: [" << p1.transpose() << "] ptR_est: [" << - // p2.transpose() << "]" << std::endl; - } - Eigen::Vector3d z = p1.cross(R * p2); - Eigen::Vector3d X = - ((z.transpose() * (essential * p2))(0) / (z.transpose() * z)(0)) * p1; - return X; -} - -ImageWithCameraInfo::ImageWithCameraInfo( - sensor_msgs::ImageConstPtr _image_msg_ptr, - sensor_msgs::CameraInfoConstPtr _info_msg_ptr) - : image_msg_ptr(_image_msg_ptr), info_msg_ptr(_info_msg_ptr), - image_time(_image_msg_ptr->header.stamp) {} - -FrameHistory::FrameHistory(std::string img_topic, unsigned int hist_size) - : topic_name(img_topic), history_size(hist_size), _image_transport(nh), - frame_count(0) { - std::stringstream console_msg; - console_msg << "[FrameHistory] size set to " << history_size << std::endl - << "\tSubscribing to image topic: " << topic_name; - ROS_INFO(console_msg.str().c_str()); - _image_sub = _image_transport.subscribeCamera( - img_topic, 1, &FrameHistory::image_callback, this); - if (_image_sub.getNumPublishers() == 0) { - std::stringstream error_msg; - error_msg << "[FrameHistory] no publishers currently publishing to " - << topic_name; - ROS_WARN(error_msg.str().c_str()); - } -} - -FrameHistory::~FrameHistory() { - std::stringstream console_msg; - console_msg << "[FrameHistory] Unsubscribed from image topic: " << topic_name - << std::endl - << "[FrameHistory] Deleting FrameHistory object" << std::endl; - ROS_INFO(console_msg.str().c_str()); -} - -void FrameHistory::image_callback( - const sensor_msgs::ImageConstPtr &image_msg, - const sensor_msgs::CameraInfoConstPtr &info_msg) { - /** - Adds an ImageWithCameraInfo object to the frame history ring buffer - */ - ImageWithCameraInfo current_frame(image_msg, info_msg); - bool full = _frame_history_ring_buffer.size() >= history_size; - std::stringstream debug_msg; - debug_msg << "Adding frame to ring buffer " - << "[frame=" << frame_count << "," - << "full=" << (full ? "true" : "false") - << ",frames_available=" << _frame_history_ring_buffer.size() << "]" - << std::endl; - ROS_DEBUG(debug_msg.str().c_str()); - if (!full) { - _frame_history_ring_buffer.push_back(current_frame); - } else { - _frame_history_ring_buffer[frame_count % history_size] = current_frame; - } - frame_count++; -} - -std::vector -FrameHistory::get_frame_history(unsigned int frames_requested) { - /** - Returns a vector with the last ImageWithCameraInfo - objects - */ - std::vector frame_history; - std::vector sorted_frame_history = - _frame_history_ring_buffer; - if (_frame_history_ring_buffer.size() < frames_requested) { - ROS_WARN("get_frame_history(%d): %d frames were requested, but there are " - "%d frames available", - frames_requested, frames_requested, - _frame_history_ring_buffer.size()); - } else { - std::sort(sorted_frame_history.begin(), sorted_frame_history.end()); - for (size_t i = 0; i < frames_requested; i++) { - frame_history.push_back(sorted_frame_history[i]); - if (i == frames_requested - 1) - break; - } - } - return frame_history; -} - -} // namespace sub diff --git a/perception/sub8_perception/src/sub8_vision_lib/object_finder.cpp b/perception/sub8_perception/src/sub8_vision_lib/object_finder.cpp index c038a7e7..a2242790 100644 --- a/perception/sub8_perception/src/sub8_vision_lib/object_finder.cpp +++ b/perception/sub8_perception/src/sub8_vision_lib/object_finder.cpp @@ -1,4 +1,4 @@ -#include +#include /////////////////////////////////////////////////////////////////////////////////////////////////// // Class: Sub8ObjectFinder //////////////////////////////////////////////////////////////// diff --git a/perception/sub8_perception/src/sub8_vision_lib/visualization.cpp b/perception/sub8_perception/src/sub8_vision_lib/visualization.cpp index 3c9c0332..67c5daf1 100644 --- a/perception/sub8_perception/src/sub8_vision_lib/visualization.cpp +++ b/perception/sub8_perception/src/sub8_vision_lib/visualization.cpp @@ -148,4 +148,4 @@ void RvizVisualizer::visualize_torpedo_board(geometry_msgs::Pose& pose, Eigen::Q rviz_pub.publish(borders); } -} // End namespace "sub" \ No newline at end of file +} // End namespace "sub" diff --git a/perception/sub8_perception/sub8_vision_tools/estimation.py b/perception/sub8_perception/sub8_vision_tools/estimation.py index ab16ea73..089a7f8e 100644 --- a/perception/sub8_perception/sub8_vision_tools/estimation.py +++ b/perception/sub8_perception/sub8_vision_tools/estimation.py @@ -284,11 +284,11 @@ def draw_line(pt_1, pt_2, color=(1.0, 0.0, 0.0)): def main(): """Here's some stupid demo code """ - import sub8_ros_tools + import mil_ros_tools import time import rospy rospy.init_node('test_estimation') - q = sub8_ros_tools.Image_Subscriber('/camera/front/left/image_rect_color') + q = mil_ros_tools.Image_Subscriber('/camera/front/left/image_rect_color') while(q.camera_info is None): time.sleep(0.1) diff --git a/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_cv2.py b/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_cv2.py index 29bf7e31..5712ed27 100644 --- a/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_cv2.py +++ b/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_cv2.py @@ -5,7 +5,7 @@ import numpy as np import sys from boost_auto import observe -import sub8_ros_tools +import mil_ros_tools if __name__ == '__main__': diff --git a/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_live.py b/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_live.py index 371cd42a..a8cbdb0e 100644 --- a/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_live.py +++ b/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_live.py @@ -5,7 +5,7 @@ import numpy as np import sys from boost_auto import observe -import sub8_ros_tools +import mil_ros_tools def got_image(img): global last_image, last_image_time @@ -35,8 +35,8 @@ def process_image(img, image_pub, clf): clf = cv2.Boost() clf.load(args.classifer) - image_sub = sub8_ros_tools.Image_Subscriber(args.topic, got_image, queue_size=1) - image_pub = sub8_ros_tools.Image_Publisher(args.topic + "_segmented") + image_sub = mil_ros_tools.Image_Subscriber(args.topic, got_image, queue_size=1) + image_pub = mil_ros_tools.Image_Publisher(args.topic + "_segmented") last_image = None last_image_time = None diff --git a/perception/sub8_perception/sub8_vision_tools/marker_occ_grid.py b/perception/sub8_perception/sub8_vision_tools/marker_occ_grid.py index 2b9de84a..4603e618 100644 --- a/perception/sub8_perception/sub8_vision_tools/marker_occ_grid.py +++ b/perception/sub8_perception/sub8_vision_tools/marker_occ_grid.py @@ -8,7 +8,7 @@ from nav_msgs.msg import OccupancyGrid, MapMetaData from sub8_msgs.srv import VisionRequest2D, VisionRequest2DResponse, SearchPose from image_geometry import PinholeCameraModel -from sub8_ros_tools import threading_helpers, msg_helpers +from mil_ros_tools import threading_helpers, msg_helpers from std_srvs.srv import Empty, EmptyResponse import cv2 import numpy as np diff --git a/perception/sub8_perception/sub8_vision_tools/multi_observation.py b/perception/sub8_perception/sub8_vision_tools/multi_observation.py index 948be752..243cb3c9 100644 --- a/perception/sub8_perception/sub8_vision_tools/multi_observation.py +++ b/perception/sub8_perception/sub8_vision_tools/multi_observation.py @@ -98,13 +98,13 @@ def test(): TODO: Make this an actual unit test """ - import sub8_ros_tools + import mil_ros_tools import time import rospy from mayavi import mlab rospy.init_node('test_estimation') - q = sub8_ros_tools.Image_Subscriber('/camera/front/left/image_raw') + q = mil_ros_tools.Image_Subscriber('/camera/front/left/image_raw') while(q.camera_info is None): time.sleep(0.1) diff --git a/perception/sub8_perception/sub8_vision_tools/rviz.py b/perception/sub8_perception/sub8_vision_tools/rviz.py index d99e457f..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 sub8_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/perception/sub8_perception/sub8_vision_tools/threshold_tools.py b/perception/sub8_perception/sub8_vision_tools/threshold_tools.py index a6fca2e9..3eb38e2f 100644 --- a/perception/sub8_perception/sub8_vision_tools/threshold_tools.py +++ b/perception/sub8_perception/sub8_vision_tools/threshold_tools.py @@ -1,7 +1,7 @@ import numpy as np import cv2 import rospy -from sub8_ros_tools.func_helpers import Cache +from mil_ros_tools.func_helpers import Cache def bgr_vec_to_hsv(vector): diff --git a/simulation/sub8_gazebo/diagnostics/gazebo_tests/align_marker_test.py b/simulation/sub8_gazebo/diagnostics/gazebo_tests/align_marker_test.py index a43353e5..1094596b 100644 --- a/simulation/sub8_gazebo/diagnostics/gazebo_tests/align_marker_test.py +++ b/simulation/sub8_gazebo/diagnostics/gazebo_tests/align_marker_test.py @@ -4,7 +4,7 @@ import tf import txros from std_srvs.srv import Empty, EmptyRequest -from sub8_ros_tools import msg_helpers +from mil_ros_tools import msg_helpers from gazebo_msgs.srv import DeleteModelRequest, SpawnModelRequest import missions diff --git a/simulation/sub8_gazebo/diagnostics/gazebo_tests/common.py b/simulation/sub8_gazebo/diagnostics/gazebo_tests/common.py index 1505dbc7..50c79351 100644 --- a/simulation/sub8_gazebo/diagnostics/gazebo_tests/common.py +++ b/simulation/sub8_gazebo/diagnostics/gazebo_tests/common.py @@ -5,7 +5,7 @@ from gazebo_msgs.msg import ModelState, ModelStates from kill_handling.srv import SetKill, SetKillRequest from kill_handling.msg import Kill -from sub8_ros_tools import numpy_to_twist +from mil_ros_tools import numpy_to_twist import txros diff --git a/simulation/sub8_gazebo/diagnostics/set_model_position_test.py b/simulation/sub8_gazebo/diagnostics/set_model_position_test.py index dfb5ce26..1fa4ef59 100644 --- a/simulation/sub8_gazebo/diagnostics/set_model_position_test.py +++ b/simulation/sub8_gazebo/diagnostics/set_model_position_test.py @@ -6,7 +6,7 @@ from geometry_msgs.msg import Pose, Point, Quaternion from nav_msgs.msg import Odometry from sub8_gazebo.srv import RunJob, RunJobResponse -from sub8_ros_tools import msg_helpers +from mil_ros_tools import msg_helpers import job_runner import tf diff --git a/simulation/sub8_gazebo/nodes/actuator_sim.py b/simulation/sub8_gazebo/nodes/actuator_sim.py index 967b105b..dbfa5bcf 100755 --- a/simulation/sub8_gazebo/nodes/actuator_sim.py +++ b/simulation/sub8_gazebo/nodes/actuator_sim.py @@ -9,7 +9,7 @@ from geometry_msgs.msg import Pose, Twist from sub8_msgs.srv import SetValve -from sub8_ros_tools import msg_helpers, geometry_helpers +from mil_ros_tools import msg_helpers, geometry_helpers import numpy as np diff --git a/simulation/sub8_gazebo/nodes/gazebo_controller.py b/simulation/sub8_gazebo/nodes/gazebo_controller.py index 34160575..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 sub8_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 @@ -8,7 +8,7 @@ from gazebo_msgs.srv import ApplyBodyWrench from gazebo_msgs.msg import LinkStates, ModelState from sub8_gazebo.srv import ResetGazebo, ResetGazeboResponse -from uf_common.msg import Float64Stamped +from mil_msgs.msg import RangeStamped import numpy as np import os @@ -27,7 +27,7 @@ def __init__(self, target='sub8::base_link'): self.state_set_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1) self.raw_dvl_sub = rospy.Subscriber('dvl/range_raw', LaserScan, self.publish_height) - self.dvl_pub = rospy.Publisher('dvl/range', Float64Stamped, queue_size=1) + self.dvl_pub = rospy.Publisher('dvl/range', RangeStamped, queue_size=1) self.reset_srv = rospy.Service('gazebo/reset_gazebo', ResetGazebo, self.reset) self.state_pub = rospy.Publisher('odom', Odometry, queue_size=1) @@ -62,9 +62,9 @@ def wrench_cb(self, msg): def publish_height(self, msg): '''Sim DVL uses laserscan message to relay height''' self.dvl_pub.publish( - Float64Stamped( - header=sub8_utils.make_header(), - data=float(np.mean(msg.ranges)) + RangeStamped( + 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_gazebo/nodes/pinger_sim.py b/simulation/sub8_gazebo/nodes/pinger_sim.py index 530fb18d..e3e685ad 100755 --- a/simulation/sub8_gazebo/nodes/pinger_sim.py +++ b/simulation/sub8_gazebo/nodes/pinger_sim.py @@ -3,8 +3,8 @@ from gazebo_msgs.srv import GetModelState from sub8_msgs.srv import Sonar from sub8_sonar import EchoLocator -from sub8_ros_tools import msg_helpers -from sub8_ros_tools.geometry_helpers import rotate_vect_by_quat +from mil_ros_tools import msg_helpers +from mil_ros_tools.geometry_helpers import rotate_vect_by_quat import tf import numpy as np diff --git a/simulation/sub8_gazebo/nodes/turbulator.py b/simulation/sub8_gazebo/nodes/turbulator.py index bf24b6d6..09dc37da 100755 --- a/simulation/sub8_gazebo/nodes/turbulator.py +++ b/simulation/sub8_gazebo/nodes/turbulator.py @@ -2,7 +2,7 @@ import rospy from gazebo_msgs.srv import GetModelState, ApplyBodyWrenchRequest, ApplyBodyWrench, ApplyBodyWrenchResponse from sub8_gazebo.srv import SetTurbulence -from sub8_ros_tools import msg_helpers +from mil_ros_tools import msg_helpers import numpy as np diff --git a/simulation/sub8_montecarlo/sub8_montecarlo_tools/controller_verify.py b/simulation/sub8_montecarlo/sub8_montecarlo_tools/controller_verify.py index 29fa7a96..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 sub8_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 7a9361a2..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 sub8_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/meshes/meshes.py b/simulation/sub8_simulation/sub8_sim_tools/meshes/meshes.py index 08197eb5..5d09cb91 100644 --- a/simulation/sub8_simulation/sub8_sim_tools/meshes/meshes.py +++ b/simulation/sub8_simulation/sub8_sim_tools/meshes/meshes.py @@ -2,7 +2,7 @@ import os from time import time import numpy as np -from sub8_misc_tools import download_and_unzip +from mil_misc_tools import download_and_unzip def load_and_cache_mesh(file_path, url=None): diff --git a/simulation/sub8_simulation/sub8_sim_tools/physics/vehicle.py b/simulation/sub8_simulation/sub8_sim_tools/physics/vehicle.py index 30e6cc5a..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 sub8_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)) diff --git a/simulation/sub8_simulation/sub8_sim_tools/rendering/world.py b/simulation/sub8_simulation/sub8_sim_tools/rendering/world.py index 5d7ccec6..7f2c0568 100644 --- a/simulation/sub8_simulation/sub8_sim_tools/rendering/world.py +++ b/simulation/sub8_simulation/sub8_sim_tools/rendering/world.py @@ -1,6 +1,6 @@ from __future__ import division from sub8_sim_tools import Shaders, ShaderManager -from sub8_ros_tools import make_rotation, compose_transformation +from mil_ros_tools import make_rotation, compose_transformation from vispy import geometry, gloo import numpy as np from vispy.util.transforms import perspective, translate, rotate diff --git a/simulation/sub8_simulation/sub8_sim_tools/widgets/sub.py b/simulation/sub8_simulation/sub8_sim_tools/widgets/sub.py index 9dbfa0d4..ee3984dd 100644 --- a/simulation/sub8_simulation/sub8_sim_tools/widgets/sub.py +++ b/simulation/sub8_simulation/sub8_sim_tools/widgets/sub.py @@ -1,5 +1,5 @@ import numpy as np -from sub8_ros_tools import make_rotation, compose_transformation +from mil_ros_tools import make_rotation, compose_transformation from sub8_sim_tools import rendering from sub8_sim_tools.physics import Sub8 from sub8_sim_tools.meshes import Sub8 as Sub8Visual diff --git a/utils/sub8_ros_tools/CMakeLists.txt b/utils/sub8_ros_tools/CMakeLists.txt deleted file mode 100644 index b5d21353..00000000 --- a/utils/sub8_ros_tools/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sub8_ros_tools) -find_package(catkin REQUIRED COMPONENTS rostest std_msgs) -catkin_python_setup() -catkin_package() - -# Add folders to be run by python nosetests -if(CATKIN_ENABLE_TESTING) - catkin_add_nosetests(test_ros_tools) -endif() - diff --git a/utils/sub8_ros_tools/package.xml b/utils/sub8_ros_tools/package.xml deleted file mode 100644 index f95bf1b2..00000000 --- a/utils/sub8_ros_tools/package.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - sub8_ros_tools - 1.0.0 - The sub8_ros_tools package - Jacob Panikulam - MIT - catkin - rostest - rospy - - - \ No newline at end of file diff --git a/utils/sub8_ros_tools/readme.md b/utils/sub8_ros_tools/readme.md deleted file mode 100644 index 77fa0c13..00000000 --- a/utils/sub8_ros_tools/readme.md +++ /dev/null @@ -1,10 +0,0 @@ -sub8_ros_tools -============== - -Miscellaneous ROS tools that are useful for various tasks. - -Examples: - -# TODO -* The items that directly interact with ros topics or the parameter server do not yet have unittests, they require rostest. - diff --git a/utils/sub8_ros_tools/setup.py b/utils/sub8_ros_tools/setup.py deleted file mode 100644 index 5dfbc42f..00000000 --- a/utils/sub8_ros_tools/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['sub8_ros_tools', 'sub8_misc_tools', 'sub8_tools'], -) - -setup(**setup_args) diff --git a/utils/sub8_ros_tools/sub8_misc_tools/__init__.py b/utils/sub8_ros_tools/sub8_misc_tools/__init__.py deleted file mode 100644 index 86a0aa42..00000000 --- a/utils/sub8_ros_tools/sub8_misc_tools/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# flake8: noqa -from download import download_and_unzip -from download import download - -import text_effects diff --git a/utils/sub8_ros_tools/sub8_misc_tools/download.py b/utils/sub8_ros_tools/sub8_misc_tools/download.py deleted file mode 100644 index 988d9567..00000000 --- a/utils/sub8_ros_tools/sub8_misc_tools/download.py +++ /dev/null @@ -1,57 +0,0 @@ -import urllib2 -import os -import zipfile -import cStringIO as StringIO - -''' -This file contains utilities for downloading a file from the internet -We're using this because I don't want to track 20MB files in Git. - -[1] Extracting zipfiles - http://stackoverflow.com/questions/9431918 - -[2] Unzip binary directly - http://stackoverflow.com/questions/18966672 - -[3] Download a file via http - http://stackoverflow.com/questions/22676 -''' - - -def download_and_unzip(url, output_dir): - '''Download and unzip a file at $url, - then put the contained files at $output_dir - ''' - try: - html = download(url) - except: - raise(IOError("Could not load file at {}".format(url))) - - fake_file = StringIO.StringIO(html) - - zip_ = zipfile.ZipFile(fake_file, "r") - for file_path in zip_.namelist(): - path, file_name = os.path.split(file_path) - file_like = zip_.open(file_path) - - f = open(os.path.join(output_dir, file_name), 'w') - f.write(file_like.read()) - f.close() - - -def download(url, output_filename=None): - '''Download a file at $url, and return the html - If you set an output location, it will also write the file - ''' - response = urllib2.urlopen(url) - html = response.read() - if output_filename is not None: - f = open(output_filename, 'w') - f.write(html) - f.close() - return html - - -if __name__ == '__main__': - sub_model_url = "http://goo.gl/f0ennf?gdriveurl" - download_and_unzip(sub_model_url, '.') diff --git a/utils/sub8_ros_tools/sub8_misc_tools/text_effects.py b/utils/sub8_ros_tools/sub8_misc_tools/text_effects.py deleted file mode 100644 index 15e6121d..00000000 --- a/utils/sub8_ros_tools/sub8_misc_tools/text_effects.py +++ /dev/null @@ -1,146 +0,0 @@ -class Colors(): - red = '\033[31m' - green = '\033[32m' - yellow = '\033[33m' - blue = '\033[34m' - purple = '\033[35m' - cyan = '\033[36m' - white = '\033[37m' - - underline = '\033[2m' - bold = '\033[1m' - negative = '\033[3m' - - reset = '\033[0m' - - def __getattr__(self, arg): - # If we get a non existent color, return the reset color - return self.reset - - -class Printer(object): - def __init__(self, string="", autospace=False): - self._autospace = autospace - self._string = string - - # Default adding text - self.text = lambda text: self + text - - # Colors - self.red = lambda text: self + (Colors.red + str(text) + Colors.reset) - self.green = lambda text: self + (Colors.green + str(text) + Colors.reset) - self.yellow = lambda text: self + (Colors.yellow + str(text) + Colors.reset) - self.blue = lambda text: self + (Colors.blue + str(text) + Colors.reset) - self.purple = lambda text: self + (Colors.purple + str(text) + Colors.reset) - self.cyan = lambda text: self + (Colors.cyna + str(text) + Colors.reset) - self.white = lambda text: self + (Colors.white + str(text) + Colors.reset) - - # Text effects - self.underline = lambda text: Printer(self._string + Colors.underline + str(text) + Colors.reset) - self.bold = lambda text: Printer(self._string + Colors.bold + str(text) + Colors.reset) - self.negative = lambda text: Printer(self._string + Colors.negative + str(text) + Colors.reset) - - # For passing in custom formatting - self.custom = lambda text, effect: self + (effect + str(text) + Colors.reset) - - def __repr__(self): - return self._string + Colors.reset - __str__ = __repr__ - - def __add__(self, other): - extra_space = ' ' if self._autospace and self._string is not '' else '' - return Printer(self._string + extra_space + str(other), self._autospace) - - @property - def set_red(self): - return Printer(self._string + Colors.red) - - @property - def set_green(self): - return Printer(self._string + Colors.green) - - @property - def set_yellow(self): - return Printer(self._string + Colors.yellow) - - @property - def set_blue(self): - return Printer(self._string + Colors.blue) - - @property - def set_purple(self): - return Printer(self._string + Colors.purple) - - @property - def set_cyan(self): - return Printer(self._string + Colors.cyan) - - @property - def set_white(self): - return Printer(self._string + Colors.white) - - @property - def reset(self): - return Printer(self._string + Colors.reset) - - def space(self, count=1): - return Printer(self._string + ' ' * count) - - def newline(self, count=1): - return Printer(self._string + '\n' * count) - - def enable_autospaces(self): - self._autospace = False - - def disable_autospaces(self): - self._autospace = True - - -class FprintFactory(object): - def __init__(self, title=None, time=None, msg_color=None, auto_bold=True, newline=1): - assert time is None or callable(time), "`time` should be `None` for no printing or a function that generates a timestamp." - assert msg_color is None or isinstance(msg_color, str), "`msg_color` should be `None` for default printing or a string color." - assert isinstance(auto_bold, bool), "`auto_bold` should be true or false if messages should be printed as bold by default or not" - assert newline is None or isinstance(newline, int), "`newline` should be the number of newlines after the text (default 1)" - - # All these can be overwritten if not specified here - self.title = title # Title to print with each message - self.time = time # Either `None` for no printing or a function that generates a timestamp - self.msg_color = msg_color # Either `None` for deafult printing or a string color - self.auto_bold = auto_bold # Should each message be bolded by default - self.newline = newline # The number of newlines characters to add to the end - - self.printer = Printer() - - def fprint(self, text, **kwargs): - title = kwargs.get("title", self.title) - time = kwargs.get("time", self.time) - msg_color = kwargs.get("msg_color", self.msg_color) - auto_bold = kwargs.get("auto_bold", self.auto_bold) - newline = kwargs.get("newline", self.newline) - - message = self.printer - if title is not None: - message = message.set_blue.bold(title).reset.space() - - if time is not None: - t = time() - message = message.bold(t).space() - - message += ": " - - if auto_bold: - text = str(self.printer.bold(text)) - - if msg_color is not None: - message = message.custom(text, getattr(Colors, msg_color)) - else: - message = message.text(text) - - if newline == 1: - print message - else: - print message.newline(newline - 1) - -# Standard instantiation -fprint = FprintFactory().fprint diff --git a/utils/sub8_ros_tools/sub8_ros_tools/__init__.py b/utils/sub8_ros_tools/sub8_ros_tools/__init__.py deleted file mode 100644 index f0d53137..00000000 --- a/utils/sub8_ros_tools/sub8_ros_tools/__init__.py +++ /dev/null @@ -1,14 +0,0 @@ -# flake8: noqa -import image_helpers -import init_helpers -import msg_helpers -import threading_helpers -import geometry_helpers -import func_helpers - -from image_helpers import * -from init_helpers import * -from msg_helpers import * -from threading_helpers import * -from geometry_helpers import * -from func_helpers import * diff --git a/utils/sub8_ros_tools/sub8_ros_tools/func_helpers.py b/utils/sub8_ros_tools/sub8_ros_tools/func_helpers.py deleted file mode 100644 index 8b2fc4c1..00000000 --- a/utils/sub8_ros_tools/sub8_ros_tools/func_helpers.py +++ /dev/null @@ -1,38 +0,0 @@ -class Cache(object): - """No support for **kwargs**""" - def __init__(self, func): - self.call_dict = {} - self.func = func - - def __call__(self, *args): - if args not in self.call_dict.keys(): - result = self.func(*args) - self.call_dict[args] = result - else: - result = self.call_dict[args] - return result - - -@Cache -def add(a, b): - print 'adding', a, b - return a + b - - -@Cache -def gooberstein(data): - print "Being called" - return data + "balls" - - -if __name__ == '__main__': - - print add(1, 2) - print add(1, 2) - - print add(2, 3) - print add(2, 3) - - print gooberstein("hello") - print gooberstein("hello") - print gooberstein("hello") diff --git a/utils/sub8_ros_tools/sub8_ros_tools/geometry_helpers.py b/utils/sub8_ros_tools/sub8_ros_tools/geometry_helpers.py deleted file mode 100644 index 06c4ff0f..00000000 --- a/utils/sub8_ros_tools/sub8_ros_tools/geometry_helpers.py +++ /dev/null @@ -1,120 +0,0 @@ -from __future__ import division -import numpy as np -from tf import transformations -import tf - - -def rotate_vect_by_quat(v, q): - ''' - Rotate a vector by a quaterion. - v' = q*vq - - q should be [x,y,z,w] (standard ros convention) - ''' - cq = np.array([-q[0], -q[1], -q[2], q[3]]) - cq_v = tf.transformations.quaternion_multiply(cq, v) - v = tf.transformations.quaternion_multiply(cq_v, q) - v[1:] *= -1 # Only seemed to work when I flipped this around, is there a problem with the math here? - return np.array(v)[:3] - - -def make_rotation(vector_a, vector_b): - '''Determine a 3D rotation that rotates A onto B - In other words, we want a matrix R that aligns a with b - - >> R = make_rotation(a, b) - >> p = R.dot(a) - >> np.cross(p, a) - >>> array([0.0, 0.0, 0.0]) - - [1] Calculate Rotation Matrix to align Vector A to Vector B in 3d? - http://math.stackexchange.com/questions/180418 - [2] N. Ho, Finding Optimal Rotation...Between Corresponding 3D Points - http://nghiaho.com/?page_id=671 - ''' - unit_a = normalize(vector_a) - unit_b = normalize(vector_b) - - v = np.cross(unit_a, unit_b) - s = np.linalg.norm(v) - - c = np.dot(unit_a, unit_b) - - skew_cross = skew_symmetric_cross(v) - skew_squared = np.linalg.matrix_power(skew_cross, 2) - - if np.isclose(c, 1.0, atol=1e-4): - R = np.eye(3) - return R - elif np.isclose(c, -1.0, atol=1e-4): - R = np.eye(3) - R[2, 2] *= -1 - return R - - normalization = (1 - c) / (s ** 2) - - R = np.eye(3) + skew_cross + (skew_squared * normalization) - - # Address the reflection case - if np.linalg.det(R) < 0: - R[:, 3] *= -1 - - return R - - -def skew_symmetric_cross(a): - '''Return the skew symmetric matrix representation of a vector - [1] https://en.wikipedia.org/wiki/Cross_product#Skew-symmetric_matrix - ''' - assert len(a) == 3, "a must be in R3" - skew_symm = np.array([ - [+0.00, -a[2], +a[1]], - [+a[2], +0.00, -a[0]], - [-a[1], +a[0], +0.00], - ], dtype=np.float32) - return skew_symm - - -def deskew(A): - return np.array([A[2, 1], A[0, 2], A[1, 0]], dtype=np.float32) - - -def normalize(vector): - return vector / np.linalg.norm(vector) - - -def compose_transformation(R, t): - '''Compose a transformation from a rotation matrix and a translation matrix''' - transformation = np.zeros((4, 4)) - transformation[:3, :3] = R - transformation[3, :3] = t - transformation[3, 3] = 1.0 - return transformation - - -def project_pt_to_plane(point, plane_normal): - dist = np.dot(plane_normal, point) - projected = point - (dist * plane_normal) - return projected - - -def clip_norm(vector, lower_bound, upper_bound): - '''Return a vector pointing the same direction as $vector, - with maximum norm $bound - if norm(vector) < bound, return vector unchanged - - Like np.clip, but for vector norms - ''' - norm = np.linalg.norm(vector) - if lower_bound < norm < upper_bound: - return np.copy(vector) - if norm < lower_bound: - v_new = (vector * lower_bound) / norm - else: - v_new = (vector * upper_bound) / norm - return v_new - - -def quaternion_matrix(q): - mat_h = transformations.quaternion_matrix(q) - return mat_h[:3, :3] / mat_h[3, 3] diff --git a/utils/sub8_ros_tools/sub8_ros_tools/image_helpers.py b/utils/sub8_ros_tools/sub8_ros_tools/image_helpers.py deleted file mode 100644 index 7fb17214..00000000 --- a/utils/sub8_ros_tools/sub8_ros_tools/image_helpers.py +++ /dev/null @@ -1,113 +0,0 @@ -#!/usr/bin/python -''' -Note: - The repeated use of CvBridge (instead of using make_image_msg and get_image_msg in the classes) - is intentional, to avoid the use of a global cvbridge, and to avoid reinstantiating a CvBrige for each use. -''' -import rospy -import numpy as np -from os import path -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image, CameraInfo -from sub8_ros_tools.init_helpers import wait_for_param - - -def get_parameter_range(parameter_root): - ''' - ex: parameter_root='/vision/buoy/red' - this will then fetch /vision/buoy/red/hsv_low and hsv_high - ''' - low_param, high_param = parameter_root + '/hsv_low', parameter_root + '/hsv_high' - - rospy.logwarn("Blocking -- waiting for parameters {} and {}".format(low_param, high_param)) - - wait_for_param(low_param) - wait_for_param(high_param) - low = rospy.get_param(low_param) - high = rospy.get_param(high_param) - - rospy.loginfo("Got {} and {}".format(low_param, high_param)) - return np.array([low, high]).transpose() - - -def make_image_msg(cv_image, encoding='bgr8'): - '''Take a cv image, and produce a ROS image message''' - bridge = CvBridge() - image_message = bridge.cv2_to_imgmsg(cv_image, encoding) - return image_message - - -def get_image_msg(ros_image, encoding='bgr8'): - '''Take a ros image message, and yield an opencv image''' - bridge = CvBridge() - cv_image = bridge.imgmsg_to_cv2(ros_image, desired_encoding=encoding) - return cv_image - - -class Image_Publisher(object): - def __init__(self, topic, encoding="bgr8", queue_size=1): - '''Create an essentially normal publisher, that will publish images without conversion hassle''' - self.bridge = CvBridge() - self.encoding = encoding - self.im_pub = rospy.Publisher(topic, Image, queue_size=queue_size) - - def publish(self, cv_image): - try: - image_message = self.bridge.cv2_to_imgmsg(cv_image, self.encoding) - self.im_pub.publish(image_message) - except CvBridgeError, e: - # Intentionally absorb CvBridge Errors - rospy.logerr(e) - - -class Image_Subscriber(object): - def __init__(self, topic, callback=None, encoding="bgr8", queue_size=1): - '''Calls $callback on each image every time a new image is published on $topic - Assumes topic of type "sensor_msgs/Image" - This behaves like a conventional subscriber, except handling the additional image conversion - ''' - if callback is None: - callback = lambda im: setattr(self, 'last_image', im) - - self.encoding = encoding - self.camera_info = None - self.last_image_time = None - self.last_image = None - self.im_sub = rospy.Subscriber(topic, Image, self.convert, queue_size=queue_size) - - root_topic, image_subtopic = path.split(topic) - self.info_sub = rospy.Subscriber(root_topic + '/camera_info', CameraInfo, self.info_cb, queue_size=queue_size) - - self.bridge = CvBridge() - self.callback = callback - - def wait_for_camera_info(self, timeout=10): - ''' - Blocks until camera info has been received. - Note: 'timeout' is in seconds. - ''' - rospy.logwarn("Blocking -- waiting at most %d seconds for camera info." % timeout) - end_time = rospy.Time.now() + rospy.Duration(timeout) - while (rospy.Time.now() < end_time) and (not rospy.is_shutdown()): - if self.camera_info is not None: - rospy.loginfo("Camera info found!") - return self.camera_info - rospy.sleep(.2) - if self.camera_info is None: - rospy.logerr("Camera info not found.") - raise Exception("Camera info not found.") - return self.camera_info - - def info_cb(self, msg): - """The path trick here is a hack""" - self.info_sub.unregister() - self.camera_info = msg - - def convert(self, data): - self.last_image_time = data.header.stamp - try: - image = self.bridge.imgmsg_to_cv2(data, desired_encoding=self.encoding) - self.callback(image) - except CvBridgeError, e: - # Intentionally absorb CvBridge Errors - rospy.logerr(e) diff --git a/utils/sub8_ros_tools/sub8_ros_tools/init_helpers.py b/utils/sub8_ros_tools/sub8_ros_tools/init_helpers.py deleted file mode 100644 index 75441670..00000000 --- a/utils/sub8_ros_tools/sub8_ros_tools/init_helpers.py +++ /dev/null @@ -1,54 +0,0 @@ -import rospy -import rostest -import time - - -def wait_for_param(param_name, timeout=None, poll_rate=0.1): - '''Blocking wait for a parameter named $parameter_name to exist - Poll at frequency $poll_rate - Once the parameter exists, return get and return it. - - This function intentionally leaves failure logging duties to the developer - ''' - start_time = time.time() - rate = rospy.Rate(poll_rate) - while not rospy.is_shutdown(): - - # Check if the parameter now exists - if rospy.has_param(param_name): - return rospy.get_param(param_name) - - # If we exceed a defined timeout, return None - if timeout is not None: - if time.time() - start_time > timeout: - return None - - # Continue to poll at poll_rate - rate.sleep() - - -def wait_for_subscriber(node_name, topic, timeout=5.0): - '''Blocks until $node_name subscribes to $topic - Useful mostly in integration tests -- - I would counsel against use elsewhere - ''' - end_time = time.time() + timeout - - # Wait for time-out or ros-shutdown - while (time.time() < end_time) and (not rospy.is_shutdown()): - subscribed = rostest.is_subscriber( - rospy.resolve_name(topic), - rospy.resolve_name(node_name) - ) - # Success scenario: node subscribes - if subscribed: - break - time.sleep(0.1) - - # Could do this with a while/else - # But chose to explicitly check - success = rostest.is_subscriber( - rospy.resolve_name(topic), - rospy.resolve_name(node_name) - ) - return success diff --git a/utils/sub8_ros_tools/sub8_ros_tools/msg_helpers.py b/utils/sub8_ros_tools/sub8_ros_tools/msg_helpers.py deleted file mode 100644 index 82637d77..00000000 --- a/utils/sub8_ros_tools/sub8_ros_tools/msg_helpers.py +++ /dev/null @@ -1,186 +0,0 @@ -import numpy as np -from tf import transformations -import geometry_msgs.msg as geometry_msgs -import std_msgs.msg as std_msgs -import nav_msgs.msg as nav_msgs -import rospy - - -def rosmsg_to_numpy(rosmsg, keys=None): - '''Convert an arbitrary ROS msg to a numpy array - With no additional arguments, it will by default handle: - Point2D, Point3D, Vector3D, and quaternions - - Ex: - quat = Quaternion(1.0, 0.0, 0.0, 0.0) - quat is not a vector, you have quat.x, quat.y,... and you can't do math on that - - But wait, there's hope! - rosmsg_to_numpy(quat) -> array([1.0, 0.0, 0.0, 0.0]) - - Yielding a vector, which you can do math on! - - Further, imagine a bounding box message, BB, with properties BB.x, BB.h, BB.y, and BB.w - - rosmsg_to_numpy(BB, ['x', 'h', 'y', 'w']) -> array([BB.x, BB.h, BB.y, BB.w]) - - or... - rosmsg_to_numpy(some_Pose2D, ['x', 'y', 'yaw']) = array([x, y, yaw]) - - Note: - - This function is designed to handle the most common use cases (vectors, points and quaternions) - without requiring any additional arguments. - ''' - if keys is None: - keys = ['x', 'y', 'z', 'w'] - output_array = [] - for key in keys: - # This is not necessarily the fastest way to do this - if hasattr(rosmsg, key): - output_array.append(getattr(rosmsg, key)) - else: - break - - return np.array(output_array).astype(np.float32) - - else: - output_array = np.zeros(len(keys), np.float32) - for n, key in enumerate(keys): - output_array[n] = getattr(rosmsg, key) - - return output_array - - -def pose_to_numpy(pose): - '''TODO: Unit-test - returns (position, orientation) - ''' - position = rosmsg_to_numpy(pose.position) - orientation = rosmsg_to_numpy(pose.orientation) - return position, orientation - - -def twist_to_numpy(twist): - '''TODO: Unit-test - Convert a twist message into a tuple of numpy arrays - returns (linear, angular) - ''' - linear = rosmsg_to_numpy(twist.linear) - angular = rosmsg_to_numpy(twist.angular) - return linear, angular - - -def posetwist_to_numpy(posetwist): - pose = pose_to_numpy(posetwist.pose) - twist = twist_to_numpy(posetwist.twist) - return pose, twist - - -def odometry_to_numpy(odom): - '''TODO: Unit-test - Convert an odometry message into a tuple of numpy arrays - returns (pose, twist, pose_covariance, twist_covariance) - ''' - pose = pose_to_numpy(odom.pose.pose) - pose_covariance = np.array(odom.pose.covariance).reshape(6, 6) - - twist = twist_to_numpy(odom.twist.twist) - twist_covariance = np.array(odom.twist.covariance).reshape(6, 6) - - return pose, twist, pose_covariance, twist_covariance - - -def wrench_to_numpy(wrench): - force = rosmsg_to_numpy(wrench.force) - torque = rosmsg_to_numpy(wrench.torque) - return force, torque - - -def numpy_to_point(np_vector): - return geometry_msgs.Point(*np_vector) - - -def numpy_to_quaternion(np_quaternion): - return geometry_msgs.Quaternion(*np_quaternion) - - -def numpy_to_twist(linear_vel, angular_vel): - '''TODO: Unit-test - ''' - return geometry_msgs.Twist(linear=geometry_msgs.Vector3(*linear_vel), angular=geometry_msgs.Vector3(*angular_vel)) - - -def numpy_to_wrench(forcetorque): - return geometry_msgs.Wrench( - force=geometry_msgs.Vector3(*forcetorque[:3]), - torque=geometry_msgs.Vector3(*forcetorque[3:]) - ) - - -def numpy_matrix_to_quaternion(np_matrix): - '''Given a 3x3 rotation matrix, return its geometry_msgs Quaternion''' - assert np_matrix.shape == (3, 3), "Must submit 3x3 rotation matrix" - pose_mat = np.eye(4) - pose_mat[:3, :3] = np_matrix - np_quaternion = transformations.quaternion_from_matrix(pose_mat) - return geometry_msgs.Quaternion(*np_quaternion) - - -def numpy_pair_to_pose(np_translation, np_rotation_matrix): - '''Convert a R, t pair to a geometry_msgs Pose''' - orientation = numpy_matrix_to_quaternion(np_rotation_matrix) - position = numpy_to_point(np_translation) - return geometry_msgs.Pose(position=position, orientation=orientation) - - -def numpy_quat_pair_to_pose(np_translation, np_quaternion): - orientation = numpy_to_quaternion(np_quaternion) - position = numpy_to_point(np_translation) - return geometry_msgs.Pose(position=position, orientation=orientation) - - -def make_header(frame='/body', stamp=None): - if stamp is None: - try: - stamp = rospy.Time.now() - except rospy.ROSInitException: - stamp = rospy.Time(0) - - header = std_msgs.Header( - stamp=stamp, - frame_id=frame - ) - return header - - -def make_wrench_stamped(force, torque, frame='/body'): - '''Make a WrenchStamped message without all the fuss - Frame defaults to body - ''' - wrench = geometry_msgs.WrenchStamped( - header=make_header(frame), - wrench=geometry_msgs.Wrench( - force=geometry_msgs.Vector3(*force), - torque=geometry_msgs.Vector3(*torque) - ) - ) - return wrench - - -def make_pose_stamped(position, orientation, frame='/body'): - wrench = geometry_msgs.WrenchStamped( - header=make_header(frame), - pose=geometry_msgs.Pose( - force=geometry_msgs.Vector3(*position), - orientation=geometry_msgs.Quaternion(*orientation) - ) - ) - return wrench - - -def odom_sub(topic, callback): - def wrapped_callback(*args): - msg = args[-1] - callback(odometry_to_numpy(msg)) - - return rospy.Subscriber(topic, nav_msgs.Odometry, wrapped_callback, queue_size=1) diff --git a/utils/sub8_ros_tools/sub8_ros_tools/threading_helpers.py b/utils/sub8_ros_tools/sub8_ros_tools/threading_helpers.py deleted file mode 100644 index 255298d4..00000000 --- a/utils/sub8_ros_tools/sub8_ros_tools/threading_helpers.py +++ /dev/null @@ -1,30 +0,0 @@ -def thread_lock(lock): - '''Use an existing thread lock to thread-lock a function - This prevents the function from being executed by multiple threads at once - - Example: - import threading - lock = threading.Lock() - - @thread_lock(lock) - def my_function(a, b, c): - print a, b, c - ''' - - def lock_thread(function_to_lock): - '''thread_lock(function) -> locked function - Thread locking decorator - If you use this as a decorator for a function, it will apply a threading lock during the execution of that function, - Which guarantees that no ROS callbacks can change the state of data while it is executing. This - is critical to make sure that a new message being sent doesn't cause a weird serial interruption - ''' - def locked_function(*args, **kwargs): - # Get threading lock - with lock: - result = function_to_lock(*args, **kwargs) - # Return, pretending the function hasn't changed at all - return result - # Return the function with locking added - return locked_function - - return lock_thread diff --git a/utils/sub8_ros_tools/sub8_tools/__init__.py b/utils/sub8_ros_tools/sub8_tools/__init__.py deleted file mode 100644 index c36a9ccb..00000000 --- a/utils/sub8_ros_tools/sub8_tools/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -from sub8_ros_tools import * -from sub8_misc_tools import * diff --git a/utils/sub8_ros_tools/test_ros_tools/test_ros_tools.py b/utils/sub8_ros_tools/test_ros_tools/test_ros_tools.py deleted file mode 100644 index 7ced2bf7..00000000 --- a/utils/sub8_ros_tools/test_ros_tools/test_ros_tools.py +++ /dev/null @@ -1,142 +0,0 @@ -#!/usr/bin/env python -import unittest -import numpy as np -from geometry_msgs.msg import Quaternion, Vector3, Pose2D -from sensor_msgs.msg import Image -from sub8_ros_tools.image_helpers import make_image_msg, get_image_msg -from sub8_ros_tools.msg_helpers import rosmsg_to_numpy, make_wrench_stamped -from sub8_ros_tools.threading_helpers import thread_lock -from sub8_ros_tools.geometry_helpers import skew_symmetric_cross, make_rotation, normalize - - -class TestROSTools(unittest.TestCase): - def test_rosmsg_to_numpy_quaternion(self): - '''Test a rosmsg conversion for a geometry_msgs/Quaternion''' - # I know this is not a unit quaternion - q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2) - numpy_array = rosmsg_to_numpy(q) - - np.testing.assert_allclose( - np.array([0.7, 0.7, 0.1, 0.2]), - numpy_array - ) - - def test_rosmsg_to_numpy_vector(self): - '''Test a rosmsg conversion for a geometry_msgs/Vector''' - v = Vector3(x=0.1, y=99., z=21.) - numpy_array = rosmsg_to_numpy(v) - - np.testing.assert_allclose( - np.array([0.1, 99., 21.]), - numpy_array - ) - - def test_rosmsg_to_numpy_custom(self): - '''Test a rosmsg conversion for a custom msg''' - pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14) - numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta']) - - np.testing.assert_allclose( - np.array([1.0, 2.0, 3.14]), - numpy_array - ) - - def test_make_wrench_stamped(self): - '''Test that wrenchmaking works''' - for k in range(10): - force = np.random.random(3) * 10 - torque = np.random.random(3) * 10 - wrench_msg = make_wrench_stamped(force, torque, frame='/enu') - - msg_force = rosmsg_to_numpy(wrench_msg.wrench.force) # noqa - msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque) # noqa - self.assertIsNotNone(msg_force) - self.assertIsNotNone(msg_torque) - - def test_make_image_msg(self): - '''Test that make ros image message doesn't fail''' - im = np.ones((255, 255, 3)).astype(np.uint8) - im_msg = make_image_msg(im) - self.assertIsInstance(im_msg, Image) - - def test_get_image_msg(self): - '''Test that a made ros image can be returned to its original form''' - im = np.ones((255, 255, 3)).astype(np.uint8) - im_msg = make_image_msg(im) - - cv_im = get_image_msg(im_msg) - np.testing.assert_array_equal(im, cv_im) - - def test_thread_lock(self): - '''Test that the thread lock decorator correctly locks, in the correct order''' - class FakeLock(object): - def __init__(self): - self.entry = False - self.exit = False - - def __enter__(self): - self.entry = True - - def __exit__(self, *args): - self.exit = True - - fake_lock = FakeLock() - - @thread_lock(fake_lock) - def lock_test(a): - return (fake_lock.entry is True) and (fake_lock.exit is False) - - result = lock_test(1) - - self.assertTrue(fake_lock.entry, msg='Thread was never locked') - self.assertTrue(fake_lock.exit, msg='Thread was never released') - self.assertTrue(result, msg='Thread was not locked while the function was executed') - - def test_skew_symmetric_cross(self): - '''Test that the skew symmetric cross product matrix produces the definition - [1] https://en.wikipedia.org/wiki/Cross_product#Skew-symmetric_matrix - ''' - skew_sym = skew_symmetric_cross([1, 2, 3]) - truth = np.array([ - [+0, -3, +2], - [+3, +0, -1], - [-2, +1, +0], - ]) - np.testing.assert_allclose(skew_sym, truth, err_msg="Did not make a Skew-symmetric matrix. Pretty big screw-up imho.") - - def test_make_rotation(self): - '''Test several random vector pairs, and see if we can generate a valid alignment''' - scaling = 10 - for k in range(10): - p = (np.random.random(3) - 0.5) * scaling - q = (np.random.random(3) - 0.5) * scaling - - R = make_rotation(p, q) - p_rotated = R.dot(p) - - # Test that the matrix actually aligns p with q - np.testing.assert_allclose( - [0.0, 0.0, 0.0], np.cross(p_rotated, q), atol=1e-5, - err_msg="The generated rotation matrix did not align the input vectors, {} to {}".format( - p, q - ) - ) - self.assertGreater(np.dot(p_rotated, q), 0.0, msg="The rotation did wacky inversion") - - def test_normalize_vector(self): - '''Test vector normalization''' - for k in range(10): - rand_vec = np.random.random(k) # Make a random k-length vector - - # Ignore the astronomically unlikely case that a vector has near 0 norm - if not np.isclose(np.sum(rand_vec), 0): - normalized = normalize(rand_vec) - norm = np.linalg.norm(normalized) - - # Test that the norm is 1 - np.testing.assert_almost_equal(norm, 1.0, err_msg="The normalized vector did not have length 1") - - -if __name__ == '__main__': - suite = unittest.TestLoader().loadTestsFromTestCase(TestROSTools) - unittest.TextTestRunner(verbosity=2).run(suite) diff --git a/utils/uf_common/CMakeLists.txt b/utils/uf_common/CMakeLists.txt deleted file mode 100644 index 62efc331..00000000 --- a/utils/uf_common/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(uf_common) -find_package(catkin REQUIRED COMPONENTS message_generation message_runtime geometry_msgs tf actionlib interactive_markers std_msgs actionlib_msgs cmake_modules) -catkin_python_setup() - -add_action_files( - FILES - MoveTo.action -) - - -add_message_files( - FILES - PoseTwistStamped.msg - PoseTwist.msg - VelocityMeasurements.msg - Float64Stamped.msg - VelocityMeasurement.msg - Acceleration.msg -) - -find_package(Eigen REQUIRED) -include_directories(${Eigen_INCLUDE_DIRS}) -generate_messages( - DEPENDENCIES std_msgs actionlib_msgs geometry_msgs uf_common -) -catkin_package( - DEPENDS Eigen - CATKIN_DEPENDS message_generation message_runtime geometry_msgs tf actionlib interactive_markers std_msgs actionlib_msgs - INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} include - LIBRARIES -) - -include_directories(${EIGEN_INCLUDE_DIRS} include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) -install(PROGRAMS scripts/interactive_marker_moveto scripts/velocitymeasurements_to_vector3 scripts/param_saver DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/utils/uf_common/action/MoveTo.action b/utils/uf_common/action/MoveTo.action deleted file mode 100644 index c2ec5960..00000000 --- a/utils/uf_common/action/MoveTo.action +++ /dev/null @@ -1,11 +0,0 @@ -# goal. copies PoseTwistStamped. -Header header -uf_common/PoseTwist posetwist -float64 speed -bool uncoordinated # false goes in a straight line, true achieves some components before others -float64 linear_tolerance # distance from goal for result to be sent -float64 angular_tolerance ---- -# result ---- -# feedback diff --git a/utils/uf_common/include/uf_common/msg_helpers.h b/utils/uf_common/include/uf_common/msg_helpers.h deleted file mode 100644 index 548a98f8..00000000 --- a/utils/uf_common/include/uf_common/msg_helpers.h +++ /dev/null @@ -1,79 +0,0 @@ -#ifndef UF_COMMON__MSG_HELPERS_H -#define UF_COMMON__MSG_HELPERS_H - -#include -#include - -namespace uf_common { - -template -inline T make_rgba(double r, double g, double b, double a) { - T c; - c.r = r; - c.g = g; - c.b = b; - c.a = a; - return c; -} - -template -inline T make_xyz(double x, double y, double z) { - T p; - p.x = x; - p.y = y; - p.z = z; - return p; -} - -template -inline T vec2xyz(Eigen::Vector3d v) { - return make_xyz(v(0), v(1), v(2)); -} -template -inline T vec2xyz(tf::Vector3 v) { - return make_xyz(v.x(), v.y(), v.z()); -} -template -inline Eigen::Vector3d xyz2vec(T m) { - return Eigen::Vector3d(m.x, m.y, m.z); -} - -inline Eigen::Vector3d vec2vec(tf::Vector3 v) { return Eigen::Vector3d(v[0], v[1], v[2]); } -inline tf::Vector3 vec2vec(Eigen::Vector3d v) { return tf::Vector3(v[0], v[1], v[2]); } -inline Eigen::Quaterniond quat2quat(const tf::Quaternion &q) { - return Eigen::Quaterniond(q[3], q[0], q[1], q[2]); -} -inline tf::Quaternion quat2quat(const Eigen::Quaterniond &q) { - return tf::Quaternion(q.x(), q.y(), q.z(), q.w()); -} - -template -inline T make_xyzw(double x, double y, double z, double w) { - T q; - q.x = x; - q.y = y; - q.z = z; - q.w = w; - return q; -} - -template -inline T vec2xyzw(Eigen::Vector4d v) { - return vec2xyzw(v(0), v(1), v(2), v(3)); -} -template -inline Eigen::Quaterniond xyzw2quat(T m) { - return Eigen::Quaterniond(m.w, m.x, m.y, m.z); -} -template -inline T vec2wxyz(Eigen::Vector4d v) { - return make_xyzw(v(1), v(2), v(3), v(0)); -} -inline tf::Quaternion vec2quat(Eigen::Vector4d v) { return tf::Quaternion(v[0], v[1], v[2], v[3]); } -template -inline T quat2xyzw(Eigen::Quaterniond q) { - return make_xyzw(q.x(), q.y(), q.z(), q.w()); -} -} - -#endif diff --git a/utils/uf_common/include/uf_common/param_helpers.h b/utils/uf_common/include/uf_common/param_helpers.h deleted file mode 100644 index fee648cf..00000000 --- a/utils/uf_common/include/uf_common/param_helpers.h +++ /dev/null @@ -1,127 +0,0 @@ -#ifndef UF_COMMON__PARAM_HELPERS_H -#define UF_COMMON__PARAM_HELPERS_H - -#include -#include -#include - -#include "msg_helpers.h" - -namespace uf_common { - -void fail(std::string const &error_string) { throw std::runtime_error(error_string); } -template -void fail(FirstType first, SecondType second, MoreTypes... more) { - std::ostringstream ss; - ss << first << second; - return fail(ss.str(), more...); -} - -template -void require(bool cond, ErrorDescTypes... error_desc) { - if (!cond) { - fail(error_desc...); - } -} - -template -bool _getParam(ros::NodeHandle &nh, const std::string &name, Eigen::Matrix &res) { - XmlRpc::XmlRpcValue my_list; - if (!nh.getParam(name, my_list)) return false; - require(my_list.getType() == XmlRpc::XmlRpcValue::TypeArray, "param ", name, " must be an array"); - if (N != Eigen::Dynamic) { - require(my_list.size() == N, "param ", name, "must have length ", N, " (is ", my_list.size(), - ")"); - } - - res.resize(my_list.size()); - for (int32_t i = 0; i < my_list.size(); i++) { - if (my_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) { - res(i) = static_cast(my_list[i]); - } else if (my_list[i].getType() == XmlRpc::XmlRpcValue::TypeInt) { - res(i) = static_cast(my_list[i]); - } else { - fail("param ", name, "[", i, "] is not numeric"); - } - } - return true; -} -template -bool _getParam(ros::NodeHandle &nh, const std::string &name, Eigen::Matrix &res) { - static_assert(N != 0, "doesn't work for N = 0"); - static_assert(M != 1, "wrong template specialization used?"); - - XmlRpc::XmlRpcValue my_list; - if (!nh.getParam(name, my_list)) return false; - require(my_list.getType() == XmlRpc::XmlRpcValue::TypeArray, "param ", name, " must be an array"); - require(my_list.size() >= 1, "param " + name + " must not have zero length"); - if (N != Eigen::Dynamic) { - require(my_list.size() == N, "param ", name, "must have length ", N, " (is ", my_list.size(), - ")"); - } - - require(my_list[0].getType() == XmlRpc::XmlRpcValue::TypeArray, "param ", name, - "[0] must be a list"); - if (M != Eigen::Dynamic) { - require(my_list[0].size() == M, "param ", name, "[0] must have length ", M, " (is ", - my_list[0].size(), ")"); - } - - res.resize(my_list.size(), my_list[0].size()); - for (int32_t i = 0; i < my_list.size(); i++) { - XmlRpc::XmlRpcValue row = my_list[i]; - require(row.getType() == XmlRpc::XmlRpcValue::TypeArray, "param ", name, "[", i, - "] must be a list"); - require(row.size() == my_list[0].size(), "param ", name, "[", i, "]'s length doesn't match"); - for (int32_t j = 0; j < row.size(); j++) { - XmlRpc::XmlRpcValue entry = row[j]; - if (entry.getType() == XmlRpc::XmlRpcValue::TypeDouble) { - res(i, j) = static_cast(entry); - } else if (entry.getType() == XmlRpc::XmlRpcValue::TypeInt) { - res(i, j) = static_cast(entry); - } else { - fail("param ", name, "[", i, ", ", j, "] is not numeric"); - } - } - } - return true; -} -template -bool _getParam(ros::NodeHandle &nh, const std::string &name, T &res) { - return nh.getParam(name, res); -} -template <> -bool _getParam(ros::NodeHandle &nh, const std::string &name, ros::Duration &res) { - double x; - if (!nh.getParam(name, x)) return false; - res = ros::Duration(x); - return true; -} -template <> -bool _getParam(ros::NodeHandle &nh, const std::string &name, unsigned int &res) { - int x; - if (!nh.getParam(name, x)) return false; - if (x < 0) { - fail("param ", name, " must be >= 0"); - } - res = static_cast(x); - return true; -} - -template -T getParam(ros::NodeHandle &nh, std::string name) { - T res; - require(_getParam(nh, name, res), "param ", name, " required"); - return res; -} -template -T getParam(ros::NodeHandle &nh, std::string name, T default_value) { - T res; - if (!_getParam(nh, name, res)) { - return default_value; - } - return res; -} -} - -#endif diff --git a/utils/uf_common/msg/Acceleration.msg b/utils/uf_common/msg/Acceleration.msg deleted file mode 100644 index 680a69dd..00000000 --- a/utils/uf_common/msg/Acceleration.msg +++ /dev/null @@ -1,2 +0,0 @@ -geometry_msgs/Vector3 linear -geometry_msgs/Vector3 angular diff --git a/utils/uf_common/msg/Float64Stamped.msg b/utils/uf_common/msg/Float64Stamped.msg deleted file mode 100644 index 5c2f1136..00000000 --- a/utils/uf_common/msg/Float64Stamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -float64 data diff --git a/utils/uf_common/msg/PoseTwist.msg b/utils/uf_common/msg/PoseTwist.msg deleted file mode 100644 index a66d185a..00000000 --- a/utils/uf_common/msg/PoseTwist.msg +++ /dev/null @@ -1,3 +0,0 @@ -geometry_msgs/Pose pose -geometry_msgs/Twist twist -Acceleration acceleration diff --git a/utils/uf_common/msg/PoseTwistStamped.msg b/utils/uf_common/msg/PoseTwistStamped.msg deleted file mode 100644 index 631c3523..00000000 --- a/utils/uf_common/msg/PoseTwistStamped.msg +++ /dev/null @@ -1,3 +0,0 @@ -# pose is in header.frame_id. twist is in frame defined by pose (ie. body) -Header header -PoseTwist posetwist diff --git a/utils/uf_common/msg/VelocityMeasurement.msg b/utils/uf_common/msg/VelocityMeasurement.msg deleted file mode 100644 index ba14a4bf..00000000 --- a/utils/uf_common/msg/VelocityMeasurement.msg +++ /dev/null @@ -1,3 +0,0 @@ -geometry_msgs/Vector3 direction -float64 velocity -float64 correlation diff --git a/utils/uf_common/msg/VelocityMeasurements.msg b/utils/uf_common/msg/VelocityMeasurements.msg deleted file mode 100644 index 1161df02..00000000 --- a/utils/uf_common/msg/VelocityMeasurements.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -VelocityMeasurement[] velocity_measurements diff --git a/utils/uf_common/package.xml b/utils/uf_common/package.xml deleted file mode 100644 index 5414fab8..00000000 --- a/utils/uf_common/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - uf_common - 1.0.0 - uf_common - Forrest Voight - - BSD - - http://ros.org/wiki/uf_common - Forrest Voight - catkin - - message_runtime - actionlib - interactive_markers - std_msgs - message_generation - geometry_msgs - tf - actionlib_msgs - cmake_modules - - message_runtime - actionlib - interactive_markers - std_msgs - message_generation - geometry_msgs - tf - actionlib_msgs - - - - - - diff --git a/utils/uf_common/scripts/param_saver b/utils/uf_common/scripts/param_saver deleted file mode 100755 index 95c114fa..00000000 --- a/utils/uf_common/scripts/param_saver +++ /dev/null @@ -1,40 +0,0 @@ -#!/usr/bin/env python - -import os -import random -import yaml - -import roslib -roslib.load_manifest('uf_common') -import rospy - - -rospy.init_node('param_saver', anonymous=True) - -class MyDumper(yaml.Dumper): - def represent_mapping(self, tag, mapping, flow_style=False): - return yaml.Dumper.represent_mapping(self, tag, mapping, flow_style) - -while not rospy.is_shutdown(): - rospy.sleep(rospy.Duration(3)) - - entries = rospy.get_param('~') - for entry in entries.itervalues(): - filename = entry['filename'] - file_basepath = entry['file_basepath'] - param_paths = entry['param_paths'] - - p = rospy.get_param(file_basepath) - data = yaml.dump(dict((k, v) for k, v in p.iteritems() if k in param_paths), Dumper=MyDumper) - - if os.path.exists(filename): - with open(filename, 'rb') as f: - if f.read() == data: - continue - - tmp_filename = '%s.%i' % (filename, random.randrange(10000)) - with open(tmp_filename, 'wb') as f: - f.write(data) - os.rename(tmp_filename, filename) - - print 'Saved %s to %s' % (file_basepath, filename) diff --git a/utils/uf_common/setup.py b/utils/uf_common/setup.py deleted file mode 100644 index 9ab73768..00000000 --- a/utils/uf_common/setup.py +++ /dev/null @@ -1,10 +0,0 @@ -# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -setup_args = generate_distutils_setup( - packages=['uf_common'], -) - -setup(**setup_args) diff --git a/utils/uf_common/uf_common/__init__.py b/utils/uf_common/uf_common/__init__.py deleted file mode 100644 index e69de29b..00000000