diff --git a/command/sub8_launch/config/torpedo_board.yaml b/command/sub8_launch/config/torpedo_board.yaml index 00815ba3..02622bfc 100644 --- a/command/sub8_launch/config/torpedo_board.yaml +++ b/command/sub8_launch/config/torpedo_board.yaml @@ -2,8 +2,8 @@ # if these parameters aren't set, the defaults in torpedo_board.cc will be used # input -input_left : "/stereo/left/image_rect_color" -input_right : "/stereo/right/image_rect_color" +input_left : "/camera/front/left/image_rect_color" +input_right : "/camera/front/right/image_rect_color" activation : "/torpedo_board/detection_activation_switch" frame_height : 644 frame_width : 482 @@ -19,4 +19,4 @@ diffusion_time : 20 max_features : 20 feature_block_size : 11 feature_min_distance : 20.0 -generate_dbg_imgs : false \ No newline at end of file +generate_dbg_imgs : false diff --git a/command/sub8_launch/launch/tf.launch b/command/sub8_launch/launch/tf.launch index b9ab1938..5b1ff291 100644 --- a/command/sub8_launch/launch/tf.launch +++ b/command/sub8_launch/launch/tf.launch @@ -1,14 +1,14 @@ - - - diff --git a/command/sub8_launch/nodes/self_check.py b/command/sub8_launch/nodes/self_check.py index f1702a2e..c587be96 100755 --- a/command/sub8_launch/nodes/self_check.py +++ b/command/sub8_launch/nodes/self_check.py @@ -118,12 +118,12 @@ def get_all_thrusters(self): class CameraChecker(TemplateChecker): @txros.util.cancellableInlineCallbacks def tx_init(self): - self.right = self.nh.subscribe("/stereo/right/image_rect_color", Image) - self.right_info = self.nh.subscribe("/stereo/right/camera_info", CameraInfo) - self.left = self.nh.subscribe("/stereo/left/image_rect_color", Image) - self.left_info = self.nh.subscribe("/stereo/left/camera_info", CameraInfo) - self.down = self.nh.subscribe("/down/left/image_rect_color", Image) # TODO - self.down_info = self.nh.subscribe("/down/left/camera_info", CameraInfo) # TODO + self.right = self.nh.subscribe("/camera/front/right/image_rect_color", Image) + self.right_info = self.nh.subscribe("/camera/front/right/camera_info", CameraInfo) + self.left = self.nh.subscribe("/camera/front/left/image_rect_color", Image) + self.left_info = self.nh.subscribe("/camera/front/left/camera_info", CameraInfo) + self.down = self.nh.subscribe("/camera/down/left/image_rect_color", Image) # TODO + self.down_info = self.nh.subscribe("/camera/down/left/camera_info", CameraInfo) # TODO self.subs = [("Right Image", self.right.get_next_message()), ("Right Info", self.right_info.get_next_message()), ("Left Image", self.left.get_next_message()), ("Left Info", self.left_info.get_next_message()), diff --git a/perception/sub8_perception/include/sub8_perception/buoy.hpp b/perception/sub8_perception/include/sub8_perception/buoy.hpp index 9a5d8f5f..7fe0a9bb 100644 --- a/perception/sub8_perception/include/sub8_perception/buoy.hpp +++ b/perception/sub8_perception/include/sub8_perception/buoy.hpp @@ -35,7 +35,7 @@ #define BACKWARD_HAS_BFD 1 #include -// ROS_NAMESPACE=/stereo/left rosrun image_proc image_proc +// ROS_NAMESPACE=/camera/front/left rosrun image_proc image_proc // rosbag play ./holding_buoy_mil.bag -r 0.1 // [1] // http://docs.ros.org/hydro/api/image_geometry/html/c++/classimage__geometry_1_1PinholeCameraModel.html @@ -93,4 +93,4 @@ class Sub8BuoyDetector { sensor_msgs::ImageConstPtr last_image_msg; bool got_cloud, got_image, line_added, computing, need_new_cloud; -}; \ No newline at end of file +}; diff --git a/perception/sub8_perception/nodes/buoy_2d.py b/perception/sub8_perception/nodes/buoy_2d.py index fe90c9c5..0cc2b700 100755 --- a/perception/sub8_perception/nodes/buoy_2d.py +++ b/perception/sub8_perception/nodes/buoy_2d.py @@ -111,7 +111,7 @@ def __init__(self): self.buoys[color] = Buoy(color, boosting) self.buoys[color].load_segmentation() - self.image_sub = sub8_ros_tools.Image_Subscriber('/stereo/left/image_raw', self.image_cb) + self.image_sub = sub8_ros_tools.Image_Subscriber('/camera/front/left/image_raw', self.image_cb) self.image_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/target_info') self.mask_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/mask') @@ -142,7 +142,7 @@ def request_buoy(self, srv): if response is False: print 'did not find' resp = VisionRequest2DResponse( - header=sub8_ros_tools.make_header(frame='/stereo_front/left'), + header=sub8_ros_tools.make_header(frame='front_left_cam'), found=False ) @@ -150,7 +150,7 @@ def request_buoy(self, srv): # Fill in center, radius = response resp = VisionRequest2DResponse( - header=Header(stamp=timestamp, frame_id='/stereo_front/left'), + header=Header(stamp=timestamp, frame_id='front_left_cam'), pose=Pose2D( x=center[0], y=center[1], @@ -295,11 +295,11 @@ def find_single_buoy(self, img, timestamp, buoy_type): if not self.sanity_check(tuple_center, timestamp): return False - (t, rot_q) = self.transformer.lookupTransform('/map', '/stereo_front/left', timestamp) + (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) - self.rviz.draw_ray_3d(tuple_center, self.camera_model, buoy.draw_colors, frame='/stereo_front/left', + self.rviz.draw_ray_3d(tuple_center, self.camera_model, buoy.draw_colors, frame='/front_left_cam', _id=self._id + 100, timestamp=timestamp) self._id += 1 if self._id >= self.max_observations * 3: @@ -353,7 +353,7 @@ def sanity_check(self, coordinate, timestamp): Check if the observation is unreasonable. More can go here if we want. ''' sane = True - if np.linalg.norm(self.transformer.lookupTwist('/map', '/stereo_front/left', timestamp, rospy.Duration(.5))) > 1: + if np.linalg.norm(self.transformer.lookupTwist('/map', '/front_left_cam', timestamp, rospy.Duration(.5))) > 1: rospy.logerr("BUOY - Moving too fast. Not observing buoy.") sane = False diff --git a/perception/sub8_perception/nodes/ocr_server.py b/perception/sub8_perception/nodes/ocr_server.py index 959ed57b..408c5b3c 100755 --- a/perception/sub8_perception/nodes/ocr_server.py +++ b/perception/sub8_perception/nodes/ocr_server.py @@ -23,7 +23,7 @@ def __init__(self): self.camera_model = None self.white_list = None self.ocr_service = rospy.Service('vision/ocr', OcrRequest, self.request_characters) - self.image_sub = sub8_ros_tools.Image_Subscriber('/stereo_front/right/image_rect_color', self.image_cb) + self.image_sub = sub8_ros_tools.Image_Subscriber('/camera/front/right/image_rect_color', self.image_cb) def request_characters(self, srv): self.white_list = srv.target_name diff --git a/perception/sub8_perception/nodes/torpedo_board.cpp b/perception/sub8_perception/nodes/torpedo_board.cpp index 981279b2..b309f4fc 100644 --- a/perception/sub8_perception/nodes/torpedo_board.cpp +++ b/perception/sub8_perception/nodes/torpedo_board.cpp @@ -17,8 +17,8 @@ Sub8TorpedoBoardDetector::Sub8TorpedoBoardDetector() int tab_sz = 4; // Default parameters - string img_topic_left_default = "/stereo/left/image_rect_color/"; - string img_topic_right_default = "/stereo/right/image_rect_color/"; + string img_topic_left_default = "/camera/front/left/image_rect_color/"; + string img_topic_right_default = "/camera/front/right/image_rect_color/"; string activation_default = "/torpedo_board/detection_activation_switch"; string dbg_topic_default = "/torpedo_board/dbg_imgs"; string pose_est_srv_default = "/torpedo_board/pose_est_srv"; diff --git a/perception/sub8_perception/nodes/torpedo_board_pose_est.py b/perception/sub8_perception/nodes/torpedo_board_pose_est.py index 5419b089..4ae6bb9d 100755 --- a/perception/sub8_perception/nodes/torpedo_board_pose_est.py +++ b/perception/sub8_perception/nodes/torpedo_board_pose_est.py @@ -84,8 +84,8 @@ def minimize_reprojection_error(self): if self.minimization_result.success: if not rospy.is_shutdown(): try: - self.tf_listener.waitForTransform('/map', 'stereo_front', self.current_req.stamp, rospy.Duration(0.1)) - (trans, rot) = self.tf_listener.lookupTransform('/map', 'stereo_front', self.current_req.stamp) + self.tf_listener.waitForTransform('/map', 'front_stereo', self.current_req.stamp, rospy.Duration(0.1)) + (trans, rot) = self.tf_listener.lookupTransform('/map', 'front_stereo', self.current_req.stamp) yaw = tf.transformations.euler_from_quaternion(rot, 'syxz')[0] pose = np.array([trans.x, trans.y, trans.z, yaw]) self.pose_obs.push_back(pose) diff --git a/perception/sub8_perception/readme.md b/perception/sub8_perception/readme.md index dc364e4c..b3737618 100644 --- a/perception/sub8_perception/readme.md +++ b/perception/sub8_perception/readme.md @@ -56,7 +56,7 @@ Add a "Marker" visualizer, and set its topic to `/visualization/buoys`. Buoy det If you are using a bag ```shell - ROS_NAMESPACE=/stereo/left rosrun image_proc image_proc + ROS_NAMESPACE=/camera/front/left rosrun image_proc image_proc rosbag play ./holding_buoy_mil.bag ``` diff --git a/perception/sub8_perception/src/sub8_perception/buoy_ros.cpp b/perception/sub8_perception/src/sub8_perception/buoy_ros.cpp index 3b94d7b5..a114772f 100644 --- a/perception/sub8_perception/src/sub8_perception/buoy_ros.cpp +++ b/perception/sub8_perception/src/sub8_perception/buoy_ros.cpp @@ -20,7 +20,7 @@ Sub8BuoyDetector::Sub8BuoyDetector() } compute_timer = nh.createTimer(ros::Duration(0.09), &Sub8BuoyDetector::compute_loop, this); - image_sub = image_transport.subscribeCamera("stereo/right/image_rect_color", 1, + image_sub = image_transport.subscribeCamera("/camera/front/right/image_rect_color", 1, &Sub8BuoyDetector::image_callback, this); image_pub = image_transport.advertise("vision/buoy/target_info", 1); @@ -37,7 +37,7 @@ Sub8BuoyDetector::Sub8BuoyDetector() // Not yet able to build with C++11, should be done with an initialized vector - data_sub = nh.subscribe("/stereo/points2", 1, &Sub8BuoyDetector::cloud_callback, this); + data_sub = nh.subscribe("/camera/front/points2", 1, &Sub8BuoyDetector::cloud_callback, this); service_3d = nh.advertiseService("/vision/buoy/pose", &Sub8BuoyDetector::request_buoy_position, this); pcl::console::print_highlight("--PCL Sub8BuoyDetector Initialized\n"); diff --git a/perception/sub8_perception/src/sub8_perception/start_gate.cpp b/perception/sub8_perception/src/sub8_perception/start_gate.cpp index d641adca..a77b35b9 100644 --- a/perception/sub8_perception/src/sub8_perception/start_gate.cpp +++ b/perception/sub8_perception/src/sub8_perception/start_gate.cpp @@ -2,7 +2,7 @@ Sub8StartGateDetector::Sub8StartGateDetector() : image_transport_(nh_), gate_line_buffer_(30), running_(true) { image_sub_ = - image_transport_.subscribeCamera("/stereo/right/image_rect_color", 1, &Sub8StartGateDetector::imageCallback, this); + image_transport_.subscribeCamera("/camera/front/right/image_rect_color", 1, &Sub8StartGateDetector::imageCallback, this); service_2d_ = nh_.advertiseService("/vision/start_gate/pose", &Sub8StartGateDetector::requestStartGatePosition2d, this); service_enable_ = @@ -214,4 +214,4 @@ bool Sub8StartGateDetector::requestStartGateDistance(sub8_msgs::BMatrix::Request { resp.B = std::vector{getGateDistance()}; return true; -} \ No newline at end of file +} 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 4922a3e1..c038a7e7 100644 --- a/perception/sub8_perception/src/sub8_vision_lib/object_finder.cpp +++ b/perception/sub8_perception/src/sub8_vision_lib/object_finder.cpp @@ -12,8 +12,8 @@ Sub8ObjectFinder::Sub8ObjectFinder() int tab_sz = 4; // Default parameters - std::string img_topic_left_default = "/stereo/left/image_rect_color/"; - std::string img_topic_right_default = "/stereo/right/image_rect_color/"; + std::string img_topic_left_default = "/camera/front/left/image_rect_color/"; + std::string img_topic_right_default = "/camera/front/right/image_rect_color/"; std::string activation_default = "/torpedo_board/detection_activation_switch"; std::string dbg_topic_default = "/torpedo_board/dbg_imgs"; float image_proc_scale_default = 0.5; diff --git a/perception/sub8_perception/sub8_vision_tools/estimation.py b/perception/sub8_perception/sub8_vision_tools/estimation.py index 2553b4ed..ab16ea73 100644 --- a/perception/sub8_perception/sub8_vision_tools/estimation.py +++ b/perception/sub8_perception/sub8_vision_tools/estimation.py @@ -288,7 +288,7 @@ def main(): import time import rospy rospy.init_node('test_estimation') - q = sub8_ros_tools.Image_Subscriber('/stereo/left/image_rect_color') + q = sub8_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_live.py b/perception/sub8_perception/sub8_vision_tools/machine_learning/playback_live.py index 45e7e7ad..371cd42a 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 @@ -28,7 +28,7 @@ def process_image(img, image_pub, clf): parser = argparse.ArgumentParser(usage="", description="") parser.add_argument(dest='classifer', type=str, help="Name of the classifer to use.") - parser.add_argument(dest='topic', type=str, help="Topic to listen to for image callbacks", default="/stereo/left/image_rect_color") + parser.add_argument(dest='topic', type=str, help="Topic to listen to for image callbacks", default="/camera/front/left/image_rect_color") args = parser.parse_args(sys.argv[1:]) diff --git a/perception/sub8_perception/sub8_vision_tools/multi_observation.py b/perception/sub8_perception/sub8_vision_tools/multi_observation.py index ee747daa..948be752 100644 --- a/perception/sub8_perception/sub8_vision_tools/multi_observation.py +++ b/perception/sub8_perception/sub8_vision_tools/multi_observation.py @@ -104,7 +104,7 @@ def test(): from mayavi import mlab rospy.init_node('test_estimation') - q = sub8_ros_tools.Image_Subscriber('/stereo/left/image_raw') + q = sub8_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 3a830892..d99e457f 100644 --- a/perception/sub8_perception/sub8_vision_tools/rviz.py +++ b/perception/sub8_perception/sub8_vision_tools/rviz.py @@ -16,7 +16,7 @@ class RvizVisualizer(object): def __init__(self): self.rviz_pub = rospy.Publisher("visualization/projection", visualization_msgs.Marker, queue_size=3) - def draw_sphere(self, position, color, scaling=(0.11, 0.11, 0.11), _id=4, frame='/stereo_front'): + 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]) @@ -35,7 +35,7 @@ def draw_sphere(self, position, color, scaling=(0.11, 0.11, 0.11), _id=4, frame= ) self.rviz_pub.publish(marker) - def draw_ray_3d(self, pix_coords, camera_model, color, frame='/stereo_front', _id=100, length=35, timestamp=None): + def draw_ray_3d(self, pix_coords, camera_model, color, frame='/front_stereo', _id=100, length=35, timestamp=None): '''Handle range data grabbed from dvl''' # future: should be /base_link/dvl, no? marker = self.make_ray( diff --git a/simulation/sub8_gazebo/models/sub8/sub8.sdf b/simulation/sub8_gazebo/models/sub8/sub8.sdf index a5ea3cb9..db95af0d 100644 --- a/simulation/sub8_gazebo/models/sub8/sub8.sdf +++ b/simulation/sub8_gazebo/models/sub8/sub8.sdf @@ -56,7 +56,7 @@ true 15 - stereo/left + front/left /camera/front/left/image_raw /camera/front/left/camera_info front_left_cam @@ -89,7 +89,7 @@ true 15 - stereo/right + front/right /camera/front/right/image_raw /camera/front/right/camera_info front_right_cam