Merge pull request #44 from christianrauch/fix_topics

fix launch file topics

README.md

@@ -1,6 +1,6 @@
-# AprilTag ROS2 Node
+# AprilTag ROS 2 Node
 
-This ROS2 node uses the AprilTag library to detect AprilTags in images and publish their pose, id and additional metadata.
+This ROS 2 node uses the AprilTag library to detect AprilTags in images and publish their pose, id and additional metadata.
 
 For more information on AprilTag, the paper and the reference implementation: https://april.eecs.umich.edu/software/apriltag.html
 
@@ -44,6 +44,8 @@ apriltag:                 # node name
       sharpening: 0.25    # sharpening of decoded images
       debug: 0            # write additional debugging images to current working directory
 
+    pose_estimation_method: "pnp" # method for estimating the tag pose
+
     # (optional) list of tags
     # If defined, 'frames' and 'sizes' must have the same length as 'ids'.
     tag:
@@ -81,7 +83,7 @@ apriltag_ros
   AprilTagNode
 ```
 
-This `AprilTagNode` component can be loaded with other nodes into a "container node" process where they used shared-memory communication to prevent unnecessary data copies. The example launch file [v4l2_36h11.launch.yml](launch/v4l2_36h11.launch.yml) loads the `AprilTagNode` component together with the `v4l2_camera::V4L2Camera` component from the [`v4l2_camera` package](https://gitlab.com/boldhearts/ros2_v4l2_camera) (`sudo apt install ros-$ROS_DISTRO-v4l2-camera`) into one container and enables `use_intra_process_comms` for both:
+This `AprilTagNode` component can be loaded with other nodes into a "container node" process where they used shared-memory communication to prevent unnecessary data copies. The example launch file [camera_36h11.launch.yml](launch/camera_36h11.launch.yml) loads the `AprilTagNode` component together with the `camera::CameraNode` component from the [`camera_ros` package](https://index.ros.org/p/camera_ros/) (`sudo apt install ros-$ROS_DISTRO-camera-ros`) into one container and enables `use_intra_process_comms` for both:
 ```sh
-ros2 launch apriltag_ros v4l2_36h11.launch.yml
+ros2 launch apriltag_ros camera_36h11.launch.yml
 ```

cfg/tags_36h11.yaml

@@ -14,6 +14,9 @@
             sharpening: 0.25    # sharpening of decoded images
             debug: False        # write additional debugging images to current working directory
 
+
+        pose_estimation_method: "pnp"   # method for estimating the tag pose
+
         # optional list of tags
         tag:
             ids: [9, 14]            # tag ID

launch/v4l2_36h11.launch.yml → launch/camera_36h11.launch.yml

@@ -9,24 +9,26 @@ launch:
     name: apriltag_container
     namespace: ""
     composable_node:
-    - pkg: v4l2_camera
-      plugin: v4l2_camera::V4L2Camera
+    - pkg: camera_ros
+      plugin: camera::CameraNode
       name: camera
-      namespace: v4l2
+      namespace: camera
       param:
-      - name: video_device
-        value: /dev/video$(var device)
+      - name: camera
+        value: $(var device)
       extra_arg:
       - name: use_intra_process_comms
         value: "True"
 
     - pkg: image_proc
       plugin: image_proc::RectifyNode
       name: rectify
-      namespace: v4l2
+      namespace: camera
       remap:
       - from: image
-        to: image_raw
+        to: /camera/camera/image_raw
+      - from: camera_info
+        to: /camera/camera/camera_info
       extra_arg:
       - name: use_intra_process_comms
         value: "True"
@@ -37,9 +39,9 @@ launch:
       namespace: apriltag
       remap:
       - from: /apriltag/image_rect
-        to: /v4l2/image_rect
-      - from: /apriltag/camera_info
-        to: /v4l2/camera_info
+        to: /camera/image_rect
+      - from: /camera/camera_info
+        to: /camera/camera/camera_info
       param:
       - from: $(find-pkg-share apriltag_ros)/cfg/tags_36h11.yaml
       extra_arg:

package.xml

@@ -20,6 +20,9 @@
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
 
+  <exec_depend>camera_ros</exec_depend>
+  <exec_depend>image_proc</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_cppcheck</test_depend>

src/AprilTagNode.cpp

@@ -117,7 +117,20 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
     const auto sizes = declare_parameter("tag.sizes", std::vector<double>{}, descr("tag sizes per id", true));
 
     // get method for estimating tag pose
-    estimate_pose = pose_estimation_methods.at(declare_parameter("pose_estimation_method", "pnp", descr("pose estimation method: \"pnp\" (more accurate) or \"homography\" (faster)", true)));
+    const std::string& pose_estimation_method =
+        declare_parameter("pose_estimation_method", "pnp",
+                          descr("pose estimation method: \"pnp\" (more accurate) or \"homography\" (faster), "
+                                "set to \"\" (empty) to disable pose estimation",
+                                true));
+
+    if(!pose_estimation_method.empty()) {
+        if(pose_estimation_methods.count(pose_estimation_method)) {
+            estimate_pose = pose_estimation_methods.at(pose_estimation_method);
+        }
+        else {
+            RCLCPP_ERROR_STREAM(get_logger(), "Unknown pose estimation method '" << pose_estimation_method << "'.");
+        }
+    }
 
     // detector parameters in "detector" namespace
     declare_parameter("detector.threads", td->nthreads, descr("number of threads"));
@@ -165,7 +178,15 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i
                             const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci)
 {
     // camera intrinsics for rectified images
-    const std::array<double, 4> intrinsics = {msg_ci->p.data()[0], msg_ci->p.data()[5], msg_ci->p.data()[2], msg_ci->p.data()[6]};
+    const std::array<double, 4> intrinsics = {msg_ci->p[0], msg_ci->p[5], msg_ci->p[2], msg_ci->p[6]};
+
+    // check for valid intrinsics
+    const bool calibrated = msg_ci->width && msg_ci->height &&
+                            intrinsics[0] && intrinsics[1] && intrinsics[2] && intrinsics[3];
+
+    if(estimate_pose != nullptr && !calibrated) {
+        RCLCPP_WARN_STREAM(get_logger(), "The camera is not calibrated! Set 'pose_estimation_method' to \"\" (empty) to disable pose estimation and this warning.");
+    }
 
     // convert to 8bit monochrome image
     const cv::Mat img_uint8 = cv_bridge::toCvShare(msg_img, "mono8")->image;
@@ -213,20 +234,21 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i
         msg_detections.detections.push_back(msg_detection);
 
         // 3D orientation and position
-        geometry_msgs::msg::TransformStamped tf;
-        tf.header = msg_img->header;
-        // set child frame name by generic tag name or configured tag name
-        tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id);
-        const double size = tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size;
-        if(estimate_pose != nullptr) {
+        if(estimate_pose != nullptr && calibrated) {
+            geometry_msgs::msg::TransformStamped tf;
+            tf.header = msg_img->header;
+            // set child frame name by generic tag name or configured tag name
+            tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id);
+            const double size = tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size;
             tf.transform = estimate_pose(det, intrinsics, size);
+            tfs.push_back(tf);
         }
-
-        tfs.push_back(tf);
     }
 
     pub_detections->publish(msg_detections);
-    tf_broadcaster.sendTransform(tfs);
+
+    if(estimate_pose != nullptr)
+        tf_broadcaster.sendTransform(tfs);
 
     apriltag_detections_destroy(detections);
 }