Merge pull request #8 from microsoft/release/0.0.3

Release 0.0.3, plumb through support for RGB images, add TF timeouts

asa_ros/include/asa_ros/asa_ros_node.h

@@ -97,6 +97,10 @@ class AsaRosNode {
   std::string camera_frame_id_;
   std::string anchor_frame_id_;
 
+  // Timeout to wait for TF messages, in seconds. 0.0 = instantaneous. 1.0 =
+  // will wait a full second on any failed attempt.
+  double tf_lookup_timeout_;
+
   // Cache of which anchors are currently being queried. This will be only used
   // when reset() (but not resetCompletely() is called, to restart any
   // previous watchers.

asa_ros/launch/asa_ros.launch

@@ -17,8 +17,12 @@
     <remap from="image" to="/camera/fisheye1_rect/image" />
     <remap from="camera_info" to="/camera/fisheye1_rect/camera_info" />
 
+    <!-- The frame ID of your world/odometry frame. Should be static relative to, well, the world. -->
     <param name="world_frame_id" value="camera_odom_frame" />
+    <!-- Optical frame of the camera, z facing "forward" (out of the camera frame). -->
     <param name="camera_frame_id" value="camera_fisheye1_optical_frame" />
+    <!-- Timeout to wait for TF lookups to finish. If you get a lot of TF errors but still smome valid lookups, try increasing this. -->
+    <param name="tf_lookup_timeout" value="0.1" />
 
     <param name="account_id" value="$(arg account_id)"/>
     <param name="account_key" value="$(arg account_key)"/>

asa_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>asa_ros</name>
-  <version>0.0.1</version>
+  <version>0.0.3</version>
   <description>ROS wrapper for the Azure Spatial Anchors Linux SDK.</description>
 
   <maintainer email="helen.oleynikova@microsoft.com">Helen Oleynikova</maintainer>

asa_ros/src/asa_interface.cpp

@@ -188,7 +188,7 @@ void AzureSpatialAnchorsInterface::addFrame(
   double cy = camera_msg.K[5];
 
   // Convert the image.
-  cv::Mat image = cv_bridge::toCvCopy(image_msg)->image;
+  cv::Mat image = cv_bridge::toCvCopy(image_msg, "mono8")->image;
 
   // Convert the pose.
   Eigen::Affine3d T_W_C = tf2::transformToEigen(T_W_C_msg);

asa_ros/src/asa_ros_node.cpp

@@ -12,10 +12,12 @@ AsaRosNode::AsaRosNode(const ros::NodeHandle& nh,
                        const ros::NodeHandle& nh_private)
     : nh_(nh),
       nh_private_(nh_private),
+      tf_buffer_(ros::Duration(120.0)),
       tf_listener_(tf_buffer_),
       world_frame_id_("world"),
       camera_frame_id_(""),
-      anchor_frame_id_("") {
+      anchor_frame_id_(""),
+      tf_lookup_timeout_(0.1) {
   initFromRosParams();
 }
 
@@ -56,6 +58,8 @@ void AsaRosNode::initFromRosParams() {
   nh_private_.param("world_frame_id", world_frame_id_, world_frame_id_);
   nh_private_.param("camera_frame_id", camera_frame_id_, camera_frame_id_);
   nh_private_.param("anchor_frame_id", anchor_frame_id_, anchor_frame_id_);
+  nh_private_.param("tf_lookup_timeout", tf_lookup_timeout_,
+                    tf_lookup_timeout_);
 
   // Load ASA config and set it up.
   AsaRosConfig asa_config;
@@ -100,7 +104,8 @@ void AsaRosNode::imageAndInfoCallback(
 
   // Look up its pose.
   if (tf_buffer_.canTransform(world_frame_id_, camera_frame_id_,
-                              image->header.stamp)) {
+                              image->header.stamp,
+                              ros::Duration(tf_lookup_timeout_))) {
     geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(
         world_frame_id_, camera_frame_id_, image->header.stamp);
 
@@ -109,7 +114,9 @@ void AsaRosNode::imageAndInfoCallback(
     ROS_INFO_ONCE("Added first frame.");
   } else {
     ROS_WARN_STREAM("Couldn't look up transform from "
-                    << world_frame_id_ << " to " << camera_frame_id_);
+                    << world_frame_id_ << " to " << camera_frame_id_
+                    << " at timestamp " << image->header.stamp
+                    << " and ros::Time::now() " << ros::Time::now());
   }
 }
 

asa_ros/src/asa_ros_provider.cpp

@@ -45,7 +45,13 @@ void AsaRosProvider::GetPixelData(const void* frame_context,
 
 size_t AsaRosProvider::addImageToQueue(const cv::Mat& image) {
   std::unique_lock<std::mutex> provider_lock(provider_mutex_);
-  image_queue_[next_queue_id_] = image;
+  if (image.channels() > 1) {
+    cv::Mat gray_image;
+    cv::cvtColor(image, gray_image, cv::COLOR_BGR2GRAY);
+    image_queue_[next_queue_id_] = gray_image;
+  } else {
+    image_queue_[next_queue_id_] = image;
+  }
 
   VLOG(3) << "Added image " << next_queue_id_ << " to queue.\n";
 

asa_ros_msgs/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>asa_ros_msgs</name>
-  <version>0.0.1</version>
+  <version>0.0.3</version>
   <description>Message definitions for the ASA ROS interface.</description>
 
   <maintainer email="helen.oleynikova@microsoft.com">Helen Oleynikova</maintainer>