diff --git a/asa_ros/include/asa_ros/asa_ros_node.h b/asa_ros/include/asa_ros/asa_ros_node.h
index a34540b..84fa53f 100644
--- a/asa_ros/include/asa_ros/asa_ros_node.h
+++ b/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.
diff --git a/asa_ros/launch/asa_ros.launch b/asa_ros/launch/asa_ros.launch
index de6c4a9..e533cd5 100644
--- a/asa_ros/launch/asa_ros.launch
+++ b/asa_ros/launch/asa_ros.launch
@@ -17,8 +17,12 @@
+
+
+
+
diff --git a/asa_ros/package.xml b/asa_ros/package.xml
index c92c2de..296e781 100644
--- a/asa_ros/package.xml
+++ b/asa_ros/package.xml
@@ -1,7 +1,7 @@
asa_ros
- 0.0.1
+ 0.0.3
ROS wrapper for the Azure Spatial Anchors Linux SDK.
Helen Oleynikova
diff --git a/asa_ros/src/asa_interface.cpp b/asa_ros/src/asa_interface.cpp
index d4851fa..9a91bea 100644
--- a/asa_ros/src/asa_interface.cpp
+++ b/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);
diff --git a/asa_ros/src/asa_ros_node.cpp b/asa_ros/src/asa_ros_node.cpp
index 795875e..3f55b53 100644
--- a/asa_ros/src/asa_ros_node.cpp
+++ b/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());
}
}
diff --git a/asa_ros/src/asa_ros_provider.cpp b/asa_ros/src/asa_ros_provider.cpp
index bea10b5..43584d3 100644
--- a/asa_ros/src/asa_ros_provider.cpp
+++ b/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 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";
diff --git a/asa_ros_msgs/package.xml b/asa_ros_msgs/package.xml
index 3dc93a7..a4c0758 100644
--- a/asa_ros_msgs/package.xml
+++ b/asa_ros_msgs/package.xml
@@ -1,7 +1,7 @@
asa_ros_msgs
- 0.0.1
+ 0.0.3
Message definitions for the ASA ROS interface.
Helen Oleynikova