bounding_box_marker.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ryohei Ueda and JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <interactive_markers/interactive_marker_server.h>
00036 #include <jsk_recognition_msgs/Int32Stamped.h>
00037 #include <jsk_interactive_marker/IndexRequest.h>
00038 #include <jsk_recognition_msgs/BoundingBoxArray.h>
00039 #include <ros/ros.h>
00040 #include <boost/algorithm/string.hpp>
00041 
00042 
00043 std::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
00044 boost::mutex mutex;
00045 ros::Publisher pub, box_pub, box_arr_pub;
00046 jsk_recognition_msgs::BoundingBoxArray::ConstPtr box_msg;
00047 
00048 void publishClickedBox(jsk_recognition_msgs::Int32Stamped& msg)
00049 {
00050   pub.publish(msg);
00051   box_pub.publish(box_msg->boxes[msg.data]);
00052   jsk_recognition_msgs::BoundingBoxArray array_msg;
00053   array_msg.header = box_msg->header;
00054   array_msg.boxes.push_back(box_msg->boxes[msg.data]);
00055   box_arr_pub.publish(array_msg);
00056 }
00057 
00058 void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00059 {
00060   boost::mutex::scoped_lock(mutex);
00061   // control_name is "sec nsec index"
00062   if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN) {
00063     std::string control_name = feedback->control_name;
00064     ROS_INFO_STREAM("control_name: " << control_name);
00065     std::list<std::string> splitted_string;
00066     boost::split(splitted_string, control_name, boost::is_space());
00067     jsk_recognition_msgs::Int32Stamped index;
00068     index.header.stamp.sec = boost::lexical_cast<int>(splitted_string.front());
00069     splitted_string.pop_front();
00070     index.header.stamp.nsec = boost::lexical_cast<int>(splitted_string.front());
00071     splitted_string.pop_front();
00072     index.data = boost::lexical_cast<int>(splitted_string.front());
00073     publishClickedBox(index);
00074   }
00075 }
00076 
00077 bool indexRequest(jsk_interactive_marker::IndexRequest::Request  &req,
00078                   jsk_interactive_marker::IndexRequest::Response &res)
00079 {
00080   publishClickedBox(req.index);
00081 }
00082 
00083 void boxCallback(const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg)
00084 {
00085   boost::mutex::scoped_lock(mutex);
00086   box_msg = msg;
00087   server->clear();
00088   // create cube markers
00089   for (size_t i = 0; i < msg->boxes.size(); i++) {
00090     jsk_recognition_msgs::BoundingBox box = msg->boxes[i];
00091     visualization_msgs::InteractiveMarker int_marker;
00092     int_marker.header.frame_id = box.header.frame_id;
00093     int_marker.pose = box.pose;
00094     {
00095       std::stringstream ss;
00096       ss << "box" << "_" << i;
00097       int_marker.name = ss.str();
00098     }
00099     visualization_msgs::InteractiveMarkerControl control;
00100     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00101     
00102     {
00103       std::stringstream ss;
00104       // encode several informations into control name
00105       ss << box.header.stamp.sec << " " << box.header.stamp.nsec << " " << i;
00106       control.name = ss.str();
00107     }
00108     visualization_msgs::Marker marker;
00109     marker.type = visualization_msgs::Marker::CUBE;
00110     marker.scale.x = box.dimensions.x + 0.01;
00111     marker.scale.y = box.dimensions.y + 0.01;
00112     marker.scale.z = box.dimensions.z + 0.01;
00113     marker.color.r = 1.0;
00114     marker.color.g = 1.0;
00115     marker.color.b = 1.0;
00116     marker.color.a = 0.0;
00117     control.markers.push_back(marker);
00118     control.always_visible = true;
00119     int_marker.controls.push_back(control);
00120     server->insert(int_marker);
00121     server->setCallback(int_marker.name, &processFeedback);
00122   }
00123   server->applyChanges();
00124 }
00125 
00126 int main(int argc, char** argv)
00127 {
00128   ros::init(argc, argv, "bounding_box_interactive_marker");
00129   ros::NodeHandle n, pnh("~");
00130   server.reset(new interactive_markers::InteractiveMarkerServer("bounding_box_interactive_marker", "", false));
00131   pub = pnh.advertise<jsk_recognition_msgs::Int32Stamped>("selected_index", 1);
00132   box_pub = pnh.advertise<jsk_recognition_msgs::BoundingBox>("selected_box", 1);
00133   box_arr_pub = pnh.advertise<jsk_recognition_msgs::BoundingBoxArray>("selected_box_array", 1);
00134   ros::Subscriber sub = pnh.subscribe("bounding_box_array", 1, boxCallback);
00135   ros::spin();
00136   server.reset();
00137   return 0;
00138 }


jsk_interactive_marker
Author(s): furuta
autogenerated on Wed May 1 2019 02:40:31