Merge pull request #217 from ironmig/camera-selfcheck

CAMERAS: remove all references to old camera topics/frames

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
+generate_dbg_imgs     :  false

command/sub8_launch/launch/tf.launch

@@ -1,14 +1,14 @@
 <launch>
   <!-- This is not exactly right, because of weirdness in the K-matrix from camera_info for the stereo cams -->
-    <node pkg="tf" type="static_transform_publisher" name="stereo_front_broadcaster"
+    <node pkg="tf" type="static_transform_publisher" name="front_cams_broadcaster"
       args="0.2559 0 0.1707   0.5 -0.5  0.5  -0.5 base_link front_stereo 100">
     </node>
 
-    <node pkg="tf" type="static_transform_publisher" name="stereo_front_left_broadcaster"
+    <node pkg="tf" type="static_transform_publisher" name="front_left_cam_broadcaster"
       args="-0.0445 0 0   0 0 0 1 front_stereo front_left_cam 100">
     </node>
 
-    <node pkg="tf" type="static_transform_publisher" name="stereo_front_right_broadcaster"
+    <node pkg="tf" type="static_transform_publisher" name="front_right_cam_broadcaster"
       args="0.0445 0 0   0 0 0 1 front_stereo front_right_cam 100">
     </node>
 

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()),

perception/sub8_perception/include/sub8_perception/buoy.hpp

@@ -35,7 +35,7 @@
 #define BACKWARD_HAS_BFD 1
 #include <sub8_build_tools/backward.hpp>
 
-// 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;
-};
+};

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,15 +142,15 @@ 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
             )
 
         else:
             # 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
 

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

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";

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)

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
 ```
 

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");

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<double>{getGateDistance()};
   return true;
-}
+}

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;

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)
 

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:])
 

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)
 

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(

simulation/sub8_gazebo/models/sub8/sub8.sdf

@@ -56,7 +56,7 @@
         <plugin name="left_camera_controller" filename="libgazebo_ros_camera.so">
           <alwaysOn>true</alwaysOn>
           <updateRate>15</updateRate>
-          <cameraName>stereo/left</cameraName>
+          <cameraName>front/left</cameraName>
           <imageTopicName>/camera/front/left/image_raw</imageTopicName>
           <cameraInfoTopicName>/camera/front/left/camera_info</cameraInfoTopicName>
           <frameName>front_left_cam</frameName>
@@ -89,7 +89,7 @@
         <plugin name="right_camera_controller" filename="libgazebo_ros_camera.so">
           <alwaysOn>true</alwaysOn>
           <updateRate>15</updateRate>
-          <cameraName>stereo/right</cameraName>
+          <cameraName>front/right</cameraName>
           <imageTopicName>/camera/front/right/image_raw</imageTopicName>
           <cameraInfoTopicName>/camera/front/right/camera_info</cameraInfoTopicName>
           <frameName>front_right_cam</frameName>