From 51680bc743e7806639dc58832d48e0e4c3fc498b Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Mon, 10 Apr 2017 23:36:28 -0400 Subject: [PATCH] MIL COMMON: replace Float64Stamped msg with DepthStamped and RangeStamped --- .../sub8_alarm/alarm_handlers/height_over_bottom.py | 6 +++--- command/sub8_launch/nodes/self_check.py | 6 +++--- command/sub8_launch/scripts/tf_republisher.py | 4 ++-- command/sub8_missions/missions/start_gate.py | 2 +- command/sub8_missions/sub8/sub_singleton.py | 8 ++++---- command/sub8_missions/tools/rviz_visualizer.py | 10 +++++----- drivers/sub8_depth_driver/scripts/fake_depth | 8 ++++---- drivers/sub8_depth_driver/src/nodelet.cpp | 8 ++++---- drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h | 8 ++++---- drivers/sub8_rdi_dvl/src/nodelet.cpp | 4 ++-- perception/sub8_perception/nodes/bins_2d.py | 6 +++--- simulation/sub8_gazebo/nodes/gazebo_controller.py | 8 ++++---- 12 files changed, 39 insertions(+), 39 deletions(-) diff --git a/command/sub8_alarm/alarm_handlers/height_over_bottom.py b/command/sub8_alarm/alarm_handlers/height_over_bottom.py index 855603f5..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 mil_msgs.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 259efc59..f209b711 100755 --- a/command/sub8_launch/nodes/self_check.py +++ b/command/sub8_launch/nodes/self_check.py @@ -148,7 +148,7 @@ def do_check(self): from nav_msgs.msg import Odometry from tf.msg import tfMessage -from mil_msgs.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 afbe4df3..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 mil_msgs.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/start_gate.py b/command/sub8_missions/missions/start_gate.py index e46a0721..fff4311c 100644 --- a/command/sub8_missions/missions/start_gate.py +++ b/command/sub8_missions/missions/start_gate.py @@ -6,7 +6,7 @@ from tf import transformations -from mil_msgs.msg import MoveToAction, PoseTwistStamped, Float64Stamped +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 diff --git a/command/sub8_missions/sub8/sub_singleton.py b/command/sub8_missions/sub8/sub_singleton.py index 5cbbfccc..37140119 100644 --- a/command/sub8_missions/sub8/sub_singleton.py +++ b/command/sub8_missions/sub8/sub_singleton.py @@ -6,7 +6,7 @@ import rospkg from tf import transformations -from mil_msgs.msg import MoveToAction, PoseTwistStamped, Float64Stamped +from mil_msgs.msg import MoveToAction, PoseTwistStamped, RangeStamped from sub8 import pose_editor import mil_ros_tools from sub8_msgs.srv import VisionRequest, VisionRequestRequest, VisionRequest2DRequest, VisionRequest2D @@ -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') @@ -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 9794e038..b8995104 100755 --- a/command/sub8_missions/tools/rviz_visualizer.py +++ b/command/sub8_missions/tools/rviz_visualizer.py @@ -9,7 +9,7 @@ from geometry_msgs.msg import Pose, Vector3 from std_msgs.msg import ColorRGBA, Float64 -from mil_msgs.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 mil_ros_tools as sub8_utils @@ -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, @@ -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: diff --git a/drivers/sub8_depth_driver/scripts/fake_depth b/drivers/sub8_depth_driver/scripts/fake_depth index 32c88b00..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 mil_msgs.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 7f57743e..4560a79c 100644 --- a/drivers/sub8_depth_driver/src/nodelet.cpp +++ b/drivers/sub8_depth_driver/src/nodelet.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include @@ -27,7 +27,7 @@ class Nodelet : public nodelet::Nodelet { 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) { - mil_msgs::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/include/rdi_explorer_dvl/driver.h b/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h index ec754171..18981cea 100644 --- a/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h +++ b/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h @@ -12,7 +12,7 @@ #include #include -#include +#include namespace rdi_explorer_dvl { static uint16_t getu16le(uint8_t *i) { return *i | (*(i + 1) << 8); } @@ -68,7 +68,7 @@ class Device { } void read(boost::optional &res, - boost::optional &height_res) { + boost::optional &height_res) { res = boost::none; height_res = boost::none; @@ -150,9 +150,9 @@ class Device { ROS_ERROR("DVL didn't return height over bottom"); continue; } - height_res = boost::make_optional(mil_msgs::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/src/nodelet.cpp b/drivers/sub8_rdi_dvl/src/nodelet.cpp index eea3a2ac..f3256e97 100644 --- a/drivers/sub8_rdi_dvl/src/nodelet.cpp +++ b/drivers/sub8_rdi_dvl/src/nodelet.cpp @@ -30,7 +30,7 @@ class Nodelet : public nodelet::Nodelet { frame_id = mil_tools::getParam(getPrivateNodeHandle(), "frame_id"); pub = getNodeHandle().advertise("dvl", 10); - range_pub = getNodeHandle().advertise("dvl/range", 10); + range_pub = getNodeHandle().advertise("dvl/range", 10); device = boost::make_shared(port, baudrate); heartbeat_timer = getNodeHandle().createTimer( @@ -45,7 +45,7 @@ class Nodelet : public nodelet::Nodelet { void polling_thread() { while (running) { boost::optional msg; - boost::optional range_msg; + boost::optional range_msg; device->read(msg, range_msg); if (msg) { msg->header.frame_id = frame_id; diff --git a/perception/sub8_perception/nodes/bins_2d.py b/perception/sub8_perception/nodes/bins_2d.py index 99dcf1e5..43b67b2a 100755 --- a/perception/sub8_perception/nodes/bins_2d.py +++ b/perception/sub8_perception/nodes/bins_2d.py @@ -8,7 +8,7 @@ from sub8_msgs.srv import VisionRequest2DResponse, VisionRequest2D from std_msgs.msg import Header from geometry_msgs.msg import Pose2D -from mil_msgs.msg import Float64Stamped # This needs to be deprecated +from mil_msgs.msg import RangeStamped def contour_sort(l): @@ -57,7 +57,7 @@ def __init__(self): 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) @@ -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/simulation/sub8_gazebo/nodes/gazebo_controller.py b/simulation/sub8_gazebo/nodes/gazebo_controller.py index 6164643b..8f408a2d 100755 --- a/simulation/sub8_gazebo/nodes/gazebo_controller.py +++ b/simulation/sub8_gazebo/nodes/gazebo_controller.py @@ -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 mil_msgs.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( + RangeStamped( header=sub8_utils.make_header(), - data=float(np.mean(msg.ranges)) + range=float(np.mean(msg.ranges)) ) )