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