From bbec1a2a5ee583b92093809a074d3a8e0783a91c Mon Sep 17 00:00:00 2001 From: Furushchev Date: Thu, 3 Jan 2019 14:38:53 +0900 Subject: [PATCH] posedetection_msgs: add object_detection_aggregator --- posedetection_msgs/CMakeLists.txt | 33 ++- posedetection_msgs/package.xml | 24 +- .../src/object_detection_aggregator.cpp | 235 ++++++++++++++++++ 3 files changed, 273 insertions(+), 19 deletions(-) create mode 100644 posedetection_msgs/src/object_detection_aggregator.cpp diff --git a/posedetection_msgs/CMakeLists.txt b/posedetection_msgs/CMakeLists.txt index a48e0ba..c66589a 100644 --- a/posedetection_msgs/CMakeLists.txt +++ b/posedetection_msgs/CMakeLists.txt @@ -1,11 +1,25 @@ -# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html cmake_minimum_required(VERSION 2.8.3) project(posedetection_msgs) -find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs geometry_msgs cv_bridge message_generation message_filters) +find_package(catkin REQUIRED COMPONENTS + cv_bridge + geometry_msgs + message_generation + message_filters + roscpp + std_msgs + sensor_msgs + tf2_geometry_msgs + tf2_ros +) + +find_package(OpenCV REQUIRED) -include_directories(SYSTEM include - ${catkin_INCLUDE_DIRS}) +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) add_message_files(FILES Curve1D.msg @@ -24,27 +38,28 @@ add_service_files(FILES TargetObj.srv ) -# to compatible with fuerte directories generate_messages( DEPENDENCIES std_msgs sensor_msgs geometry_msgs ) catkin_package( CATKIN_DEPENDS roscpp std_msgs sensor_msgs geometry_msgs message_runtime - DEPENDS INCLUDE_DIRS include - LIBRARIES # TODO ) -find_package(OpenCV) add_executable(feature0d_view src/feature0d_view.cpp) target_link_libraries(feature0d_view ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(feature0d_view posedetection_msgs_gencpp) + add_executable(feature0d_to_image src/feature0d_to_image.cpp) target_link_libraries(feature0d_to_image ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(feature0d_to_image posedetection_msgs_gencpp) -install(TARGETS feature0d_view feature0d_to_image +add_executable(object_detection_aggregator src/object_detection_aggregator.cpp) +target_link_libraries(object_detection_aggregator ${catkin_LIBRARIES}) +add_dependencies(object_detection_aggregator posedetection_msgs_gencpp) + +install(TARGETS feature0d_view feature0d_to_image object_detection_aggregator RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/posedetection_msgs/package.xml b/posedetection_msgs/package.xml index 4b401ff..ec7a622 100644 --- a/posedetection_msgs/package.xml +++ b/posedetection_msgs/package.xml @@ -14,20 +14,24 @@ catkin - roscpp - std_msgs - sensor_msgs - geometry_msgs cv_bridge - message_generation + geometry_msgs message_filters + message_generation + roscpp + sensor_msgs + std_msgs + tf2_geometry_msgs + tf2_ros - roscpp - std_msgs - sensor_msgs - geometry_msgs cv_bridge - message_runtime + geometry_msgs message_filters + message_runtime + roscpp + sensor_msgs + std_msgs + tf2_geometry_msgs + tf2_ros diff --git a/posedetection_msgs/src/object_detection_aggregator.cpp b/posedetection_msgs/src/object_detection_aggregator.cpp new file mode 100644 index 0000000..9279319 --- /dev/null +++ b/posedetection_msgs/src/object_detection_aggregator.cpp @@ -0,0 +1,235 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, JSK Robotics Lab. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* + * object_detection_aggregator.cpp + * Author: Yuki Furuta + */ + +#include +#include +#include +#include +#include + +#include +#include + +namespace posedetection_msgs { + + bool compareMessageByStamp(const posedetection_msgs::ObjectDetection& a, + const posedetection_msgs::ObjectDetection& b) { + return a.header.stamp < b.header.stamp; + } + + std::string validateFrameId(const std::string& frame_id) { + for (size_t i = 0; i < frame_id.length(); ++i) { + if (frame_id[i] != '/') { + return frame_id.substr(i, frame_id.length() - i); + } + } + return frame_id; + } + + class ObjectDetectionAggregator { + public: + ObjectDetectionAggregator() + : nh_("") + , pnh_("~") + , subscribed_(false) { + // initialize parameters + pnh_.param("always_subscribe", always_subscribe_, false); + pnh_.param("pub_rate", pub_rate_, 1.0); + pnh_.param("queue_size", queue_size_, 10); + std::string target_frame_id; + pnh_.param("target_frame_id", target_frame_id, std::string()); + target_frame_id_ = validateFrameId(target_frame_id); + if (!target_frame_id_.empty()) { + ROS_INFO("Target frame id is set: %s", target_frame_id_.c_str()); + } + + // initialize transform listener + tf_buffer_client_ = boost::make_shared("/tf2_buffer_server"); + if (!tf_buffer_client_->waitForServer(ros::Duration(5.0))) { + ROS_WARN("/tf2_buffer_server not found."); + tf_buffer_client_.reset(); + tf_buffer_ = boost::make_shared(); + tf2_ros::TransformListener tf_listener(*tf_buffer_); + } + + // advertise aggregated topic + ros::SubscriberStatusCallback connect_cb + = boost::bind(&ObjectDetectionAggregator::connectionCallback, this, _1); + pub_ = nh_.advertise( + "ObjectDetection_agg", 1, + connect_cb, connect_cb, + ros::VoidConstPtr(), /* latch= */false); + + // start publish timer + timer_ = nh_.createTimer(ros::Duration(1.0 / pub_rate_), + &ObjectDetectionAggregator::periodicCallback, this); + } + + void connectionCallback(const ros::SingleSubscriberPublisher&) { + boost::mutex::scoped_lock lock(mutex_); + + if (always_subscribe_) { + if (!subscribed_) { + subscribe(); + subscribed_ = true; + } + } else { + if (!subscribed_ && pub_.getNumSubscribers() > 0) { + subscribe(); + subscribed_ = true; + } else if (subscribed_ && pub_.getNumSubscribers() == 0) { + unsubscribe(); + subscribed_ = false; + } + } + } + + void subscribe() { + sub_ = nh_.subscribe("ObjectDetection", queue_size_, + &ObjectDetectionAggregator::messageCallback, this); + } + + void unsubscribe() { + sub_.shutdown(); + } + + void messageCallback(const posedetection_msgs::ObjectDetection::ConstPtr& msg) { + boost::mutex::scoped_lock lock(mutex_); + + // if ~target_frame_id param is empty, + // use the header.frame_id of the first message as target frame. + std::string frame_id = validateFrameId(msg->header.frame_id); + if (target_frame_id_.empty()) { + target_frame_id_ = frame_id; + ROS_INFO("Target frame id is set: %s", target_frame_id_.c_str()); + } + + // transform pose frame to target_frame_id_ + if (target_frame_id_ != frame_id) { + posedetection_msgs::ObjectDetection transformed_msg; + transformed_msg.header = msg->header; + transformed_msg.header.frame_id = target_frame_id_; + transformed_msg.objects.resize(msg->objects.size()); + try { + geometry_msgs::PoseStamped pose_in, pose_out; + pose_in.header = msg->header; + pose_in.header.frame_id = frame_id; + for (size_t i = 0; i < msg->objects.size(); ++i) { + pose_in.pose = msg->objects[i].pose; + if (tf_buffer_client_) { + tf_buffer_client_->transform(pose_in, pose_out, target_frame_id_); + } else { + tf_buffer_->transform(pose_in, pose_out, target_frame_id_); + } + posedetection_msgs::Object6DPose& pose6d = transformed_msg.objects[i]; + pose6d.pose = pose_out.pose; + pose6d.reliability = msg->objects[i].reliability; + pose6d.type = msg->objects[i].type; + } + messages_.push_back(transformed_msg); + } catch (tf2::TransformException &err) { + ROS_ERROR_THROTTLE(1.0, "Failed to transform: %s", err.what()); + } + } else { + messages_.push_back(*msg); + } + } + + void periodicCallback(const ros::TimerEvent& ev) { + boost::mutex::scoped_lock lock(mutex_); + + if (messages_.empty()) return; + + // sort by stamp + std::sort(messages_.begin(), messages_.end(), compareMessageByStamp); + + // aggregate detections + // - use the latest most reliable pose for each type + posedetection_msgs::ObjectDetection pub_msg; + pub_msg.header = messages_[messages_.size()-1].header; + std::map m; + for (size_t i = 0; i < messages_.size(); ++i) { + posedetection_msgs::ObjectDetection& d = messages_[i]; + for (size_t j = 0; j < d.objects.size(); ++j) { + posedetection_msgs::Object6DPose& p = d.objects[j]; + if (!m.count(p.type) || + m[p.type].reliability <= p.reliability) { + m[p.type] = p; + } + } + } + + for (std::map::iterator it = m.begin(); + it != m.end(); ++it) { + pub_msg.objects.push_back(it->second); + } + + pub_.publish(pub_msg); + messages_.clear(); + } + + // variables + boost::mutex mutex_; + ros::NodeHandle nh_, pnh_; + ros::Publisher pub_; + ros::Subscriber sub_; + ros::Timer timer_; + boost::shared_ptr tf_buffer_client_; + boost::shared_ptr tf_buffer_; + bool subscribed_; + bool always_subscribe_; + double pub_rate_; + int queue_size_; + std::string target_frame_id_; + std::vector messages_; + + }; // class +} // namespace + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "object_detection_aggregator"); + + posedetection_msgs::ObjectDetectionAggregator agg; + + ros::spin(); + + return 0; +}