From 90b07934244e5dfb0a4cdb0effede839645e1f66 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 12 Jun 2015 13:00:47 +0900 Subject: [PATCH 1/4] image_view2 and multi_map_server is kept in jsk_common --- jsk_ros_patch/image_view2/CHANGELOG.rst | 341 --- jsk_ros_patch/image_view2/CMakeLists.txt | 49 - jsk_ros_patch/image_view2/cfg/ImageView2.cfg | 29 - jsk_ros_patch/image_view2/image_view2.cpp | 1875 ----------------- jsk_ros_patch/image_view2/image_view2.h | 320 --- .../image_view2/image_view2_node.cpp | 58 - jsk_ros_patch/image_view2/mainpage.dox | 11 - .../image_view2/msg/ImageMarker2.msg | 44 - jsk_ros_patch/image_view2/msg/MouseEvent.msg | 19 - .../image_view2/msg/PointArrayStamped.msg | 4 - jsk_ros_patch/image_view2/package.xml | 57 - .../points_rectangle_extractor.cpp | 152 -- .../image_view2/scale_interaction.py | 67 - jsk_ros_patch/image_view2/srv/ChangeMode.srv | 2 - jsk_ros_patch/image_view2/test-imageview2.l | 150 -- jsk_ros_patch/image_view2/test-toolframe.l | 27 - jsk_ros_patch/multi_map_server/CHANGELOG.rst | 242 --- jsk_ros_patch/multi_map_server/CMakeLists.txt | 44 - jsk_ros_patch/multi_map_server/package.xml | 45 - jsk_ros_patch/multi_map_server/src/main.cpp | 268 --- 20 files changed, 3804 deletions(-) delete mode 100644 jsk_ros_patch/image_view2/CHANGELOG.rst delete mode 100644 jsk_ros_patch/image_view2/CMakeLists.txt delete mode 100755 jsk_ros_patch/image_view2/cfg/ImageView2.cfg delete mode 100644 jsk_ros_patch/image_view2/image_view2.cpp delete mode 100644 jsk_ros_patch/image_view2/image_view2.h delete mode 100644 jsk_ros_patch/image_view2/image_view2_node.cpp delete mode 100644 jsk_ros_patch/image_view2/mainpage.dox delete mode 100644 jsk_ros_patch/image_view2/msg/ImageMarker2.msg delete mode 100644 jsk_ros_patch/image_view2/msg/MouseEvent.msg delete mode 100644 jsk_ros_patch/image_view2/msg/PointArrayStamped.msg delete mode 100644 jsk_ros_patch/image_view2/package.xml delete mode 100644 jsk_ros_patch/image_view2/points_rectangle_extractor.cpp delete mode 100755 jsk_ros_patch/image_view2/scale_interaction.py delete mode 100644 jsk_ros_patch/image_view2/srv/ChangeMode.srv delete mode 100755 jsk_ros_patch/image_view2/test-imageview2.l delete mode 100755 jsk_ros_patch/image_view2/test-toolframe.l delete mode 100644 jsk_ros_patch/multi_map_server/CHANGELOG.rst delete mode 100644 jsk_ros_patch/multi_map_server/CMakeLists.txt delete mode 100644 jsk_ros_patch/multi_map_server/package.xml delete mode 100644 jsk_ros_patch/multi_map_server/src/main.cpp diff --git a/jsk_ros_patch/image_view2/CHANGELOG.rst b/jsk_ros_patch/image_view2/CHANGELOG.rst deleted file mode 100644 index e7a4f6a63..000000000 --- a/jsk_ros_patch/image_view2/CHANGELOG.rst +++ /dev/null @@ -1,341 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package image_view2 -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.0.72 (2015-06-07) -------------------- -* [image_view2] Fix drawing of rectangle when interaction_mode is grabcut_rect -* Contributors: Ryohei Ueda - -1.0.71 (2015-05-17) -------------------- - -1.0.70 (2015-05-08) -------------------- - -1.0.69 (2015-05-05) -------------------- -* [CMakeLists.txt] add depends to gen_cfg -* Contributors: Kei Okada - -1.0.68 (2015-05-05) -------------------- - -1.0.67 (2015-05-03) -------------------- -* [image_view2] add Fisheye Grid Line option -* [jsk_perception] add dynamic reconf for image_view2 -* [image_view2] add grid option -* Contributors: Yuto Inagaki - -1.0.66 (2015-04-03) -------------------- - -1.0.65 (2015-04-02) -------------------- -* [image_view2] Use loner queue for event callback in order not to miss event topics -* Contributors: Ryohei Ueda - -1.0.64 (2015-03-29) -------------------- -* [image_view2] Clear poly mode caches when image_view2 is resetted -* [image_view2] Support poly mode to select polygonal region on image -* [image_view2] Check if input image is valid and skip if the input is invalid -* [image_view2] Do not show warning message when scale is 0 -* Contributors: Ryohei Ueda - -1.0.63 (2015-02-19) -------------------- -* [image_view2] Publish marked image in local namespace -* [image_view2] Ignore scale=0 data in scale_interaction.py -* Contributors: Ryohei Ueda - -1.0.62 (2015-02-17) -------------------- -* [image_view2] Add utility script to scale mouse event from image_view2 - for resized image -* [image_view2] Initialize window_selection_ and font_ variable even in - no-window mode -* [image_view2] Publish rectangular region infromation even in grabcut_rect mode -* [image_view2] Reset rectangle region when changing mode -* [image_view2] Add none mode to ignore any interaction with the user -* [image_view2] Add new flag: ratio_scale to pecify size of text by ratio - to the size of image -* [image_view2] Add left_up_origin flag to ImageMarker2 to draw text from left up origin -* Contributors: Ryohei Ueda - -1.0.61 (2015-02-11) -------------------- -* [image_view2] Add topic interface to emulate mouse event -* [image_view2] Separate main function to another cpp file -* [image_view2] Add std_srvs/Empty interface to change interaction mode -* Contributors: Ryohei Ueda - -1.0.60 (2015-02-03) -------------------- - -1.0.59 (2015-02-03) -------------------- -* Remove rosbuild files -* [image_view2] Add service to change interaction mode -* [image_view2] Support continuous publishing in line selection mode -* [image_view2] Fix timing to publish points selected in line mode -* [image_view2] Add new interaction mode to select line -* [image_view2] Do not publish region outside of the image -* [image_view2] Add ~region_continuous_publish parameter and if it's true, - image_view2 will keep publishing region selected by user -* [image_view2] Do not show image if no image is available -* [image_view2] Do not use time difference to detect point or rectangle -* Contributors: Ryohei Ueda - -1.0.58 (2015-01-07) -------------------- -* [image_view2] Call GUI functions from main thread -* [image_view2] Add new interaction mode to image_view 2 to select - foreground and background by rectangular region -* [image_view2] add mode to select foreground and background - for grabcut -* [image_view2] Use opencv2 c++ function to handle window -* [image_view2] add utility function to resolve tf -* [image_view2] refactor to se smaller function -* [image_view2] Use parameter to change mode to select rectangle or - freeform trajectory instad of "SHIFT KEY" -* [image_view2] Use camel case for methods and functions -* [image_view2] Separate header and cpp file for maintainance -* [image_view2] fix variable name with _ suffix and untabify indents -* [image_view2] Optimize image_view2 to decrease CPU load. - 1) add ~skip_draw_rate to throttle redrawing. - 2) use ros::spin if possible -* Redraw image even though no new message is available -* Add tab-width to image_view2.cpp -* Contributors: Ryohei Ueda - -1.0.57 (2014-12-23) -------------------- - -1.0.56 (2014-12-17) -------------------- - -1.0.55 (2014-12-09) -------------------- - -1.0.54 (2014-11-15) -------------------- - -1.0.53 (2014-11-01) -------------------- - -1.0.52 (2014-10-23) -------------------- - -1.0.51 (2014-10-20) -------------------- - -1.0.50 (2014-10-20) -------------------- - -1.0.49 (2014-10-13) -------------------- - -1.0.48 (2014-10-12) -------------------- -* remove depends to opencv2, since indigo depends on libopencv-dev, so we depends on cv_bridge whcih both hydro/indigo depends on it -* Contributors: Kei Okada - -1.0.47 (2014-10-08) -------------------- - -1.0.46 (2014-10-03) -------------------- - -1.0.45 (2014-09-29) -------------------- - -1.0.44 (2014-09-26) -------------------- - -1.0.43 (2014-09-26) -------------------- - -1.0.42 (2014-09-25) -------------------- - -1.0.41 (2014-09-23) -------------------- - -1.0.40 (2014-09-19) -------------------- - -1.0.39 (2014-09-17) -------------------- - -1.0.38 (2014-09-13) -------------------- - -1.0.36 (2014-09-01) -------------------- - -1.0.35 (2014-08-16) -------------------- - -1.0.34 (2014-08-14) -------------------- - -1.0.33 (2014-07-28) -------------------- - -1.0.32 (2014-07-26) -------------------- - -1.0.31 (2014-07-23) -------------------- - -1.0.30 (2014-07-15) -------------------- - -1.0.29 (2014-07-02) -------------------- - -1.0.28 (2014-06-24) -------------------- - -1.0.27 (2014-06-10) -------------------- -* publish the mouse position to movepoint topic during mouse move event -* Contributors: Ryohei Ueda - -1.0.26 (2014-05-30) -------------------- - -1.0.25 (2014-05-26) -------------------- - -1.0.24 (2014-05-24) -------------------- - -1.0.23 (2014-05-23) -------------------- - -1.0.22 (2014-05-22) -------------------- - -1.0.21 (2014-05-20) -------------------- -* does not check 0.5sec test if the image_view2 is in series mode. -* not use ros::Rate's sleep, use cvWaitKey to captuere - keys to be pressed -* Contributors: Ryohei Ueda - -1.0.20 (2014-05-09) -------------------- - -1.0.19 (2014-05-06) -------------------- - -1.0.18 (2014-05-04) -------------------- - -1.0.17 (2014-04-20) -------------------- - -1.0.16 (2014-04-19) -------------------- - -1.0.15 (2014-04-19) -------------------- - -1.0.14 (2014-04-19) -------------------- - -1.0.13 (2014-04-19) -------------------- - -1.0.12 (2014-04-18) -------------------- - -1.0.11 (2014-04-18) -------------------- - -1.0.10 (2014-04-17) -------------------- - -1.0.9 (2014-04-12) ------------------- - -1.0.8 (2014-04-11) ------------------- - -1.0.4 (2014-03-27) ------------------- -* image_View2:add message_generation, message_runtime to package.xml -* in order to avoid empty catkin_LIBRARIES problem, call generate_messaegs after target_link_libraries -* fix typo CATKIN-DEPENDS -> CATKIN_DEPENDS -* Contributors: Ryohei Ueda, Kei Okada - -1.0.2 (2014-03-12) ------------------- -* `#299 `_: add dependency image_view2 to image_view -* fix image_view2 dependency for rosbuild environment -* Contributors: Ryohei Ueda, nozawa - -1.0.1 (2014-03-07) ------------------- -* added CIRCLE3D type marker sample -* add CIRCLE3D type marker -* Contributors: Kei Okada, HiroyukiMikita - -1.0.0 (2014-03-05) ------------------- -* set all package to 1.0.0 -* install image_view2 -* use rosdep instead of depend package -* add find_package PCL for catkin -* supporting series selection in addition to rectangle selection -* use image_transport parameter, it is the same as image_view -* change for updating drawing while not image comming -* adding dependency to generation_message -* add show_info parameter to display curret frame rate, see Issue 247 -* catkinize image_view2 -* fix all the indent and add the function to fill in the polygon -* add function to draw in the circle -* new parameter: tf_timeout -* support to set the width of a line -* add ~resize_scale_x, ~resize_scale_y parameters for using resized image -* add subscribing point click -* add points_rectangle_extractor.cpp -* changed text msg visualizationo, bigger textsize and color -* add 3d strip/list/polygon/points/text `#850 `_ -* fix typo -* add use_window param -* fix for fuerte -* fix deprecated functions -* update comment for TEXT -* use scale for size of the font -* add text example -* fix putText -* check lastCommonTime -* add comments -* added a flag for action==REMOVE&&id==-1, for clear all the markers -* namespace std is needed in image_view2.cpp -* add blurry mode -* set points size to 10 -* fix out_msg.encoding from TYPE_32FC1 to bgr8 -* update deprecated funcitons to current function api for cam_model -* change fond and use ROS_DEBUG to display tf exception -* send TF exception error at fist 5 times -* changed debug messages for markers from ROS_INFO to ROS_DEBUG -* update to new roseus msg format -* remove deprecated codes -* update to support bayer image and move to cv2 -* draw selecting rectangle every time -* add TEXT type marker, only simple outputs yet -* enable ADD/REMOVE action, lifetime, marker colors partially -* change marker_sub buffer from 1 to 10 -* remove /reset_time -* publish screenpoint and screenrectangle on namespace + imagetopic_name -* add example to see gripper_tool_frame in image_view2 -* remove unused function cmvision-cb -* back to previous version, which is not using subscribeCamera, becouse of slow connection of pr2-network -* rewrite using subscribeCamera -* add image_view2/ -* Contributors: Manabu Saito, kazuto, Kei Okada, youhei, Xiangyu Chen, Ryohei Ueda, mikita diff --git a/jsk_ros_patch/image_view2/CMakeLists.txt b/jsk_ros_patch/image_view2/CMakeLists.txt deleted file mode 100644 index 4847f7f2f..000000000 --- a/jsk_ros_patch/image_view2/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html -cmake_minimum_required(VERSION 2.8.3) -project(image_view2) - -find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure cv_bridge std_msgs sensor_msgs geometry_msgs image_transport tf image_geometry message_filters message_generation std_srvs) - -generate_dynamic_reconfigure_options( - cfg/ImageView2.cfg -) - -add_message_files( - FILES ImageMarker2.msg PointArrayStamped.msg MouseEvent.msg -) - -add_service_files( - FILES ChangeMode.srv - ) - -include_directories(include ${catkin_INCLUDE_DIRS}) - -# Image viewers -find_package(OpenCV REQUIRED) -add_executable(image_view2 image_view2.cpp image_view2_node.cpp) -target_link_libraries(image_view2 ${OpenCV_LIBS} ${catkin_LIBRARIES}) -add_dependencies(image_view2 ${PROJECT_NAME}_gencpp ${PROJECT_NAME}_gencfg) - -# Point Rectangle Extractor -add_executable(points_rectangle_extractor points_rectangle_extractor.cpp) -find_package(Boost REQUIRED COMPONENTS signals) -include_directories(${Boost_INCLUDE_DIRS}) -find_package(PCL REQUIRED) -include_directories(${PCL_INCLUDE_DIRS}) -target_link_libraries(points_rectangle_extractor ${Boost_LIBRARIES} ${catkin_LIBRARIES}) -add_dependencies(points_rectangle_extractor ${PROJECT_NAME}_gencpp) - -generate_messages(DEPENDENCIES geometry_msgs std_msgs) - -catkin_package( - DEPENDS OpenCV PCL - CATKIN_DEPENDS roscpp cv_bridge std_msgs sensor_msgs geometry_msgs image_transport tf image_geometry message_filters - INCLUDE_DIRS # TODO include - LIBRARIES # TODO -) - -install(TARGETS image_view2 points_rectangle_extractor - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/jsk_ros_patch/image_view2/cfg/ImageView2.cfg b/jsk_ros_patch/image_view2/cfg/ImageView2.cfg deleted file mode 100755 index 773b4853a..000000000 --- a/jsk_ros_patch/image_view2/cfg/ImageView2.cfg +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env python - -# set up parameters that we care about -PACKAGE = 'image_view2' - -try: - import imp - imp.find_module(PACKAGE) - from dynamic_reconfigure.parameter_generator_catkin import *; -except: - import roslib; roslib.load_manifest(PACKAGE) - from dynamic_reconfigure.parameter_generator import *; - -from math import pi - -gen = ParameterGenerator () - - -gen.add("grid", bool_t, 0, "use grid", False) -gen.add("fisheye_mode", bool_t, 0, "use fisheye_mode", False) -gen.add("div_u", int_t, 0, "Grid ", 10, 1, 50) -gen.add("div_v", int_t, 0, "Grid ", 10, 1, 50) -gen.add("grid_red", int_t, 0, "Grid Color Red", 255, 0, 255) -gen.add("grid_green", int_t, 0, "Grid Color Green", 0, 0, 255) -gen.add("grid_blue", int_t, 0, "Grid Color Blue", 0, 0, 255) -gen.add("grid_thickness", int_t, 0, "Grid Thickness", 2, 0, 100) -gen.add("grid_space", int_t, 0, "Grid Space for fisheye", 10, 0, 100) - -exit (gen.generate (PACKAGE, "image_view2", "ImageView2")) diff --git a/jsk_ros_patch/image_view2/image_view2.cpp b/jsk_ros_patch/image_view2/image_view2.cpp deleted file mode 100644 index b8bc995df..000000000 --- a/jsk_ros_patch/image_view2/image_view2.cpp +++ /dev/null @@ -1,1875 +0,0 @@ -// -*- tab-width: 8; indent-tabs-mode: nil; -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, Willow Garage, Inc. - * 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/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage 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. - *********************************************************************/ - -#include "image_view2.h" -#include -namespace image_view2{ - ImageView2::ImageView2() : marker_topic_("image_marker"), filename_format_(""), count_(0), mode_(MODE_RECTANGLE), times_(100), window_initialized_(false),space_(10) - { - } - - ImageView2::ImageView2(ros::NodeHandle& nh) - : marker_topic_("image_marker"), filename_format_(""), count_(0), mode_(MODE_RECTANGLE), times_(100), selecting_fg_(true), - left_button_clicked_(false), continuous_ready_(false), window_initialized_(false), - line_select_start_point_(true), line_selected_(false), poly_selecting_done_(true) - { - std::string camera = nh.resolveName("image"); - std::string camera_info = nh.resolveName("camera_info"); - ros::NodeHandle local_nh("~"); - std::string format_string; - std::string transport; - image_transport::ImageTransport it(nh); - image_transport::ImageTransport local_it(camera); - - point_pub_ = nh.advertise(camera + "/screenpoint",100); - point_array_pub_ = nh.advertise(camera + "/screenpoint_array",100); - rectangle_pub_ = nh.advertise(camera + "/screenrectangle",100); - move_point_pub_ = nh.advertise(camera + "/movepoint", 100); - foreground_mask_pub_ = nh.advertise(camera + "/foreground", 100); - background_mask_pub_ = nh.advertise(camera + "/background", 100); - foreground_rect_pub_ = nh.advertise(camera + "/foreground_rect", 100); - background_rect_pub_ = nh.advertise(camera + "/background_rect", 100); - line_pub_ = nh.advertise(camera + "/line", 100); - poly_pub_ = nh.advertise(camera + "/poly", 100); - local_nh.param("window_name", window_name_, std::string("image_view2 [")+camera+std::string("]")); - local_nh.param("skip_draw_rate", skip_draw_rate_, 0); - local_nh.param("autosize", autosize_, false); - local_nh.param("image_transport", transport, std::string("raw")); - local_nh.param("draw_grid", draw_grid_, false); - local_nh.param("blurry", blurry_mode_, false); - local_nh.param("region_continuous_publish", region_continuous_publish_, false); - local_nh.param("filename_format", format_string, std::string("frame%04i.jpg")); - local_nh.param("use_window", use_window, true); - local_nh.param("show_info", show_info_, false); - - double xx,yy; - local_nh.param("resize_scale_x", xx, 1.0); - local_nh.param("resize_scale_y", yy, 1.0); - local_nh.param("tf_timeout", tf_timeout_, 1.0); - - std::string interaction_mode; - local_nh.param("interaction_mode", interaction_mode, std::string("rectangle")); - setMode(stringToMode(interaction_mode)); - resize_x_ = 1.0/xx; - resize_y_ = 1.0/yy; - filename_format_.parse(format_string); - - font_ = cv::FONT_HERSHEY_DUPLEX; - window_selection_.x = window_selection_.y = - window_selection_.height = window_selection_.width = 0; - - image_pub_ = it.advertise("image_marked", 1); - local_image_pub_ = local_it.advertise("marked", 1); - - image_sub_ = it.subscribe(camera, 1, &ImageView2::imageCb, this, transport); - info_sub_ = nh.subscribe(camera_info, 1, &ImageView2::infoCb, this); - marker_sub_ = nh.subscribe(marker_topic_, 10, &ImageView2::markerCb, this); - event_sub_ = local_nh.subscribe(camera + "/event", 100, &ImageView2::eventCb, this); - - change_mode_srv_ = local_nh.advertiseService( - "change_mode", &ImageView2::changeModeServiceCallback, this); - rectangle_mode_srv_ = local_nh.advertiseService( - "rectangle_mode", &ImageView2::rectangleModeServiceCallback, this); - grabcut_mode_srv_ = local_nh.advertiseService( - "grabcut_mode", &ImageView2::grabcutModeServiceCallback, this); - grabcut_rect_mode_srv_ = local_nh.advertiseService( - "grabcut_rect_mode", &ImageView2::grabcutRectModeServiceCallback, this); - line_mode_srv_ = local_nh.advertiseService( - "line_mode", &ImageView2::lineModeServiceCallback, this); - poly_mode_srv_ = local_nh.advertiseService( - "poly_mode", &ImageView2::polyModeServiceCallback, this); - none_mode_srv_ = local_nh.advertiseService( - "none_mode", &ImageView2::noneModeServiceCallback, this); - - srv_ = boost::make_shared >(local_nh); - dynamic_reconfigure::Server::CallbackType f = - boost::bind(&ImageView2::config_callback, this, _1, _2); - srv_->setCallback(f); - } - - ImageView2::~ImageView2() - { - if ( use_window ) { - cv::destroyWindow(window_name_.c_str()); - } - } - - void ImageView2::config_callback(Config &config, uint32_t level) - { - draw_grid_ = config.grid; - fisheye_mode_ = config.fisheye_mode; - div_u_ = config.div_u; - div_v_ = config.div_v; - - grid_red_ = config.grid_red; - grid_blue_ = config.grid_blue; - grid_green_ = config.grid_green; - grid_thickness_ = config.grid_thickness; - space_ = config.grid_space; - } - - void ImageView2::markerCb(const image_view2::ImageMarker2ConstPtr& marker) - { - ROS_DEBUG("markerCb"); - // convert lifetime to duration from Time(0) - if(marker->lifetime != ros::Duration(0)) - boost::const_pointer_cast(marker)->lifetime = (ros::Time::now() - ros::Time(0)) + marker->lifetime; - { - boost::mutex::scoped_lock lock(queue_mutex_); - marker_queue_.push_back(marker); - } - redraw(); - } - - void ImageView2::infoCb(const sensor_msgs::CameraInfoConstPtr& msg) { - ROS_DEBUG("infoCb"); - boost::mutex::scoped_lock lock(info_mutex_); - info_msg_ = msg; - } - - bool ImageView2::lookupTransformation( - std::string frame_id, ros::Time& acquisition_time, - std::map& tf_fail, - tf::StampedTransform &transform) - { - ros::Duration timeout(tf_timeout_); // wait 0.5 sec - try { - ros::Time tm; - tf_listener_.getLatestCommonTime(cam_model_.tfFrame(), frame_id, tm, NULL); - tf_listener_.waitForTransform(cam_model_.tfFrame(), frame_id, - acquisition_time, timeout); - tf_listener_.lookupTransform(cam_model_.tfFrame(), frame_id, - acquisition_time, transform); - tf_fail[frame_id]=0; - return true; - } - catch (tf::TransformException& ex) { - tf_fail[frame_id]++; - if ( tf_fail[frame_id] < 5 ) { - ROS_ERROR("[image_view2] TF exception:\n%s", ex.what()); - } else { - ROS_DEBUG("[image_view2] TF exception:\n%s", ex.what()); - } - return false; - } - } - - void ImageView2::drawCircle(const image_view2::ImageMarker2::ConstPtr& marker) - { - cv::Point2d uv = cv::Point2d(marker->position.x, marker->position.y); - if ( blurry_mode_ ) { - int s0 = (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width); - CvScalar co = MsgToRGB(marker->outline_color); - for (int s1 = s0*10; s1 >= s0; s1--) { - double m = pow((1.0-((double)(s1 - s0))/(s0*9)),2); - cv::circle(draw_, uv, - (marker->scale == 0 ? DEFAULT_CIRCLE_SCALE : marker->scale), - CV_RGB(co.val[2] * m,co.val[1] * m,co.val[0] * m), - s1); - } - } else { - cv::circle(draw_, uv, - (marker->scale == 0 ? DEFAULT_CIRCLE_SCALE : marker->scale), - MsgToRGB(marker->outline_color), - (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - if (marker->filled) { - cv::circle(draw_, uv, - (marker->scale == 0 ? DEFAULT_CIRCLE_SCALE : marker->scale) - (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width) / 2.0, - MsgToRGB(marker->fill_color), - -1); - } - } - } - - void ImageView2::drawLineStrip(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - cv::Point2d p0, p1; - if ( blurry_mode_ ) { - int s0 = (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width); - std::vector::iterator col_it = colors.begin(); - CvScalar co = (*col_it); - for (int s1 = s0*10; s1 >= s0; s1--) { - double m = pow((1.0-((double)(s1 - s0))/(s0*9)),2); - std::vector::const_iterator it = marker->points.begin(); - std::vector::const_iterator end = marker->points.end(); - p0 = cv::Point2d(it->x, it->y); it++; - for ( ; it!= end; it++ ) { - p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, - CV_RGB(co.val[2] * m,co.val[1] * m,co.val[0] * m), - s1); - p0 = p1; - if(++col_it == colors.end()) col_it = colors.begin(); - } - } - } else { - std::vector::const_iterator it = marker->points.begin(); - std::vector::const_iterator end = marker->points.end(); - p0 = cv::Point2d(it->x, it->y); it++; - for ( ; it!= end; it++ ) { - p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, *col_it, (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - p0 = p1; - if(++col_it == colors.end()) col_it = colors.begin(); - } - } - } - - void ImageView2::drawLineList(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - cv::Point2d p0, p1; - if ( blurry_mode_ ) { - int s0 = (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width); - std::vector::iterator col_it = colors.begin(); - CvScalar co = (*col_it); - for (int s1 = s0*10; s1 >= s0; s1--) { - double m = pow((1.0-((double)(s1 - s0))/(s0*9)),2); - std::vector::const_iterator it = marker->points.begin(); - std::vector::const_iterator end = marker->points.end(); - for ( ; it!= end; ) { - p0 = cv::Point2d(it->x, it->y); it++; - if ( it != end ) p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, CV_RGB(co.val[2] * m,co.val[1] * m,co.val[0] * m), s1); - it++; - if(++col_it == colors.end()) col_it = colors.begin(); - } - } - } else { - std::vector::const_iterator it = marker->points.begin(); - std::vector::const_iterator end = marker->points.end(); - for ( ; it!= end; ) { - p0 = cv::Point2d(it->x, it->y); it++; - if ( it != end ) p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, *col_it, (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - it++; - if(++col_it == colors.end()) col_it = colors.begin(); - } - } - } - - void ImageView2::drawPolygon(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - cv::Point2d p0, p1; - if ( blurry_mode_ ) { - int s0 = (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width); - std::vector::iterator col_it = colors.begin(); - CvScalar co = (*col_it); - for (int s1 = s0*10; s1 >= s0; s1--) { - double m = pow((1.0-((double)(s1 - s0))/(s0*9)),2); - std::vector::const_iterator it = marker->points.begin(); - std::vector::const_iterator end = marker->points.end(); - p0 = cv::Point2d(it->x, it->y); it++; - for ( ; it!= end; it++ ) { - p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, CV_RGB(co.val[2] * m,co.val[1] * m,co.val[0] * m), s1); - p0 = p1; - if(++col_it == colors.end()) col_it = colors.begin(); - } - it = marker->points.begin(); - p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, CV_RGB(co.val[2] * m,co.val[1] * m,co.val[0] * m), s1); - } - } else { - std::vector::const_iterator it = marker->points.begin(); - std::vector::const_iterator end = marker->points.end(); - std::vector points; - - if (marker->filled) { - points.push_back(cv::Point(it->x, it->y)); - } - p0 = cv::Point2d(it->x, it->y); it++; - for ( ; it!= end; it++ ) { - p1 = cv::Point2d(it->x, it->y); - if (marker->filled) { - points.push_back(cv::Point(it->x, it->y)); - } - cv::line(draw_, p0, p1, *col_it, (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - p0 = p1; - if(++col_it == colors.end()) col_it = colors.begin(); - } - it = marker->points.begin(); - p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, *col_it, (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - if (marker->filled) { - cv::fillConvexPoly(draw_, points.data(), points.size(), MsgToRGB(marker->fill_color)); - } - } - } - - void ImageView2::drawPoints(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - BOOST_FOREACH(geometry_msgs::Point p, marker->points) { - cv::Point2d uv = cv::Point2d(p.x, p.y); - if ( blurry_mode_ ) { - int s0 = (marker->scale == 0 ? 3 : marker->scale); - CvScalar co = (*col_it); - for (int s1 = s0*2; s1 >= s0; s1--) { - double m = pow((1.0-((double)(s1 - s0))/s0),2); - cv::circle(draw_, uv, s1, - CV_RGB(co.val[2] * m,co.val[1] * m,co.val[0] * m), - -1); - } - } else { - cv::circle(draw_, uv, (marker->scale == 0 ? 3 : marker->scale) , *col_it, -1); - } - if(++col_it == colors.end()) col_it = colors.begin(); - } - } - - void ImageView2::drawFrames(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - static std::map tf_fail; - BOOST_FOREACH(std::string frame_id, marker->frames) { - tf::StampedTransform transform; - ros::Time acquisition_time = last_msg_->header.stamp; - if(!lookupTransformation(frame_id, acquisition_time, tf_fail, transform)) { - return; - } - // center point - tf::Point pt = transform.getOrigin(); - cv::Point3d pt_cv(pt.x(), pt.y(), pt.z()); - cv::Point2d uv; - uv = cam_model_.project3dToPixel(pt_cv); - - static const int RADIUS = 3; - cv::circle(draw_, uv, RADIUS, DEFAULT_COLOR, -1); - - // x, y, z - cv::Point2d uv0, uv1, uv2; - tf::Stamped pin, pout; - - // x - pin = tf::Stamped(tf::Point(0.05, 0, 0), acquisition_time, frame_id); - tf_listener_.transformPoint(cam_model_.tfFrame(), pin, pout); - uv0 = cam_model_.project3dToPixel(cv::Point3d(pout.x(), pout.y(), pout.z())); - // y - pin = tf::Stamped(tf::Point(0, 0.05, 0), acquisition_time, frame_id); - tf_listener_.transformPoint(cam_model_.tfFrame(), pin, pout); - uv1 = cam_model_.project3dToPixel(cv::Point3d(pout.x(), pout.y(), pout.z())); - - // z - pin = tf::Stamped(tf::Point(0, 0, 0.05), acquisition_time, frame_id); - tf_listener_.transformPoint(cam_model_.tfFrame(), pin, pout); - uv2 = cam_model_.project3dToPixel(cv::Point3d(pout.x(), pout.y(), pout.z())); - - // draw - if ( blurry_mode_ ) { - int s0 = 2; - CvScalar c0 = CV_RGB(255,0,0); - CvScalar c1 = CV_RGB(0,255,0); - CvScalar c2 = CV_RGB(0,0,255); - for (int s1 = s0*10; s1 >= s0; s1--) { - double m = pow((1.0-((double)(s1 - s0))/(s0*9)),2); - cv::line(draw_, uv, uv0, - CV_RGB(c0.val[2] * m,c0.val[1] * m,c0.val[0] * m), - s1); - cv::line(draw_, uv, uv1, - CV_RGB(c1.val[2] * m,c1.val[1] * m,c1.val[0] * m), - s1); - cv::line(draw_, uv, uv2, - CV_RGB(c2.val[2] * m,c2.val[1] * m,c2.val[0] * m), - s1); - } - } else { - cv::line(draw_, uv, uv0, CV_RGB(255,0,0), 2); - cv::line(draw_, uv, uv1, CV_RGB(0,255,0), 2); - cv::line(draw_, uv, uv2, CV_RGB(0,0,255), 2); - } - - // index - cv::Size text_size; - int baseline; - text_size = cv::getTextSize(frame_id.c_str(), font_, 1.0, 1.0, &baseline); - cv::Point origin = cv::Point(uv.x - text_size.width / 2, - uv.y - RADIUS - baseline - 3); - cv::putText(draw_, frame_id.c_str(), origin, font_, 1.0, DEFAULT_COLOR, 1.5); - } - } - - cv::Point ImageView2::ratioPoint(double x, double y) - { - if (last_msg_) { - return cv::Point(last_msg_->width * x, - last_msg_->height * y); - } - else { - return cv::Point(0, 0); - } - } - - void ImageView2::drawText(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - cv::Size text_size; - int baseline; - float scale = marker->scale; - if ( scale == 0 ) scale = 1.0; - text_size = cv::getTextSize(marker->text.c_str(), font_, - scale, scale, &baseline); - // fix scale - if (marker->ratio_scale) { - cv::Size a_size = cv::getTextSize("A", font_, 1.0, 1.0, &baseline); - int height_size = a_size.height; - double desired_size = last_msg_->height * scale; - scale = desired_size / height_size; - ROS_DEBUG("text scale: %f", scale); - } - - cv::Point origin; - if (marker->left_up_origin) { - if (marker->ratio_scale) { - origin = ratioPoint(marker->position.x, - marker->position.y); - } - else { - origin = cv::Point(marker->position.x, - marker->position.y); - } - } - else { - if (marker->ratio_scale) { - cv::Point p = ratioPoint(marker->position.x, marker->position.y); - origin = cv::Point(p.x - text_size.width/2, - p.y + baseline+3); - } - else { - origin = cv::Point(marker->position.x - text_size.width/2, - marker->position.y + baseline+3); - } - } - - if (marker->filled) { - cv::putText(draw_, marker->text.c_str(), origin, font_, scale, DEFAULT_COLOR, marker->filled); - - } - else { - cv::putText(draw_, marker->text.c_str(), origin, font_, scale, DEFAULT_COLOR); - } - } - - void ImageView2::drawLineStrip3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - static std::map tf_fail; - std::string frame_id = marker->points3D.header.frame_id; - tf::StampedTransform transform; - ros::Time acquisition_time = last_msg_->header.stamp; - //ros::Time acquisition_time = msg->points3D.header.stamp; - ros::Duration timeout(tf_timeout_); // wait 0.5 sec - try { - ros::Time tm; - tf_listener_.getLatestCommonTime(cam_model_.tfFrame(), frame_id, tm, NULL); - ros::Duration diff = ros::Time::now() - tm; - if ( diff > ros::Duration(1.0) ) { return; } - tf_listener_.waitForTransform(cam_model_.tfFrame(), frame_id, - acquisition_time, timeout); - tf_listener_.lookupTransform(cam_model_.tfFrame(), frame_id, - acquisition_time, transform); - tf_fail[frame_id]=0; - } - catch (tf::TransformException& ex) { - tf_fail[frame_id]++; - if ( tf_fail[frame_id] < 5 ) { - ROS_ERROR("[image_view2] TF exception:\n%s", ex.what()); - } else { - ROS_DEBUG("[image_view2] TF exception:\n%s", ex.what()); - } - return; - } - std::vector points2D; - BOOST_FOREACH(geometry_msgs::Point p, marker->points3D.points) { - tf::Point pt = transform.getOrigin(); - geometry_msgs::PointStamped pt_cam, pt_; - pt_cam.header.frame_id = cam_model_.tfFrame(); - pt_cam.header.stamp = acquisition_time; - pt_cam.point.x = pt.x(); - pt_cam.point.y = pt.y(); - pt_cam.point.z = pt.z(); - tf_listener_.transformPoint(frame_id, pt_cam, pt_); - - cv::Point2d uv; - tf::Stamped pin, pout; - pin = tf::Stamped(tf::Point(pt_.point.x + p.x, pt_.point.y + p.y, pt_.point.z + p.z), acquisition_time, frame_id); - tf_listener_.transformPoint(cam_model_.tfFrame(), pin, pout); - uv = cam_model_.project3dToPixel(cv::Point3d(pout.x(), pout.y(), pout.z())); - geometry_msgs::Point point2D; - point2D.x = uv.x; - point2D.y = uv.y; - points2D.push_back(point2D); - } - cv::Point2d p0, p1; - std::vector::const_iterator it = points2D.begin(); - std::vector::const_iterator end = points2D.end(); - p0 = cv::Point2d(it->x, it->y); it++; - for ( ; it!= end; it++ ) { - p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, *col_it, (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - p0 = p1; - if(++col_it == colors.end()) col_it = colors.begin(); - } - } - - void ImageView2::drawLineList3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - static std::map tf_fail; - std::string frame_id = marker->points3D.header.frame_id; - tf::StampedTransform transform; - ros::Time acquisition_time = last_msg_->header.stamp; - if(!lookupTransformation(frame_id, acquisition_time, tf_fail, transform)) { - return; - } - std::vector points2D; - BOOST_FOREACH(geometry_msgs::Point p, marker->points3D.points) { - tf::Point pt = transform.getOrigin(); - geometry_msgs::PointStamped pt_cam, pt_; - pt_cam.header.frame_id = cam_model_.tfFrame(); - pt_cam.header.stamp = acquisition_time; - pt_cam.point.x = pt.x(); - pt_cam.point.y = pt.y(); - pt_cam.point.z = pt.z(); - tf_listener_.transformPoint(frame_id, pt_cam, pt_); - - cv::Point2d uv; - tf::Stamped pin, pout; - pin = tf::Stamped(tf::Point(pt_.point.x + p.x, pt_.point.y + p.y, pt_.point.z + p.z), acquisition_time, frame_id); - tf_listener_.transformPoint(cam_model_.tfFrame(), pin, pout); - uv = cam_model_.project3dToPixel(cv::Point3d(pout.x(), pout.y(), pout.z())); - geometry_msgs::Point point2D; - point2D.x = uv.x; - point2D.y = uv.y; - points2D.push_back(point2D); - } - cv::Point2d p0, p1; - std::vector::const_iterator it = points2D.begin(); - std::vector::const_iterator end = points2D.end(); - for ( ; it!= end; ) { - p0 = cv::Point2d(it->x, it->y); it++; - if ( it != end ) p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, *col_it, (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - it++; - if(++col_it == colors.end()) col_it = colors.begin(); - } - } - - void ImageView2::drawPolygon3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - static std::map tf_fail; - std::string frame_id = marker->points3D.header.frame_id; - tf::StampedTransform transform; - ros::Time acquisition_time = last_msg_->header.stamp; - if(!lookupTransformation(frame_id, acquisition_time, tf_fail, transform)) { - return; - } - std::vector points2D; - BOOST_FOREACH(geometry_msgs::Point p, marker->points3D.points) { - tf::Point pt = transform.getOrigin(); - geometry_msgs::PointStamped pt_cam, pt_; - pt_cam.header.frame_id = cam_model_.tfFrame(); - pt_cam.header.stamp = acquisition_time; - pt_cam.point.x = pt.x(); - pt_cam.point.y = pt.y(); - pt_cam.point.z = pt.z(); - tf_listener_.transformPoint(frame_id, pt_cam, pt_); - - cv::Point2d uv; - tf::Stamped pin, pout; - pin = tf::Stamped(tf::Point(pt_.point.x + p.x, pt_.point.y + p.y, pt_.point.z + p.z), acquisition_time, frame_id); - tf_listener_.transformPoint(cam_model_.tfFrame(), pin, pout); - uv = cam_model_.project3dToPixel(cv::Point3d(pout.x(), pout.y(), pout.z())); - geometry_msgs::Point point2D; - point2D.x = uv.x; - point2D.y = uv.y; - points2D.push_back(point2D); - } - cv::Point2d p0, p1; - std::vector::const_iterator it = points2D.begin(); - std::vector::const_iterator end = points2D.end(); - std::vector points; - - if (marker->filled) { - points.push_back(cv::Point(it->x, it->y)); - } - p0 = cv::Point2d(it->x, it->y); it++; - for ( ; it!= end; it++ ) { - p1 = cv::Point2d(it->x, it->y); - if (marker->filled) { - points.push_back(cv::Point(it->x, it->y)); - } - cv::line(draw_, p0, p1, *col_it, (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - p0 = p1; - if(++col_it == colors.end()) col_it = colors.begin(); - } - it = points2D.begin(); - p1 = cv::Point2d(it->x, it->y); - cv::line(draw_, p0, p1, *col_it, (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - if (marker->filled) { - cv::fillConvexPoly(draw_, points.data(), points.size(), MsgToRGB(marker->fill_color)); - } - } - - void ImageView2::drawPoints3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - static std::map tf_fail; - std::string frame_id = marker->points3D.header.frame_id; - tf::StampedTransform transform; - ros::Time acquisition_time = last_msg_->header.stamp; - if(!lookupTransformation(frame_id, acquisition_time, tf_fail, transform)) { - return; - } - BOOST_FOREACH(geometry_msgs::Point p, marker->points3D.points) { - tf::Point pt = transform.getOrigin(); - geometry_msgs::PointStamped pt_cam, pt_; - pt_cam.header.frame_id = cam_model_.tfFrame(); - pt_cam.header.stamp = acquisition_time; - pt_cam.point.x = pt.x(); - pt_cam.point.y = pt.y(); - pt_cam.point.z = pt.z(); - tf_listener_.transformPoint(frame_id, pt_cam, pt_); - - cv::Point2d uv; - tf::Stamped pin, pout; - pin = tf::Stamped(tf::Point(pt_.point.x + p.x, pt_.point.y + p.y, pt_.point.z + p.z), acquisition_time, frame_id); - tf_listener_.transformPoint(cam_model_.tfFrame(), pin, pout); - uv = cam_model_.project3dToPixel(cv::Point3d(pout.x(), pout.y(), pout.z())); - cv::circle(draw_, uv, (marker->scale == 0 ? 3 : marker->scale) , *col_it, -1); - } - } - - void ImageView2::drawText3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - static std::map tf_fail; - std::string frame_id = marker->position3D.header.frame_id; - tf::StampedTransform transform; - ros::Time acquisition_time = last_msg_->header.stamp; - if(!lookupTransformation(frame_id, acquisition_time, tf_fail, transform)) { - return; - } - tf::Point pt = transform.getOrigin(); - geometry_msgs::PointStamped pt_cam, pt_; - pt_cam.header.frame_id = cam_model_.tfFrame(); - pt_cam.header.stamp = acquisition_time; - pt_cam.point.x = pt.x(); - pt_cam.point.y = pt.y(); - pt_cam.point.z = pt.z(); - tf_listener_.transformPoint(frame_id, pt_cam, pt_); - - cv::Point2d uv; - tf::Stamped pin, pout; - pin = tf::Stamped(tf::Point(pt_.point.x + marker->position3D.point.x, pt_.point.y + marker->position3D.point.y, pt_.point.z + marker->position3D.point.z), acquisition_time, frame_id); - tf_listener_.transformPoint(cam_model_.tfFrame(), pin, pout); - uv = cam_model_.project3dToPixel(cv::Point3d(pout.x(), pout.y(), pout.z())); - cv::Size text_size; - int baseline; - float scale = marker->scale; - if ( scale == 0 ) scale = 1.0; - text_size = cv::getTextSize(marker->text.c_str(), font_, - scale, scale, &baseline); - cv::Point origin = cv::Point(uv.x - text_size.width/2, - uv.y - baseline-3); - cv::putText(draw_, marker->text.c_str(), origin, font_, scale, CV_RGB(0,255,0),3); - } - - void ImageView2::drawCircle3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it) - { - static std::map tf_fail; - std::string frame_id = marker->pose.header.frame_id; - geometry_msgs::PoseStamped pose; - ros::Time acquisition_time = last_msg_->header.stamp; - ros::Duration timeout(tf_timeout_); // wait 0.5 sec - try { - ros::Time tm; - tf_listener_.getLatestCommonTime(cam_model_.tfFrame(), frame_id, tm, NULL); - tf_listener_.waitForTransform(cam_model_.tfFrame(), frame_id, - acquisition_time, timeout); - tf_listener_.transformPose(cam_model_.tfFrame(), - acquisition_time, marker->pose, frame_id, pose); - tf_fail[frame_id]=0; - } - catch (tf::TransformException& ex) { - tf_fail[frame_id]++; - if ( tf_fail[frame_id] < 5 ) { - ROS_ERROR("[image_view2] TF exception:\n%s", ex.what()); - } else { - ROS_DEBUG("[image_view2] TF exception:\n%s", ex.what()); - } - return; - } - - tf::Quaternion q; - tf::quaternionMsgToTF(pose.pose.orientation, q); - tf::Matrix3x3 rot = tf::Matrix3x3(q); - double angle = (marker->arc == 0 ? 360.0 :marker->angle); - double scale = (marker->scale == 0 ? DEFAULT_CIRCLE_SCALE : marker->scale); - int N = 100; - std::vector< std::vector > ptss; - std::vector pts; - - for (int i=0; iarc == 0 ? true : false), MsgToRGB(marker->outline_color), (marker->width == 0 ? DEFAULT_LINE_WIDTH : marker->width)); - - if (marker->filled) { - if(marker->arc != 0){ - cv::Point2d pt = cam_model_.project3dToPixel(cv::Point3d(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)); - pts.push_back(cv::Point2i((int)pt.x, (int)pt.y)); - ptss.clear(); - ptss.push_back(pts); - } - cv::fillPoly(draw_, ptss, MsgToRGB(marker->fill_color)); - } - } - - void ImageView2::resolveLocalMarkerQueue() - { - { - boost::mutex::scoped_lock lock(queue_mutex_); - - while ( ! marker_queue_.empty() ) { - // remove marker by namespace and id - V_ImageMarkerMessage::iterator new_msg = marker_queue_.begin(); - V_ImageMarkerMessage::iterator message_it = local_queue_.begin(); - for ( ; message_it < local_queue_.end(); ++message_it ) { - if((*new_msg)->ns == (*message_it)->ns && (*new_msg)->id == (*message_it)->id) - message_it = local_queue_.erase(message_it); - } - local_queue_.push_back(*new_msg); - marker_queue_.erase(new_msg); - // if action == REMOVE and id == -1, clear all marker_queue - if ( (*new_msg)->action == image_view2::ImageMarker2::REMOVE && - (*new_msg)->id == -1 ) { - local_queue_.clear(); - } - } - } - - // check lifetime and remove REMOVE-type marker msg - for(V_ImageMarkerMessage::iterator it = local_queue_.begin(); it < local_queue_.end(); it++) { - if((*it)->action == image_view2::ImageMarker2::REMOVE || - ((*it)->lifetime.toSec() != 0.0 && (*it)->lifetime.toSec() < ros::Time::now().toSec())) { - it = local_queue_.erase(it); - } - } - } - - void ImageView2::drawMarkers() - { - resolveLocalMarkerQueue(); - if ( !local_queue_.empty() ) - { - V_ImageMarkerMessage::iterator message_it = local_queue_.begin(); - V_ImageMarkerMessage::iterator message_end = local_queue_.end(); - ROS_DEBUG("markers = %ld", local_queue_.size()); - //processMessage; - for ( ; message_it != message_end; ++message_it ) - { - image_view2::ImageMarker2::ConstPtr& marker = *message_it; - - ROS_DEBUG_STREAM("message type = " << marker->type << ", id " << marker->id); - - // outline colors - std::vector colors; - BOOST_FOREACH(std_msgs::ColorRGBA color, marker->outline_colors) { - colors.push_back(MsgToRGB(color)); - } - if(colors.size() == 0) colors.push_back(DEFAULT_COLOR); - std::vector::iterator col_it = colors.begin(); - - // check camera_info - if( marker->type == image_view2::ImageMarker2::FRAMES || - marker->type == image_view2::ImageMarker2::LINE_STRIP3D || - marker->type == image_view2::ImageMarker2::LINE_LIST3D || - marker->type == image_view2::ImageMarker2::POLYGON3D || - marker->type == image_view2::ImageMarker2::POINTS3D || - marker->type == image_view2::ImageMarker2::TEXT3D || - marker->type == image_view2::ImageMarker2::CIRCLE3D) { - { - boost::mutex::scoped_lock lock(info_mutex_); - if (!info_msg_) { - ROS_WARN("[image_view2] camera_info could not found"); - continue; - } - cam_model_.fromCameraInfo(info_msg_); - } - } - // CIRCLE, LINE_STRIP, LINE_LIST, POLYGON, POINTS - switch ( marker->type ) { - case image_view2::ImageMarker2::CIRCLE: { - drawCircle(marker); - break; - } - case image_view2::ImageMarker2::LINE_STRIP: { - drawLineStrip(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::LINE_LIST: { - drawLineList(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::POLYGON: { - drawPolygon(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::POINTS: { - drawPoints(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::FRAMES: { - drawFrames(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::TEXT: { - drawText(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::LINE_STRIP3D: { - drawLineStrip3D(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::LINE_LIST3D: { - drawLineList3D(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::POLYGON3D: { - drawPolygon3D(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::POINTS3D: { - drawPoints3D(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::TEXT3D: { - drawText3D(marker, colors, col_it); - break; - } - case image_view2::ImageMarker2::CIRCLE3D: { - drawCircle3D(marker, colors, col_it); - break; - } - default: { - ROS_WARN("Undefined Marker type(%d)", marker->type); - break; - } - } - } - } - } - - void ImageView2::drawInteraction() - { - if (mode_ == MODE_RECTANGLE) { - cv::rectangle(draw_, cv::Point(window_selection_.x, window_selection_.y), - cv::Point(window_selection_.x + window_selection_.width, - window_selection_.y + window_selection_.height), - USER_ROI_COLOR, 3, 8, 0); - } - else if (mode_ == MODE_SERIES) { - if (point_array_.size() > 1) { - cv::Point2d from, to; - from = point_array_[0]; - for (size_t i = 1; i < point_array_.size(); i++) { - to = point_array_[i]; - cv::line(draw_, from, to, USER_ROI_COLOR, 2, 8, 0); - from = to; - } - } - } - else if (mode_ == MODE_SELECT_FORE_AND_BACK) { - boost::mutex::scoped_lock lock(point_array_mutex_); - if (point_fg_array_.size() > 1) { - cv::Point2d from, to; - from = point_fg_array_[0]; - for (size_t i = 1; i < point_fg_array_.size(); i++) { - to = point_fg_array_[i]; - cv::line(draw_, from, to, CV_RGB(255, 0, 0), 8, 8, 0); - from = to; - } - } - if (point_bg_array_.size() > 1) { - cv::Point2d from, to; - from = point_bg_array_[0]; - for (size_t i = 1; i < point_bg_array_.size(); i++) { - to = point_bg_array_[i]; - cv::line(draw_, from, to, CV_RGB(0, 255, 0), 8, 8, 0); - from = to; - } - } - } - else if (mode_ == MODE_SELECT_FORE_AND_BACK_RECT) { - boost::mutex::scoped_lock lock(point_array_mutex_); - if (rect_fg_.width != 0 && rect_fg_.height != 0) { - cv::rectangle(draw_, - cv::Point(rect_fg_.x, rect_fg_.y), - cv::Point(rect_fg_.x + rect_fg_.width, rect_fg_.y + rect_fg_.height), - CV_RGB(255, 0, 0), 4); - } - if (rect_bg_.width != 0 && rect_bg_.height != 0) { - cv::rectangle(draw_, - cv::Point(rect_bg_.x, rect_bg_.y), - cv::Point(rect_bg_.x + rect_bg_.width, rect_bg_.y + rect_bg_.height), - CV_RGB(0, 255, 0), 4); - } - } - else if (mode_ == MODE_LINE) { - boost::mutex::scoped_lock lock(line_point_mutex_); - if (line_selected_) { - cv::line(draw_, line_start_point_, line_end_point_, CV_RGB(0, 255, 0), 8, 8, 0); - } - } - else if (mode_ == MODE_POLY) { - boost::mutex::scoped_lock lock(poly_point_mutex_); - if (poly_points_.size() > 0) { - // draw selected points - for (size_t i = 0; i < poly_points_.size() - 1; i++) { - cv::line(draw_, poly_points_[i], poly_points_[i + 1], CV_RGB(255, 0, 0), 8, 8, 0); - } - // draw selecting points - if (poly_selecting_done_) { - cv::line(draw_, poly_points_[poly_points_.size() - 1], - poly_points_[0], - CV_RGB(255, 0, 0), 8, 8, 0); - } - else { - cv::line(draw_, poly_points_[poly_points_.size() - 1], - poly_selecting_point_, - CV_RGB(0, 255, 0), 8, 8, 0); - } - } - } - } - - void ImageView2::drawInfo(ros::Time& before_rendering) - { - static ros::Time last_time; - static std::string info_str_1, info_str_2; - // update info_str_1, info_str_2 if possible - if ( show_info_ && times_.size() > 0 && ( before_rendering.toSec() - last_time.toSec() > 2 ) ) { - int n = times_.size(); - double mean = 0, rate = 1.0, std_dev = 0.0, max_delta, min_delta; - - std::for_each( times_.begin(), times_.end(), (mean += boost::lambda::_1) ); - mean /= n; - rate /= mean; - - std::for_each( times_.begin(), times_.end(), (std_dev += (boost::lambda::_1 - mean)*(boost::lambda::_1 - mean) ) ); - std_dev = sqrt(std_dev/n); - min_delta = *std::min_element(times_.begin(), times_.end()); - max_delta = *std::max_element(times_.begin(), times_.end()); - - std::stringstream f1, f2; - f1.precision(3); f1 << std::fixed; - f2.precision(3); f2 << std::fixed; - f1 << "" << image_sub_.getTopic() << " : rate:" << rate; - f2 << "min:" << min_delta << "s max: " << max_delta << "s std_dev: " << std_dev << "s n: " << n; - info_str_1 = f1.str(); - info_str_2 = f2.str(); - ROS_INFO_STREAM(info_str_1 + " " + info_str_2); - times_.clear(); - last_time = before_rendering; - } - if (!info_str_1.empty() && !info_str_2.empty()) { - cv::putText(image_, info_str_1.c_str(), cv::Point(10,image_.rows-34), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar::all(255), 2); - cv::putText(image_, info_str_2.c_str(), cv::Point(10,image_.rows-10), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar::all(255), 2); - } - } - - void ImageView2::redraw() - { - if (original_image_.empty()) { - ROS_WARN("no image is available yet"); - return; - } - ros::Time before_rendering = ros::Time::now(); - original_image_.copyTo(image_); - // Draw Section - if ( blurry_mode_ ) { - draw_ = cv::Mat(image_.size(), image_.type(), CV_RGB(0,0,0)); - } else { - draw_ = image_; - } - drawMarkers(); - drawInteraction(); - - if ( draw_grid_ ) drawGrid(); - if ( blurry_mode_ ) cv::addWeighted(image_, 0.9, draw_, 1.0, 0.0, image_); - if ( use_window ) { - if (show_info_) { - drawInfo(before_rendering); - } - } - cv_bridge::CvImage out_msg; - out_msg.header = last_msg_->header; - out_msg.encoding = "bgr8"; - out_msg.image = image_; - image_pub_.publish(out_msg.toImageMsg()); - local_image_pub_.publish(out_msg.toImageMsg()); - } - - void ImageView2::createDistortGridImage() - { - distort_grid_mask_.setTo(cv::Scalar(0,0,0)); - cv::Mat distort_grid_mask(draw_.size(), CV_8UC1); - distort_grid_mask.setTo(cv::Scalar(0)); - const float K = 341.0; - float R_divier = (1.0/(draw_.cols/2)), center_x = distort_grid_mask.rows/2, center_y = distort_grid_mask.cols/2; - for(int degree = -80; degree<=80; degree+=space_){ - double C = draw_.cols/2.0 * tan(degree * 3.14159265/180); - for(float theta = -1.57; theta <= 1.57; theta+=0.001){ - double sin_phi = C * R_divier / tan(theta); - if (sin_phi > 1.0 || sin_phi < -1.0) - continue; - int x1 = K * theta * sqrt(1-sin_phi * sin_phi) + center_x; - int y1 = K * theta * sin_phi + center_y; - int x2 = K * theta * sin_phi + center_x; - int y2 = K * theta * sqrt(1-sin_phi * sin_phi) + center_y; - cv::circle(distort_grid_mask, cvPoint(y1, x1), 2, 1, grid_thickness_); - cv::circle(distort_grid_mask, cvPoint(y2, x2), 2, 1, grid_thickness_); - } - } - cv::Mat red(draw_.size(), draw_.type(), CV_8UC3); - red = cv::Scalar(grid_blue_, grid_green_, grid_red_); - red.copyTo(distort_grid_mask_, distort_grid_mask); - } - - void ImageView2::drawGrid() - { - if(fisheye_mode_){ - if(space_ != prev_space_ || prev_red_ != grid_red_ - || prev_green_ != grid_green_ || prev_blue_ != grid_blue_ - || prev_thickness_ != grid_thickness_){ - createDistortGridImage(); - prev_space_ = space_; - prev_red_ = grid_red_; - prev_green_ = grid_green_; - prev_blue_ = grid_blue_; - prev_thickness_ = grid_thickness_; - } - cv::add(draw_, distort_grid_mask_, draw_); - }else{ - //Main Center Lines - cv::Point2d p0 = cv::Point2d(0, last_msg_->height/2.0); - cv::Point2d p1 = cv::Point2d(last_msg_->width, last_msg_->height/2.0); - cv::line(draw_, p0, p1, CV_RGB(255,0,0), DEFAULT_LINE_WIDTH); - - cv::Point2d p2 = cv::Point2d(last_msg_->width/2.0, 0); - cv::Point2d p3 = cv::Point2d(last_msg_->width/2.0, last_msg_->height); - cv::line(draw_, p2, p3, CV_RGB(255,0,0), DEFAULT_LINE_WIDTH); - - for(int i = 1; i < div_u_ ;i ++){ - cv::Point2d u0 = cv::Point2d(0, last_msg_->height * i * 1.0 / div_u_); - cv::Point2d u1 = cv::Point2d(last_msg_->width, last_msg_->height * i * 1.0 / div_u_); - cv::line(draw_, u0, u1, CV_RGB(255,0,0), 1); - } - - for(int i = 1; i < div_v_ ;i ++){ - cv::Point2d v0 = cv::Point2d(last_msg_->width * i * 1.0 / div_v_, 0); - cv::Point2d v1 = cv::Point2d(last_msg_->width * i * 1.0 / div_v_, last_msg_->height); - cv::line(draw_, v0, v1, CV_RGB(255,0,0), 1); - } - } - } - - void ImageView2::imageCb(const sensor_msgs::ImageConstPtr& msg) - { - if (msg->width == 0 && msg->height == 0) { - ROS_DEBUG("invalid image"); - return; - } - static int count = 0; - if (count < skip_draw_rate_) { - count++; - return; - } - else { - count = 0; - } - static ros::Time old_time; - times_.push_front(ros::Time::now().toSec() - old_time.toSec()); - old_time = ros::Time::now(); - - if(old_time.toSec() - ros::Time::now().toSec() > 0) { - ROS_WARN("TF Cleared for old time"); - } - { - boost::mutex::scoped_lock lock(image_mutex_); - if (msg->encoding.find("bayer") != std::string::npos) { - original_image_ = cv::Mat(msg->height, msg->width, CV_8UC1, - const_cast(&msg->data[0]), msg->step); - } else { - try { - original_image_ = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image; - } catch (cv_bridge::Exception& e) { - ROS_ERROR("Unable to convert %s image to bgr8", msg->encoding.c_str()); - return; - } - } - // Hang on to message pointer for sake of mouseCb - last_msg_ = msg; - redraw(); - } - if (region_continuous_publish_ && continuous_ready_) { - publishMouseInteractionResult(); - } - } - - void ImageView2::drawImage() { - if (image_.rows > 0 && image_.cols > 0) { - redraw(); - } - } - - void ImageView2::addPoint(int x, int y) - { - cv::Point2d p; - p.x = x; - p.y = y; - { - boost::mutex::scoped_lock lock(point_array_mutex_); - point_array_.push_back(p); - } - } - - void ImageView2::setRegionWindowPoint(int x, int y) - { - boost::mutex::scoped_lock lock(point_array_mutex_); - ROS_DEBUG("setRegionWindowPoint(%d, %d)", x, y); - if (selecting_fg_) { - rect_fg_.x = x; - rect_fg_.y = y; - rect_fg_.width = 0; - rect_fg_.height = 0; - } - else { - rect_bg_.x = x; - rect_bg_.y = y; - rect_bg_.width = 0; - rect_bg_.height = 0; - } - } - - void ImageView2::updateRegionWindowSize(int x, int y) - { - ROS_DEBUG("updateRegionWindowPoint"); - boost::mutex::scoped_lock lock(point_array_mutex_); - if (selecting_fg_) { - rect_fg_.width = x - rect_fg_.x; - rect_fg_.height = y - rect_fg_.y; - } - else { - rect_bg_.width = x - rect_bg_.x; - rect_bg_.height = y - rect_bg_.y; - } - } - - void ImageView2::addRegionPoint(int x, int y) - { - cv::Point2d p; - p.x = x; - p.y = y; - { - boost::mutex::scoped_lock lock(point_array_mutex_); - if (selecting_fg_) { - point_fg_array_.push_back(p); - } - else { - point_bg_array_.push_back(p); - } - } - } - - void ImageView2::clearPointArray() - { - boost::mutex::scoped_lock lock(point_array_mutex_); - point_fg_array_.clear(); - point_bg_array_.clear(); - point_array_.clear(); - } - - void ImageView2::publishPointArray() - { - pcl::PointCloud pcl_cloud; - for (size_t i = 0; i < point_array_.size(); i++) { - pcl::PointXY p; - p.x = point_array_[i].x; - p.y = point_array_[i].y; - pcl_cloud.points.push_back(p); - } - sensor_msgs::PointCloud2::Ptr ros_cloud(new sensor_msgs::PointCloud2); - pcl::toROSMsg(pcl_cloud, *ros_cloud); - ros_cloud->header.stamp = ros::Time::now(); - - point_array_pub_.publish(ros_cloud); - } - - - void ImageView2::setMode(KEY_MODE mode) - { - mode_ = mode; - } - - ImageView2::KEY_MODE ImageView2::getMode() - { - return mode_; - } - - bool ImageView2::toggleSelection() - { - boost::mutex::scoped_lock lock(point_array_mutex_); - selecting_fg_ = !selecting_fg_; - return selecting_fg_; - } - - void ImageView2::pointArrayToMask(std::vector& points, - cv::Mat& mask) - { - cv::Point2d from, to; - if (points.size() > 1) { - from = points[0]; - for (size_t i = 1; i < points.size(); i++) { - to = points[i]; - cv::line(mask, from, to, cv::Scalar(255), 8, 8, 0); - from = to; - } - } - } - - void ImageView2::publishMonoImage(ros::Publisher& pub, - cv::Mat& image, - const std_msgs::Header& header) - { - cv_bridge::CvImage image_bridge( - header, sensor_msgs::image_encodings::MONO8, image); - pub.publish(image_bridge.toImageMsg()); - } - - void ImageView2::publishForegroundBackgroundMask() - { - boost::mutex::scoped_lock lock(image_mutex_); - boost::mutex::scoped_lock lock2(point_array_mutex_); - cv::Mat foreground_mask - = cv::Mat::zeros(last_msg_->height, last_msg_->width, CV_8UC1); - cv::Mat background_mask - = cv::Mat::zeros(last_msg_->height, last_msg_->width, CV_8UC1); - if (getMode() == MODE_SELECT_FORE_AND_BACK) { - pointArrayToMask(point_fg_array_, foreground_mask); - pointArrayToMask(point_bg_array_, background_mask); - } - else if (getMode() == MODE_SELECT_FORE_AND_BACK_RECT) { - cv::rectangle(foreground_mask, rect_fg_, cv::Scalar(255), CV_FILLED); - cv::rectangle(background_mask, rect_bg_, cv::Scalar(255), CV_FILLED); - } - publishMonoImage(foreground_mask_pub_, foreground_mask, last_msg_->header); - publishMonoImage(background_mask_pub_, background_mask, last_msg_->header); - publishRectFromMaskImage(foreground_rect_pub_, foreground_mask, last_msg_->header); - publishRectFromMaskImage(background_rect_pub_, background_mask, last_msg_->header); - } - - void ImageView2::publishRectFromMaskImage( - ros::Publisher& pub, - cv::Mat& image, - const std_msgs::Header& header) - { - int min_x = image.cols; - int min_y = image.rows; - int max_x = 0; - int max_y = 0; - for (int j = 0; j < image.rows; j++) { - for (int i = 0; i < image.cols; i++) { - if (image.at(j, i) != 0) { - min_x = std::min(min_x, i); - min_y = std::min(min_y, j); - max_x = std::max(max_x, i); - max_y = std::max(max_y, j); - } - } - } - geometry_msgs::PolygonStamped poly; - poly.header = header; - geometry_msgs::Point32 min_pt, max_pt; - min_pt.x = min_x; - min_pt.y = min_y; - max_pt.x = max_x; - max_pt.y = max_y; - poly.polygon.points.push_back(min_pt); - poly.polygon.points.push_back(max_pt); - pub.publish(poly); - } - - void ImageView2::publishLinePoints() - { - boost::mutex::scoped_lock lock(line_point_mutex_); - geometry_msgs::PolygonStamped ros_line; - ros_line.header = last_msg_->header; - geometry_msgs::Point32 ros_start_point, ros_end_point; - ros_start_point.x = line_start_point_.x; - ros_start_point.y = line_start_point_.y; - ros_end_point.x = line_end_point_.x; - ros_end_point.y = line_end_point_.y; - ros_line.polygon.points.push_back(ros_start_point); - ros_line.polygon.points.push_back(ros_end_point); - line_pub_.publish(ros_line); - } - - void ImageView2::publishMouseInteractionResult() - { - if (getMode() == MODE_SERIES) { - publishPointArray(); - } - else if (getMode() == MODE_SELECT_FORE_AND_BACK || - getMode() == MODE_SELECT_FORE_AND_BACK_RECT) { - publishForegroundBackgroundMask(); - } - else if (getMode() == MODE_LINE) { - publishLinePoints(); - } - else { - cv::Point2f Pt_1(window_selection_.x, window_selection_.y); - cv::Point2f Pt(button_up_pos_); - std::cout << "PT_1" << Pt_1 << std::endl; - std::cout << "Pt" << Pt << std::endl; - if (!isValidMovement(Pt_1, Pt)) { - geometry_msgs::PointStamped screen_msg; - screen_msg.point.x = window_selection_.x * resize_x_; - screen_msg.point.y = window_selection_.y * resize_y_; - screen_msg.point.z = 0; - screen_msg.header.stamp = last_msg_->header.stamp; - ROS_INFO("Publish screen point %s (%f %f)", point_pub_.getTopic().c_str(), screen_msg.point.x, screen_msg.point.y); - point_pub_.publish(screen_msg); - } else { - geometry_msgs::PolygonStamped screen_msg; - screen_msg.polygon.points.resize(2); - screen_msg.polygon.points[0].x = window_selection_.x * resize_x_; - screen_msg.polygon.points[0].y = window_selection_.y * resize_y_; - screen_msg.polygon.points[1].x = (window_selection_.x + window_selection_.width) * resize_x_; - screen_msg.polygon.points[1].y = (window_selection_.y + window_selection_.height) * resize_y_; - screen_msg.header.stamp = last_msg_->header.stamp; - ROS_INFO("Publish rectangle point %s (%f %f %f %f)", rectangle_pub_.getTopic().c_str(), - screen_msg.polygon.points[0].x, screen_msg.polygon.points[0].y, - screen_msg.polygon.points[1].x, screen_msg.polygon.points[1].y); - rectangle_pub_.publish(screen_msg); - continuous_ready_ = true; - } - } - } - - - bool ImageView2::isValidMovement(const cv::Point2f& start_point, - const cv::Point2f& end_point) - { - double dist_px = cv::norm(cv::Mat(start_point), cv::Mat(end_point)); - return dist_px > 3.0; - } - - void ImageView2::updateLineStartPoint(cv::Point p) - { - boost::mutex::scoped_lock lock(line_point_mutex_); - line_start_point_ = p; - } - - void ImageView2::updateLineEndPoint(cv::Point p) - { - boost::mutex::scoped_lock lock(line_point_mutex_); - line_end_point_ = p; - } - - cv::Point ImageView2::getLineStartPoint() - { - boost::mutex::scoped_lock lock(line_point_mutex_); - return cv::Point(line_start_point_); - } - - cv::Point ImageView2::getLineEndPoint() - { - boost::mutex::scoped_lock lock(line_point_mutex_); - return cv::Point(line_end_point_); - } - - void ImageView2::updateLinePoint(cv::Point p) - { - - if (isSelectingLineStartPoint()) { - updateLineStartPoint(p); - updateLineEndPoint(p); - } - else { - updateLineEndPoint(p); - } - { - boost::mutex::scoped_lock lock(line_point_mutex_); - line_select_start_point_ = !line_select_start_point_; - line_selected_ = true; - } - } - - bool ImageView2::isSelectingLineStartPoint() - { - boost::mutex::scoped_lock lock(line_point_mutex_); - return line_select_start_point_; - } - - void ImageView2::processLeftButtonDown(int x, int y) - { - ROS_DEBUG("processLeftButtonDown"); - left_button_clicked_ = true; - continuous_ready_ = false; - window_selection_.x = x; - window_selection_.y = y; - window_selection_.width = window_selection_.height = 0; - if (getMode() == MODE_SELECT_FORE_AND_BACK_RECT) { - setRegionWindowPoint(x, y); - } - if (getMode() == MODE_LINE) { - continuous_ready_ = false; - } - } - - void ImageView2::processMove(int x, int y) - { - if (left_button_clicked_) { - cv::Point2f Pt_1(window_selection_.x, window_selection_.y); - cv::Point2f Pt(x, y); - if (isValidMovement(Pt_1, Pt)) { - if (getMode() == MODE_RECTANGLE) { - window_selection_.width = x - window_selection_.x; - window_selection_.height = y - window_selection_.y; - } - else if (getMode() == MODE_SERIES) { - addPoint(x, y); - } - else if (getMode() == MODE_SELECT_FORE_AND_BACK) { - addRegionPoint(x, y); - } - else if (getMode() == MODE_SELECT_FORE_AND_BACK_RECT) { - updateRegionWindowSize(x, y); - } - } - // publish the points - geometry_msgs::PointStamped move_point; - move_point.header.stamp = ros::Time::now(); - move_point.point.x = x; - move_point.point.y = y; - move_point.point.z = 0; - move_point_pub_.publish(move_point); - } - else { - if (getMode() == MODE_LINE) { - if (!isSelectingLineStartPoint()) { - updateLineEndPoint(cv::Point(x, y)); - } - } - else if (getMode() == MODE_POLY) { - updatePolySelectingPoint(cv::Point(x, y)); - } - } - } - - void ImageView2::processLeftButtonUp(int x, int y) - { - if (!left_button_clicked_) { - return; - } - if (getMode() == MODE_SERIES) { - publishMouseInteractionResult(); - clearPointArray(); - } - else if (getMode() == MODE_SELECT_FORE_AND_BACK || - getMode() == MODE_SELECT_FORE_AND_BACK_RECT) { - bool fgp = toggleSelection(); - if (fgp) { - publishMouseInteractionResult(); - continuous_ready_ = true; - //clearPointArray(); - } - } - else if (getMode() == MODE_RECTANGLE) { - // store x and y - button_up_pos_ = cv::Point2f(x, y); - publishMouseInteractionResult(); - } - else if (getMode() == MODE_LINE) { - updateLinePoint(cv::Point(x, y)); - if (isSelectingLineStartPoint()) { - publishMouseInteractionResult(); - continuous_ready_ = true; - } - } - else if (getMode() == MODE_POLY) { - if (isPolySelectingFirstTime()) { - continuous_ready_ = false; - clearPolyPoints(); - } - updatePolyPoint(cv::Point(x, y)); - } - left_button_clicked_ = false; - } - - void ImageView2::updatePolyPoint(cv::Point p) - { - boost::mutex::scoped_lock lock(poly_point_mutex_); - poly_points_.push_back(p); - } - - void ImageView2::publishPolyPoints() - { - boost::mutex::scoped_lock lock(poly_point_mutex_); - geometry_msgs::PolygonStamped poly; - poly.header = last_msg_->header; - for (size_t i = 0; i < poly_points_.size(); i++) { - geometry_msgs::Point32 p; - p.x = poly_points_[i].x; - p.y = poly_points_[i].y; - p.z = 0; - poly.polygon.points.push_back(p); - } - poly_pub_.publish(poly); - } - - void ImageView2::updatePolySelectingPoint(cv::Point p) - { - boost::mutex::scoped_lock lock(poly_point_mutex_); - poly_selecting_point_ = p; - } - - void ImageView2::finishSelectingPoly() - { - boost::mutex::scoped_lock lock(poly_point_mutex_); - poly_selecting_done_ = true; - } - - bool ImageView2::isPolySelectingFirstTime() - { - boost::mutex::scoped_lock lock(poly_point_mutex_); - return poly_selecting_done_; - } - - void ImageView2::clearPolyPoints() - { - boost::mutex::scoped_lock lock(poly_point_mutex_); - poly_selecting_done_ = false; - poly_points_.clear(); - } - - void ImageView2::processMouseEvent(int event, int x, int y, int flags, void* param) - { - checkMousePos(x, y); - switch (event){ - case CV_EVENT_MOUSEMOVE: { - processMove(x, y); - break; - } - case CV_EVENT_LBUTTONDOWN: // click - processLeftButtonDown(x, y); - break; - case CV_EVENT_LBUTTONUP: - processLeftButtonUp(x, y); - break; - case CV_EVENT_RBUTTONDOWN: - { - if (getMode() == MODE_POLY) { - // close the polygon - finishSelectingPoly(); - publishPolyPoints(); - continuous_ready_ = true; - } - else { - boost::mutex::scoped_lock lock(image_mutex_); - if (!image_.empty()) { - std::string filename = (filename_format_ % count_).str(); - cv::imwrite(filename.c_str(), image_); - ROS_INFO("Saved image %s", filename.c_str()); - count_++; - } else { - ROS_WARN("Couldn't save image, no data!"); - } - } - break; - } - } - { - boost::mutex::scoped_lock lock2(image_mutex_); - drawImage(); - } - return; - } - - void ImageView2::mouseCb(int event, int x, int y, int flags, void* param) - { - ROS_DEBUG("mouseCB"); - ImageView2 *iv = (ImageView2*)param; - iv->processMouseEvent(event, x, y, flags, param); - return; - } - - void ImageView2::pressKey(int key) - { - if (key != -1) { - switch (key) { - case 27: { - resetInteraction(); - break; - } - } - } - } - - void ImageView2::showImage() - { - if (use_window) { - if (!window_initialized_) { - cv::setMouseCallback(window_name_.c_str(), &ImageView2::mouseCb, this); - cv::namedWindow(window_name_.c_str(), autosize_ ? CV_WINDOW_AUTOSIZE : 0); - window_initialized_ = false; - } - if(!image_.empty()) { - cv::imshow(window_name_.c_str(), image_); - } - } - } - - void ImageView2::checkMousePos(int& x, int& y) - { - if (last_msg_) { - x = std::max(std::min(x, (int)last_msg_->width), 0); - y = std::max(std::min(y, (int)last_msg_->height), 0); - } - } - void ImageView2::resetInteraction() - { - if (getMode() == MODE_SELECT_FORE_AND_BACK) { - boost::mutex::scoped_lock lock(point_array_mutex_); - point_fg_array_.clear(); - point_bg_array_.clear(); - selecting_fg_ = true; - } - else if (getMode() == MODE_SELECT_FORE_AND_BACK_RECT) { - boost::mutex::scoped_lock lock(point_array_mutex_); - rect_fg_.width = rect_fg_.height = 0; - rect_bg_.width = rect_bg_.height = 0; - selecting_fg_ = true; - } - else if (getMode() == MODE_LINE) { - boost::mutex::scoped_lock lock(line_point_mutex_); - line_select_start_point_ = true; - line_selected_ = false; - continuous_ready_ = false; - } - else if (getMode() == MODE_RECTANGLE) { - button_up_pos_ = cv::Point2f(0, 0); - window_selection_.width = 0; - window_selection_.height = 0; - } - else if (getMode() == MODE_POLY) { - clearPolyPoints(); - } - } - - bool ImageView2::rectangleModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res) - { - resetInteraction(); - setMode(MODE_RECTANGLE); - resetInteraction(); - return true; - } - - bool ImageView2::seriesModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res) - { - resetInteraction(); - setMode(MODE_SERIES); - resetInteraction(); - return true; - } - - bool ImageView2::grabcutModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res) - { - resetInteraction(); - setMode(MODE_SELECT_FORE_AND_BACK); - resetInteraction(); - return true; - } - - bool ImageView2::grabcutRectModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res) - { - resetInteraction(); - setMode(MODE_SELECT_FORE_AND_BACK_RECT); - resetInteraction(); - return true; - } - - bool ImageView2::lineModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res) - { - resetInteraction(); - setMode(MODE_LINE); - resetInteraction(); - return true; - } - - bool ImageView2::polyModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res) - { - resetInteraction(); - setMode(MODE_POLY); - resetInteraction(); - return true; - } - - bool ImageView2::noneModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res) - { - resetInteraction(); - setMode(MODE_NONE); - resetInteraction(); - return true; - } - - bool ImageView2::changeModeServiceCallback( - image_view2::ChangeModeRequest& req, - image_view2::ChangeModeResponse& res) - { - resetInteraction(); - KEY_MODE next_mode = stringToMode(req.mode); - setMode(next_mode); - resetInteraction(); - return true; - } - - ImageView2::KEY_MODE ImageView2::stringToMode(const std::string& interaction_mode) - { - if (interaction_mode == "rectangle") { - return MODE_RECTANGLE; - } - else if (interaction_mode == "freeform" || - interaction_mode == "series") { - return MODE_SERIES; - } - else if (interaction_mode == "grabcut") { - return MODE_SELECT_FORE_AND_BACK; - } - else if (interaction_mode == "grabcut_rect") { - return MODE_SELECT_FORE_AND_BACK_RECT; - } - else if (interaction_mode == "line") { - return MODE_LINE; - } - else if (interaction_mode == "poly") { - return MODE_POLY; - } - else if (interaction_mode == "none") { - return MODE_NONE; - } - else { - throw std::string("Unknown mode"); - } - } - - void ImageView2::eventCb( - const image_view2::MouseEvent::ConstPtr& event_msg) - { - //ROS_INFO("eventCb"); - if (event_msg->type == image_view2::MouseEvent::KEY_PRESSED) { - pressKey(event_msg->key); - } - else { - // scale x, y according to width/height - int x, y; - { - boost::mutex::scoped_lock lock(image_mutex_); - if (!last_msg_) { - ROS_WARN("Image is not yet available"); - return; - } - x = ((float)last_msg_->width / event_msg->width) * event_msg->x; - y = ((float)last_msg_->height / event_msg->height) * event_msg->y; - } - // scale - if (event_msg->type == image_view2::MouseEvent::MOUSE_LEFT_UP) { - //ROS_INFO("mouse_left_up"); - processMouseEvent(CV_EVENT_LBUTTONUP, x, y, 0, NULL); - } - else if (event_msg->type == image_view2::MouseEvent::MOUSE_LEFT_DOWN) { - //ROS_INFO("mouse_left_down"); - processMouseEvent(CV_EVENT_LBUTTONDOWN, x, y, 0, NULL); - } - else if (event_msg->type == image_view2::MouseEvent::MOUSE_MOVE) { - //ROS_INFO("mouse_move"); - processMouseEvent(CV_EVENT_MOUSEMOVE, x, y, 0, NULL); - } - else if (event_msg->type == image_view2::MouseEvent::MOUSE_RIGHT_DOWN) { - //ROS_INFO("mouse_right_down"); - processMouseEvent(CV_EVENT_RBUTTONDOWN, x, y, 0, NULL); - } - } - } -} - - diff --git a/jsk_ros_patch/image_view2/image_view2.h b/jsk_ros_patch/image_view2/image_view2.h deleted file mode 100644 index a49602a12..000000000 --- a/jsk_ros_patch/image_view2/image_view2.h +++ /dev/null @@ -1,320 +0,0 @@ -// -*- mode: c++ -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014, JSK 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 Willow Garage 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. - *********************************************************************/ - -#ifndef IMAGE_VIEW2_H_ -#define IMAGE_VIEW2_H_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#define DEFAULT_COLOR CV_RGB(255,0,0) -#define USER_ROI_COLOR CV_RGB(255,0,0) -#define DEFAULT_CIRCLE_SCALE 20 -#define DEFAULT_LINE_WIDTH 3 - -namespace image_view2 -{ - typedef std::vector V_ImageMarkerMessage; - inline CvScalar MsgToRGB(const std_msgs::ColorRGBA &color){ - if(color.a == 0.0 && color.r == 0.0 && color.g == 0.0 && color.b == 0.0) - return DEFAULT_COLOR; - else - return CV_RGB(color.r*255, color.g*255, color.b*255); - } - - class ImageView2 - { - public: - typedef ImageView2Config Config; - enum KEY_MODE { - MODE_RECTANGLE, - MODE_SERIES, - MODE_SELECT_FORE_AND_BACK, - MODE_SELECT_FORE_AND_BACK_RECT, - MODE_LINE, - MODE_POLY, - MODE_NONE - }; - - ImageView2(); - ImageView2(ros::NodeHandle& nh); - ~ImageView2(); - void pressKey(int key); - void markerCb(const image_view2::ImageMarker2ConstPtr& marker); - void infoCb(const sensor_msgs::CameraInfoConstPtr& msg); - void redraw(); - void imageCb(const sensor_msgs::ImageConstPtr& msg); - void drawImage(); - void addPoint(int x, int y); - void addRegionPoint(int x, int y); - void updateRegionWindowSize(int x, int y); - void setRegionWindowPoint(int x, int y); - void clearPointArray(); - void publishPointArray(); - void setMode(KEY_MODE mode); - KEY_MODE getMode(); - void showImage(); - static void mouseCb(int event, int x, int y, int flags, void* param); - bool isValidMovement(const cv::Point2f& start_point, - const cv::Point2f& end_point); - bool toggleSelection(); - void publishForegroundBackgroundMask(); - bool use_window; - protected: - private: - void config_callback(Config &config, uint32_t level); - void eventCb( - const image_view2::MouseEvent::ConstPtr& event_msg); - void pointArrayToMask(std::vector& points, - cv::Mat& mask); - void publishMonoImage(ros::Publisher& pub, - cv::Mat& image, - const std_msgs::Header& header); - void publishRectFromMaskImage(ros::Publisher& pub, - cv::Mat& image, - const std_msgs::Header& header); - //////////////////////////////////////////////////////// - // drawing helper methods - //////////////////////////////////////////////////////// - void drawLineStrip(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawLineList(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawPolygon(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawPoints(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawFrames(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawText(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawLineStrip3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawLineList3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawPolygon3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawPoints3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawText3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawCircle3D(const image_view2::ImageMarker2::ConstPtr& marker, - std::vector& colors, - std::vector::iterator& col_it); - void drawCircle(const image_view2::ImageMarker2::ConstPtr& marker); - void drawMarkers(); - void drawInteraction(); - void drawGrid(); - void drawInfo(ros::Time& before_rendering); - void resolveLocalMarkerQueue(); - bool lookupTransformation( - std::string frame_id, ros::Time& acquisition_time, - std::map& tf_fail, - tf::StampedTransform &transform); - void processMouseEvent(int event, int x, int y, int flags, void* param); - void processLeftButtonDown(int x, int y); - void processMove(int x, int y); - void processLeftButtonUp(int x, int y); - void publishMouseInteractionResult(); - void checkMousePos(int& x, int& y); - void createDistortGridImage(); - V_ImageMarkerMessage local_queue_; - image_transport::Subscriber image_sub_; - ros::Subscriber event_sub_; - ros::Subscriber info_sub_; - ros::Subscriber marker_sub_; - std::string marker_topic_; - boost::circular_buffer times_; - image_transport::Publisher image_pub_; - image_transport::Publisher local_image_pub_; - boost::shared_ptr > srv_; - - V_ImageMarkerMessage marker_queue_; - boost::mutex queue_mutex_; - boost::mutex point_array_mutex_; - sensor_msgs::ImageConstPtr last_msg_; - sensor_msgs::CameraInfoConstPtr info_msg_; - cv_bridge::CvImage img_bridge_; - boost::mutex image_mutex_; - int skip_draw_rate_; - cv::Mat original_image_, image_, draw_; - cv::Mat distort_grid_mask_; - int div_u_, div_v_; - int space_,prev_space_; - int grid_red_, grid_green_, grid_blue_, prev_red_, prev_green_, prev_blue_; - int grid_thickness_, prev_thickness_; - bool fisheye_mode_; - - tf::TransformListener tf_listener_; - image_geometry::PinholeCameraModel cam_model_; - std::vector frame_ids_; - std::vector point_array_; - boost::mutex info_mutex_; - - // for grabcut selection - bool selecting_fg_; - std::vector point_bg_array_; - std::vector point_fg_array_; - std::vector poly_points_; - cv::Point poly_selecting_point_; - bool poly_selecting_done_; - cv::Rect rect_bg_; - cv::Rect rect_fg_; - std::string window_name_; - boost::format filename_format_; - int font_; - - double resize_x_, resize_y_; - CvRect window_selection_; - cv::Point2f button_up_pos_; - int count_; - bool draw_grid_; - bool blurry_mode_; - bool show_info_; - double tf_timeout_; - bool region_continuous_publish_; - bool continuous_ready_; - bool left_button_clicked_; - ros::Publisher point_pub_; - ros::Publisher point_array_pub_; - ros::Publisher rectangle_pub_; - ros::Publisher move_point_pub_; - ros::Publisher foreground_mask_pub_; - ros::Publisher background_mask_pub_; - ros::Publisher foreground_rect_pub_; - ros::Publisher background_rect_pub_; - ros::Publisher line_pub_; - ros::Publisher poly_pub_; - KEY_MODE mode_; - bool autosize_; - bool window_initialized_; - - // for line mode interaction - boost::mutex poly_point_mutex_; - boost::mutex line_point_mutex_; - bool line_select_start_point_; - bool line_selected_; - cv::Point line_start_point_; - cv::Point line_end_point_; - // thread safe setter - void updateLineStartPoint(cv::Point p); - void updateLineEndPoint(cv::Point p); - cv::Point getLineStartPoint(); - cv::Point getLineEndPoint(); - void publishLinePoints(); - void updateLinePoint(cv::Point p); - void updatePolyPoint(cv::Point p); - void updatePolySelectingPoint(cv::Point p); - void clearPolyPoints(); - void publishPolyPoints(); - void finishSelectingPoly(); - bool isPolySelectingFirstTime(); - bool isSelectingLineStartPoint(); - void resetInteraction(); - ros::ServiceServer rectangle_mode_srv_; - ros::ServiceServer series_mode_srv_; - ros::ServiceServer grabcut_mode_srv_; - ros::ServiceServer grabcut_rect_mode_srv_; - ros::ServiceServer line_mode_srv_; - ros::ServiceServer none_mode_srv_; - ros::ServiceServer poly_mode_srv_; - ros::ServiceServer change_mode_srv_; - bool changeModeServiceCallback( - image_view2::ChangeModeRequest& req, - image_view2::ChangeModeResponse& res); - bool rectangleModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res); - bool seriesModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res); - bool grabcutModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res); - bool grabcutRectModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res); - bool lineModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res); - bool polyModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res); - bool noneModeServiceCallback( - std_srvs::EmptyRequest& req, - std_srvs::EmptyResponse& res); - cv::Point ratioPoint(double x, double y); - KEY_MODE stringToMode(const std::string& str); - }; -} - -#endif diff --git a/jsk_ros_patch/image_view2/image_view2_node.cpp b/jsk_ros_patch/image_view2/image_view2_node.cpp deleted file mode 100644 index fc6623985..000000000 --- a/jsk_ros_patch/image_view2/image_view2_node.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -*- mode: c++ -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, JSK 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. - *********************************************************************/ - -#include "image_view2.h" - -int main(int argc, char **argv) -{ - //ros::init(argc, argv, "image_view2", ros::init_options::AnonymousName); - ros::init(argc, argv, "image_view2"); - ros::NodeHandle n; - - if ( n.resolveName("image") == "/image") { - ROS_WARN("image_view: image has not been remapped! Typical command-line usage:\n" - "\t$ ./image_view image:= [transport]"); - } - ros::AsyncSpinner spinner(1); - image_view2::ImageView2 view(n); - spinner.start(); - while (ros::ok()) { - int key = cv::waitKey(1000 / 30); - view.pressKey(key); - view.showImage(); - } - return 0; -} - diff --git a/jsk_ros_patch/image_view2/mainpage.dox b/jsk_ros_patch/image_view2/mainpage.dox deleted file mode 100644 index e91916028..000000000 --- a/jsk_ros_patch/image_view2/mainpage.dox +++ /dev/null @@ -1,11 +0,0 @@ -/** -@mainpage image_view2 - -@htmlinclude manifest.html - -@b image_view is a ... - - -Currently this package has no public code API. - -*/ diff --git a/jsk_ros_patch/image_view2/msg/ImageMarker2.msg b/jsk_ros_patch/image_view2/msg/ImageMarker2.msg deleted file mode 100644 index de0912370..000000000 --- a/jsk_ros_patch/image_view2/msg/ImageMarker2.msg +++ /dev/null @@ -1,44 +0,0 @@ -byte CIRCLE=0 -byte LINE_STRIP=1 -byte LINE_LIST=2 -byte POLYGON=3 -byte POINTS=4 -byte FRAMES=5 -byte TEXT=6 - -byte LINE_STRIP3D=7 -byte LINE_LIST3D=8 -byte POLYGON3D=9 -byte POINTS3D=10 -byte TEXT3D=11 -byte CIRCLE3D=12 - -byte ADD=0 -byte REMOVE=1 - -Header header -string ns # namespace, used with id to form a unique id -int32 id # unique id within the namespace -int32 type # CIRCLE/LINE_STRIP/etc. -int32 action # ADD/REMOVE -geometry_msgs/Point position # used for CIRCLE/TEXT, 2D in pixel-coords -geometry_msgs/PointStamped position3D # used for 3DTEXT -geometry_msgs/PoseStamped pose # used for CIRCLE3D -float32 scale # the diameter for a circle, etc. -float32 width # the width for a line, etc. -std_msgs/ColorRGBA outline_color -byte filled # whether to fill in the shape with color -std_msgs/ColorRGBA fill_color # color [0.0-1.0] -duration lifetime # How long the object should last before being automatically deleted. 0 means forever -byte arc # used for CIRCLE3D -float32 angle # used for CIRCLE3D - - -geometry_msgs/Point[] points # used for LINE_STRIP/LINE_LIST/POLYGON/POINTS., 2D in pixel coords -PointArrayStamped points3D # used for 3DLINE_STRIP/3DLINE_LIST/3DPOLYGON/3DPOINTS -std_msgs/ColorRGBA[] outline_colors # a color for each line, point, etc. - -string[] frames # used for FRAMES, tf names -string text # used for TEXT, draw size of text is scale -bool left_up_origin # draw text from left up origin -bool ratio_scale #Use ratio respected to original image to specify scale and position diff --git a/jsk_ros_patch/image_view2/msg/MouseEvent.msg b/jsk_ros_patch/image_view2/msg/MouseEvent.msg deleted file mode 100644 index f47245d98..000000000 --- a/jsk_ros_patch/image_view2/msg/MouseEvent.msg +++ /dev/null @@ -1,19 +0,0 @@ -Header header - -# event type -int32 KEY_PRESSED=1 -int32 MOUSE_LEFT_UP=2 -int32 MOUSE_LEFT_DOWN=3 -int32 MOUSE_MOVE=4 -int32 MOUSE_RIGHT_DOWN=5 -int32 type - -int32 key - -# location of mouse -int32 x -int32 y - -# size of image -int32 width -int32 height \ No newline at end of file diff --git a/jsk_ros_patch/image_view2/msg/PointArrayStamped.msg b/jsk_ros_patch/image_view2/msg/PointArrayStamped.msg deleted file mode 100644 index de1dc134b..000000000 --- a/jsk_ros_patch/image_view2/msg/PointArrayStamped.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header - -geometry_msgs/Point[] points - diff --git a/jsk_ros_patch/image_view2/package.xml b/jsk_ros_patch/image_view2/package.xml deleted file mode 100644 index 66b065f19..000000000 --- a/jsk_ros_patch/image_view2/package.xml +++ /dev/null @@ -1,57 +0,0 @@ - - image_view2 - 1.0.72 - A simple viewer for ROS image topics with draw-on features - Kei Okada - - BSD - - http://ros.org/wiki/image_view2 - - - Kei Okada - - catkin - - roscpp - cv_bridge - std_msgs - sensor_msgs - geometry_msgs - image_transport - tf - image_geometry - message_filters - pcl_ros - image_view - message_generation - std_srvs - - roscpp - cv_bridge - std_msgs - sensor_msgs - geometry_msgs - image_transport - tf - image_geometry - message_filters - pcl_ros - image_view - message_runtime - std_srvs - - - - - - - - - - - - - - - diff --git a/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp b/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp deleted file mode 100644 index 89d4f13cc..000000000 --- a/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp +++ /dev/null @@ -1,152 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include - -//#include -#include -#include - -class PointsRectExtractor -{ - typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, - geometry_msgs::PolygonStamped > PolygonApproxSyncPolicy; - - typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, - geometry_msgs::PointStamped > PointApproxSyncPolicy; - -private: - ros::NodeHandle pnode_; - ros::Publisher pub_; - int queue_size; - int crop_width; - - message_filters::Subscriber < sensor_msgs::PointCloud2 > points_sub_; - message_filters::Subscriber < geometry_msgs::PolygonStamped > rect_sub_; - message_filters::Subscriber < geometry_msgs::PointStamped > point_sub_; - - boost::shared_ptr < message_filters::Synchronizer < PolygonApproxSyncPolicy > > sync_a_polygon_; - boost::shared_ptr < message_filters::Synchronizer < PointApproxSyncPolicy > > sync_a_point_; - -public: - PointsRectExtractor() : pnode_("~"), queue_size(200), crop_width(2) - { - rect_sub_.subscribe (pnode_, "rect", queue_size); - point_sub_.subscribe (pnode_, "point", queue_size); - points_sub_.subscribe (pnode_, "points", queue_size); - - sync_a_polygon_ = boost::make_shared < message_filters::Synchronizer< PolygonApproxSyncPolicy > > (queue_size); - sync_a_polygon_->connectInput (points_sub_, rect_sub_); - sync_a_polygon_->registerCallback (boost::bind (&PointsRectExtractor::callback_polygon, this, _1, _2)); - - sync_a_point_ = boost::make_shared < message_filters::Synchronizer< PointApproxSyncPolicy > > (queue_size); - sync_a_point_->connectInput (points_sub_, point_sub_); - sync_a_point_->registerCallback (boost::bind (&PointsRectExtractor::callback_point, this, _1, _2)); - - pub_ = pnode_.advertise< sensor_msgs::PointCloud2 > ("output", 1); - } - - void callback_point(const sensor_msgs::PointCloud2ConstPtr& points_ptr, - const geometry_msgs::PointStampedConstPtr& array_ptr) { - int st_x = array_ptr->point.x - crop_width; - int st_y = array_ptr->point.y - crop_width; - int ed_x = array_ptr->point.x + crop_width; - int ed_y = array_ptr->point.y + crop_width; - - int wd = points_ptr->width; - int ht = points_ptr->height; - int rstep = points_ptr->row_step; - int pstep = points_ptr->point_step; - if (st_x < 0) st_x = 0; - if (st_y < 0) st_x = 0; - if (ed_x >= wd) ed_x = wd - 1; - if (ed_y >= ht) ed_y = ht - 1; - - sensor_msgs::PointCloud2 pt; - pt.header = points_ptr->header; - pt.width = ed_x - st_x + 1; - pt.height = ed_y - st_y + 1; - pt.row_step = pt.width * pstep; - pt.point_step = pstep; - pt.is_bigendian = false; - pt.fields = points_ptr->fields; - pt.is_dense = false; - pt.data.resize(pt.row_step * pt.height); - - unsigned char * dst_ptr = &(pt.data[0]); - - for (size_t idx_y = st_y; idx_y <= ed_y; idx_y++) { - for (size_t idx_x = st_x; idx_x <= ed_x; idx_x++) { - const unsigned char * src_ptr = &(points_ptr->data[idx_y * rstep + idx_x * pstep]); - memcpy(dst_ptr, src_ptr, pstep); - dst_ptr += pstep; - } - } - pub_.publish (pt); - } - - void callback_polygon(const sensor_msgs::PointCloud2ConstPtr& points_ptr, - const geometry_msgs::PolygonStampedConstPtr& array_ptr) { - // Solve all of perception here... - ROS_DEBUG("callback"); -#if 0 - if(pub_.getNumSubscribers()==0 ){ - ROS_INFO("number of subscribers is 0, ignoring image"); - return; - } -#endif - - if (array_ptr->polygon.points.size() > 1) { - int st_x = array_ptr->polygon.points[0].x; - int st_y = array_ptr->polygon.points[0].y; - int ed_x = array_ptr->polygon.points[1].x; - int ed_y = array_ptr->polygon.points[1].y; - int wd = points_ptr->width; - int ht = points_ptr->height; - int rstep = points_ptr->row_step; - int pstep = points_ptr->point_step; - - sensor_msgs::PointCloud2 pt; - pt.header = points_ptr->header; - pt.width = ed_x - st_x + 1; - pt.height = ed_y - st_y + 1; - pt.row_step = pt.width * pstep; - pt.point_step = pstep; - pt.is_bigendian = false; - pt.fields = points_ptr->fields; - pt.is_dense = false; - pt.data.resize(pt.row_step * pt.height); - - unsigned char * dst_ptr = &(pt.data[0]); - - for (size_t idx_y = st_y; idx_y <= ed_y; idx_y++) { - for (size_t idx_x = st_x; idx_x <= ed_x; idx_x++) { - const unsigned char * src_ptr = &(points_ptr->data[idx_y * rstep + idx_x * pstep]); - memcpy(dst_ptr, src_ptr, pstep); - dst_ptr += pstep; - } - } - pub_.publish (pt); - } - } -}; - -int main (int argc, char **argv) -{ - ros::init (argc, argv, "points_rectangle_extractor"); - - if(!ros::master::check()) - return -1; - - PointsRectExtractor vs; - - ros::spin(); - - return 0; -} diff --git a/jsk_ros_patch/image_view2/scale_interaction.py b/jsk_ros_patch/image_view2/scale_interaction.py deleted file mode 100755 index 8edd87b2f..000000000 --- a/jsk_ros_patch/image_view2/scale_interaction.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python - -import rospy -from geometry_msgs.msg import Polygon, Point32, PolygonStamped -from std_msgs.msg import Float32 -from threading import Lock - -width_scale = None -height_scale = None -pending_polygon = None -lock = Lock() - -def widthScallCallback(msg): - global width_scale - with lock: - if msg.data == 0: - rospy.logdebug("scale is 0!, we skip it") - else: - width_scale = msg.data - updateForPendingData() - -def heightScallCallback(msg): - global height_scale - with lock: - if msg.data == 0: - rospy.logdebug("scale is 0!, we skip it") - else: - height_scale = msg.data - updateForPendingData() - -def polygonCallback(msg): - global pending_polygon - with lock: - if width_scale and height_scale: - publishResizedPolygon(msg) - else: - pending_polygon = msg - -def updateForPendingData(): - global pending_polygon, width_scale, height_scale - if (pending_polygon and width_scale and height_scale - and width_scale != 0 and height_scale != 0): - publishResizedPolygon(pending_polygon) - pending_polygon = None - -def publishResizedPolygon(poly_msg): - global width_scale, height_scale, inverse, pub - if inverse: - Rw = 1.0 / width_scale - Rh = 1.0 / height_scale - else: - Rw = width_scale - Rh = height_scale - for point in poly_msg.polygon.points: - point.x = Rw * point.x - point.y = Rh * point.y - pub.publish(poly_msg) - -rospy.init_node("scale_interaction") - -inverse = rospy.get_param("~inverse", False) -pub = rospy.Publisher("~output", PolygonStamped) -sub_polygon = rospy.Subscriber("~input", PolygonStamped, polygonCallback) -sub_width = rospy.Subscriber("~input/width_scale", Float32, widthScallCallback) -sub_height = rospy.Subscriber("~input/height_scale", Float32, heightScallCallback) - -rospy.spin() diff --git a/jsk_ros_patch/image_view2/srv/ChangeMode.srv b/jsk_ros_patch/image_view2/srv/ChangeMode.srv deleted file mode 100644 index b88186743..000000000 --- a/jsk_ros_patch/image_view2/srv/ChangeMode.srv +++ /dev/null @@ -1,2 +0,0 @@ -string mode ---- diff --git a/jsk_ros_patch/image_view2/test-imageview2.l b/jsk_ros_patch/image_view2/test-imageview2.l deleted file mode 100755 index 0d4934abb..000000000 --- a/jsk_ros_patch/image_view2/test-imageview2.l +++ /dev/null @@ -1,150 +0,0 @@ -#!/usr/bin/env roseus - -#| -;; -;; imageview2 sample program -;; -;; before invoke this script, first launch following programs -;; -roslaunch roseus_tutorials usb-camera.launch -roslaunch roseus_tutorials image-view.launch -rosrun tf static_transform_publisher 0 0 1 0 0 0 /camera /target 10 -|# - -(ros::roseus-add-msgs "geometry_msgs") -(ros::roseus-add-msgs "image_view2") -;;; -;;; -(ros::roseus "imageview2-client") - -(ros::advertise "image_marker" image_view2::ImageMarker2 1) - -(setq i 0) -(ros::rate 10) -(while (ros::ok) - (let ((mrk (instance image_view2::ImageMarker2 :init))) - (case (mod (/ i 10) 15) - (0 - (send mrk :type image_view2::ImageMarker2::*CIRCLE*) - (send mrk :id 5) - (send mrk :position (instance geometry_msgs::Point :init :x 320 :y 240)) - (send mrk :outline_color (instance std_msgs::ColorRGBA :init :r 0.0 :g 1.0 :b 0.0 :a 1.0)) - (send mrk :lifetime (ros::Time 10)) - (send mrk :scale 100.0) - ) - (1 - (send mrk :type image_view2::ImageMarker2::*LINE_STRIP*) - (send mrk :points (list (instance geometry_msgs::Point :init :x 200 :y 150) - (instance geometry_msgs::Point :init :x 440 :y 150) - (instance geometry_msgs::Point :init :x 440 :y 330) - (instance geometry_msgs::Point :init :x 200 :y 330))) - (send mrk :outline_colors (list (instance std_msgs::ColorRGBA :init :r 0.0 :g 1.0 :b 0.0 :a 1.0) - (instance std_msgs::ColorRGBA :init :r 0.0 :g 0.0 :b 1.0 :a 1.0))) - ) - (2 - (send mrk :type image_view2::ImageMarker2::*LINE_LIST*) - (send mrk :points (list (instance geometry_msgs::Point :init :x 200 :y 150) - (instance geometry_msgs::Point :init :x 440 :y 150) - (instance geometry_msgs::Point :init :x 440 :y 330) - (instance geometry_msgs::Point :init :x 200 :y 330))) - ) - (3 - (send mrk :type image_view2::ImageMarker2::*POLYGON*) - (send mrk :points (list (instance geometry_msgs::Point :init :x 200 :y 150) - (instance geometry_msgs::Point :init :x 440 :y 150) - (instance geometry_msgs::Point :init :x 440 :y 330) - (instance geometry_msgs::Point :init :x 200 :y 330))) - ) - (4 - (send mrk :type image_view2::ImageMarker2::*POINTS*) - (send mrk :points (list (instance geometry_msgs::Point :init :x 320 :y 240))) - (send mrk :scale 10.0) - ) - (5 - (send mrk :action image_view2::ImageMarker2::*REMOVE*) - (send mrk :id -1) - ) - (6 - (send mrk :type image_view2::ImageMarker2::*FRAMES*) - (send mrk :frames (list "/target")) - ) - (7 - (send mrk :type image_view2::ImageMarker2::*TEXT*) - (send mrk :position (instance geometry_msgs::Point :init :x 320 :y 240)) - (send mrk :scale 0.5) - (send mrk :text "Hello World!") - ) - (8 - (send mrk :header :stamp (ros::time-now)) - (send mrk :type image_view2::ImageMarker2::*LINE_STRIP3D*) - (send mrk :points3D :header :frame_id "base_link") - (send mrk :points3D :points (list (instance geometry_msgs::Point :init :x 1.5 :y 0.5 :z 2.0) - (instance geometry_msgs::Point :init :x 1.5 :y -0.5 :z 2.0) - (instance geometry_msgs::Point :init :x 1.5 :y -0.5 :z 1.0) - (instance geometry_msgs::Point :init :x 1.5 :y 0.5 :z 1.0))) - (send mrk :outline_colors (list (instance std_msgs::ColorRGBA :init :r 0.0 :g 1.0 :b 0.0 :a 1.0) - (instance std_msgs::ColorRGBA :init :r 0.0 :g 0.0 :b 1.0 :a 1.0))) - ) - (9 - (send mrk :header :stamp (ros::time-now)) - (send mrk :type image_view2::ImageMarker2::*LINE_LIST3D*) - (send mrk :points3D :header :frame_id "base_link") - (send mrk :points3D :points (list (instance geometry_msgs::Point :init :x 1.5 :y 0.5 :z 2.0) - (instance geometry_msgs::Point :init :x 1.5 :y -0.5 :z 2.0) - (instance geometry_msgs::Point :init :x 1.5 :y -0.5 :z 1.0) - (instance geometry_msgs::Point :init :x 1.5 :y 0.5 :z 1.0))) - ) - (10 - (send mrk :header :stamp (ros::time-now)) - (send mrk :type image_view2::ImageMarker2::*POLYGON3D*) - (send mrk :points3D :header :frame_id "base_link") - (send mrk :points3D :points (list (instance geometry_msgs::Point :init :x 1.5 :y 0.5 :z 2.0) - (instance geometry_msgs::Point :init :x 1.5 :y -0.5 :z 2.0) - (instance geometry_msgs::Point :init :x 1.5 :y -0.5 :z 2.0) - (instance geometry_msgs::Point :init :x 1.5 :y 0.5 :z 2.0))) - ) - (11 - (send mrk :header :stamp (ros::time-now)) - (send mrk :type image_view2::ImageMarker2::*POINTS3D*) - (send mrk :points3D :header :frame_id "base_link") - (send mrk :points3D :points (list (instance geometry_msgs::Point :init :x 1.5 :y 0.0 :z 0.8) - (instance geometry_msgs::Point :init :x 1.5 :y 0.0 :z 1.0) - (instance geometry_msgs::Point :init :x 1.5 :y 0.0 :z 1.2) - (instance geometry_msgs::Point :init :x 1.5 :y 0.0 :z 1.4) - (instance geometry_msgs::Point :init :x 1.5 :y 0.0 :z 1.6) - (instance geometry_msgs::Point :init :x 1.5 :y 0.0 :z 1.8) - (instance geometry_msgs::Point :init :x 1.5 :y 0.0 :z 2.0))) - (send mrk :scale 10.0) - ) - (12 - (send mrk :action image_view2::ImageMarker2::*REMOVE*) - (send mrk :id -1) - ) - (13 - (send mrk :header :stamp (ros::time-now)) - (send mrk :type image_view2::ImageMarker2::*TEXT3D*) - (send mrk :position3D :header :frame_id "base_link") - (send mrk :position3D :point (instance geometry_msgs::Point :init :x 1.0 :y 0.0 :z 1.5)) - (send mrk :scale 1.0) - (send mrk :text "Hello World!") - ) - (14 - (send mrk :header :stamp (ros::time-now)) - (send mrk :type image_view2::ImageMarker2::*CIRCLE3D*) - (send mrk :pose :header :frame_id "base_link") - (send mrk :pose :pose :position (instance geometry_msgs::Point :init :x 1.0 :y 0.0 :z 1.5)) - (send mrk :pose :pose :orientation (instance geometry_msgs::Quaternion :init :x 0.0 :y 0.0 :z 0.0 :w 1.0)) - (send mrk :outline_color (instance std_msgs::ColorRGBA :init :r 1.0 :g 0.0 :b 0.0 :a 1.0)) - (send mrk :fill_color (instance std_msgs::ColorRGBA :init :r 0.0 :g 0.0 :b 1.0 :a 1.0)) - (send mrk :filled 1) - (send mrk :arc 1) - (send mrk :angle 30.0) - (send mrk :scale 0.5) - ) - (t - (warn "unknwon case"))) - (ros::publish "image_marker" mrk) - (ros::ros-info "type ~A id ~A" (send mrk :type) (send mrk :id)) - (ros::spin-once) - (ros::sleep) - (incf i))) diff --git a/jsk_ros_patch/image_view2/test-toolframe.l b/jsk_ros_patch/image_view2/test-toolframe.l deleted file mode 100755 index f32a1c5f1..000000000 --- a/jsk_ros_patch/image_view2/test-toolframe.l +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/env roseus -;; -;; FOR PR2 USER ONLY -;; for non-pr2 users, use test-imageview2.l -;; -;; rosrun image_view2 image_view2 image:=/wide_stereo/left/image_raw camera_info:=/wide_stereo/left/camera_info -;; -(ros::roseus-add-msgs "geometry_msgs") -(ros::roseus-add-msgs "image_view2") -;;; -;;; -(ros::roseus "imageview2-client") - -(ros::advertise "image_marker" image_view2::ImageMarker2 1) - -(ros::rate 10) -(while (ros::ok) - (let ((mrk (instance image_view2::ImageMarker2 :init))) - (send mrk :header :stamp (ros::time-now)) - (send mrk :type image_view2::ImageMarker2::*FRAMES*) - (send mrk :frames (list "/r_gripper_tool_frame" "/l_gripper_tool_frame")) - (ros::publish "image_marker" mrk) - (print (list (send mrk :header :stamp) (send mrk :frames))) - (ros::spin-once) - (ros::sleep))) - - diff --git a/jsk_ros_patch/multi_map_server/CHANGELOG.rst b/jsk_ros_patch/multi_map_server/CHANGELOG.rst deleted file mode 100644 index 40099baee..000000000 --- a/jsk_ros_patch/multi_map_server/CHANGELOG.rst +++ /dev/null @@ -1,242 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package multi_map_server -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.0.72 (2015-06-07) -------------------- - -1.0.71 (2015-05-17) -------------------- - -1.0.70 (2015-05-08) -------------------- - -1.0.69 (2015-05-05) -------------------- - -1.0.68 (2015-05-05) -------------------- - -1.0.67 (2015-05-03) -------------------- - -1.0.66 (2015-04-03) -------------------- - -1.0.65 (2015-04-02) -------------------- - -1.0.64 (2015-03-29) -------------------- - -1.0.63 (2015-02-19) -------------------- - -1.0.62 (2015-02-17) -------------------- - -1.0.61 (2015-02-11) -------------------- - -1.0.60 (2015-02-03) -------------------- - -1.0.59 (2015-02-03) -------------------- -* Remove rosbuild files -* Contributors: Ryohei Ueda - -1.0.58 (2015-01-07) -------------------- - -1.0.57 (2014-12-23) -------------------- - -1.0.56 (2014-12-17) -------------------- - -1.0.55 (2014-12-09) -------------------- - -1.0.54 (2014-11-15) -------------------- - -1.0.53 (2014-11-01) -------------------- - -1.0.52 (2014-10-23) -------------------- - -1.0.51 (2014-10-20) -------------------- - -1.0.50 (2014-10-20) -------------------- - -1.0.49 (2014-10-13) -------------------- - -1.0.48 (2014-10-12) -------------------- - -1.0.47 (2014-10-08) -------------------- - -1.0.46 (2014-10-03) -------------------- - -1.0.45 (2014-09-29) -------------------- - -1.0.44 (2014-09-26) -------------------- - -1.0.43 (2014-09-26) -------------------- - -1.0.42 (2014-09-25) -------------------- -* Rename executable of multi_map_server to avoid conflict against map_server -* Contributors: Ryohei Ueda - -1.0.41 (2014-09-23) -------------------- - -1.0.40 (2014-09-19) -------------------- - -1.0.39 (2014-09-17) -------------------- - -1.0.38 (2014-09-13) -------------------- -* catkinize python_twoauth and voice_text, modify multi_map_server's catkin.cmake -* Contributors: Yuto Inagaki - -1.0.36 (2014-09-01) -------------------- -* modify to install executable to right path -* Contributors: Yuto Inagaki - -1.0.35 (2014-08-16) -------------------- - -1.0.34 (2014-08-14) -------------------- - -1.0.33 (2014-07-28) -------------------- - -1.0.32 (2014-07-26) -------------------- - -1.0.31 (2014-07-23) -------------------- - -1.0.30 (2014-07-15) -------------------- - -1.0.29 (2014-07-02) -------------------- - -1.0.28 (2014-06-24) -------------------- - -1.0.27 (2014-06-10) -------------------- - -1.0.26 (2014-05-30) -------------------- -* enable depend to map_server for default -* Contributors: Kei Okada - -1.0.25 (2014-05-26) -------------------- - -1.0.24 (2014-05-24) -------------------- - -1.0.23 (2014-05-23) -------------------- - -1.0.22 (2014-05-22) -------------------- - -1.0.21 (2014-05-20) -------------------- - -1.0.20 (2014-05-09) -------------------- - -1.0.19 (2014-05-06) -------------------- - -1.0.18 (2014-05-04) -------------------- - -1.0.17 (2014-04-20) -------------------- - -1.0.16 (2014-04-19) -------------------- - -1.0.15 (2014-04-19) -------------------- - -1.0.14 (2014-04-19) -------------------- - -1.0.13 (2014-04-19) -------------------- - -1.0.12 (2014-04-18) -------------------- - -1.0.11 (2014-04-18) -------------------- - -1.0.10 (2014-04-17) -------------------- - -1.0.9 (2014-04-12) ------------------- - -1.0.8 (2014-04-11) ------------------- - -1.0.6 (2014-04-07) ------------------- -* Added support for YAML-CPP 0.5+. - The new yaml-cpp API removes the "node >> outputvar;" operator, and - it has a new way of loading documents. There's no version hint in the - library's headers, so I'm getting the version number from pkg-config. - This is a port of @ktossell's patch for map_server. -* Contributors: Scott K Logan - -1.0.5 (2014-03-31) ------------------- -* check if map_server exists under /opt/ros/{ROS_DISTRO}/stacks/navigation/map_server for groovy -* Contributors: Kei Okada - -1.0.4 (2014-03-27) ------------------- -* multi_map_server: disable map_server for default in build_depend, run_depend -* fix typo CATKIN-DEPENDS -> CATKIN_DEPENDS -* multi_map_server: catkinize -* Contributors: Ryohei Ueda, Kei Okada - -1.0.3 (2014-03-19) ------------------- -* update revision number to 1.0.3 - -1.0.2 (2014-03-12) ------------------- - -1.0.1 (2014-03-07) ------------------- - -1.0.0 (2014-03-05) ------------------- -* add explicit dependency to yaml-cpp as yaml-cpp i sinstalled as a rosdep system dependency -* add multi_map_server, map_server with switch service, (this will publish TF between maps in the future) -* Contributors: Kei Okada, Manabu Saito diff --git a/jsk_ros_patch/multi_map_server/CMakeLists.txt b/jsk_ros_patch/multi_map_server/CMakeLists.txt deleted file mode 100644 index 4c0ab0c84..000000000 --- a/jsk_ros_patch/multi_map_server/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html -cmake_minimum_required(VERSION 2.8.3) -project(multi_map_server) -find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rospy nav_msgs tf) - -# if not found map_server -find_package(map_server QUIET) -if(NOT map_server_FOUND) - if(EXISTS /opt/ros/$ENV{ROS_DISTRO}/stacks/navigation/map_server) - set(map_server_PREFIX /opt/ros/$ENV{ROS_DISTRO}/stacks/navigation/map_server) - include_directories(${map_server_PREFIX}/include) - endif() -endif() -if(NOT map_server_PREFIX) - link_directories(${map_server_PREFIX}/lib) - find_package(jsk_tools) - download_package_for_groovy(map_server http://github.com/ros-planning/navigation groovy) -endif() - -find_package(PkgConfig) -pkg_check_modules(NEW_YAMLCPP yaml-cpp>=0.5) -if(NEW_YAMLCPP_FOUND) -add_definitions(-DHAVE_NEW_YAMLCPP) -endif(NEW_YAMLCPP_FOUND) - -catkin_package( - DEPENDS rosconsole roscpp rospy nav_msgs tf - CATKIN_DEPENDS - INCLUDE_DIRS - LIBRARIES -) - -link_directories(${map_server_PREFIX}/lib) -add_executable(multi_map_server src/main.cpp) -target_link_libraries(multi_map_server ${catkin_LIBRARIES} image_loader SDL SDL_image yaml-cpp) - -install(TARGETS multi_map_server - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - - - diff --git a/jsk_ros_patch/multi_map_server/package.xml b/jsk_ros_patch/multi_map_server/package.xml deleted file mode 100644 index b050b92ab..000000000 --- a/jsk_ros_patch/multi_map_server/package.xml +++ /dev/null @@ -1,45 +0,0 @@ - - multi_map_server - 1.0.72 - multi_map_server provides the - Kei Okada - - BSD - - http://ros.org/wiki/map_server - - - Manabu Saito - - catkin - - map_server - nav_msgs - - - rosconsole - roscpp - rospy - tf - - python-imaging - python-yaml - yaml-cpp - sdl-image - - jsk_tools - rosmake - - map_server - nav_msgs - - rosconsole - roscpp - rospy - tf - yaml-cpp - sdl-image - - - - diff --git a/jsk_ros_patch/multi_map_server/src/main.cpp b/jsk_ros_patch/multi_map_server/src/main.cpp deleted file mode 100644 index 45fbd9f91..000000000 --- a/jsk_ros_patch/multi_map_server/src/main.cpp +++ /dev/null @@ -1,268 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. 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. - */ - -/* Author: Brian Gerkey */ - -#define USAGE "\nUSAGE: map_server \n" \ - " map.yaml: map description file\n" \ - "DEPRECATED USAGE: map_server \n" \ - " map: image file to load\n"\ - " resolution: map resolution [meters/pixel]" - -#include -#include -#include -#include - -#include "ros/ros.h" -#include "ros/console.h" -#include "map_server/image_loader.h" -#include "std_msgs/String.h" -#include "nav_msgs/MapMetaData.h" -#include "yaml-cpp/yaml.h" - -#ifdef HAVE_NEW_YAMLCPP -// The >> operator disappeared in yaml-cpp 0.5, so this function is -// added to provide support for code written under the yaml-cpp 0.3 API. -template -void operator >> (const YAML::Node& node, T& i) -{ - i = node.as(); -} -#endif - -class MapServer -{ - public: - /** Trivial constructor */ - MapServer(const std::string& fname, double res) - { - std::string mapfname = ""; - double origin[3]; - int negate; - double occ_th, free_th; - std::string frame_id; - ros::NodeHandle private_nh("~"); - private_nh.param("frame_id", frame_id, std::string("map")); - deprecated = (res != 0); - if (!deprecated) { - //mapfname = fname + ".pgm"; - //std::ifstream fin((fname + ".yaml").c_str()); - std::ifstream fin(fname.c_str()); - if (fin.fail()) { - ROS_ERROR("Map_server could not open %s.", fname.c_str()); - exit(-1); - } -#ifdef HAVE_NEW_YAMLCPP - // The document loading process changed in yaml-cpp 0.5. - YAML::Node doc = YAML::Load(fin); -#else - YAML::Parser parser(fin); - YAML::Node doc; - parser.GetNextDocument(doc); -#endif - try { - doc["resolution"] >> res; - } catch (YAML::InvalidScalar) { - ROS_ERROR("The map does not contain a resolution tag or it is invalid."); - exit(-1); - } - try { - doc["negate"] >> negate; - } catch (YAML::InvalidScalar) { - ROS_ERROR("The map does not contain a negate tag or it is invalid."); - exit(-1); - } - try { - doc["occupied_thresh"] >> occ_th; - } catch (YAML::InvalidScalar) { - ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid."); - exit(-1); - } - try { - doc["free_thresh"] >> free_th; - } catch (YAML::InvalidScalar) { - ROS_ERROR("The map does not contain a free_thresh tag or it is invalid."); - exit(-1); - } - try { - doc["map_id"] >> map_id; - } catch (YAML::InvalidScalar) { - ROS_WARN("The map does not contain a map_id tag or it is invalid."); - map_id = fname; - } - try { - doc["origin"][0] >> origin[0]; - doc["origin"][1] >> origin[1]; - doc["origin"][2] >> origin[2]; - } catch (YAML::InvalidScalar) { - ROS_ERROR("The map does not contain an origin tag or it is invalid."); - exit(-1); - } - try { - doc["image"] >> mapfname; - // TODO: make this path-handling more robust - if(mapfname.size() == 0) - { - ROS_ERROR("The image tag cannot be an empty string."); - exit(-1); - } - if(mapfname[0] != '/') - { - // dirname can modify what you pass it - char* fname_copy = strdup(fname.c_str()); - mapfname = std::string(dirname(fname_copy)) + '/' + mapfname; - free(fname_copy); - } - } catch (YAML::InvalidScalar) { - ROS_ERROR("The map does not contain an image tag or it is invalid."); - exit(-1); - } - } else { - private_nh.param("negate", negate, 0); - private_nh.param("occupied_thresh", occ_th, 0.65); - private_nh.param("free_thresh", free_th, 0.196); - mapfname = fname; - origin[0] = origin[1] = origin[2] = 0.0; - } - - ROS_INFO("Loading map from image \"%s\"", mapfname.c_str()); - map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin); - map_resp_.map.info.map_load_time = ros::Time::now(); - map_resp_.map.header.frame_id = frame_id; - map_resp_.map.header.stamp = ros::Time::now(); - ROS_INFO("Read a %d X %d map @ %.3lf m/cell", - map_resp_.map.info.width, - map_resp_.map.info.height, - map_resp_.map.info.resolution); - meta_data_message_ = map_resp_.map.info; - - // Latched publisher for metadata - metadata_pub= n.advertise("map_metadata", 1, true); - metadata_pub.publish( meta_data_message_ ); - - } - - void SwitchPublisher(bool up) { - if( up ) { - // Latched publisher for data - map_pub = n.advertise("map", 1, true); - map_pub.publish( map_resp_.map ); - } else { - map_pub.shutdown(); - } - } - - /** Callback invoked when someone requests our service */ - bool mapCallback(nav_msgs::GetMap::Request &req, - nav_msgs::GetMap::Response &res ) - { - // request is empty; we ignore it - - // = operator is overloaded to make deep copy (tricky!) - res = map_resp_; - ROS_INFO("Sending map"); - - return true; - } - - std::string map_id; - - private: - ros::NodeHandle n; - ros::Publisher metadata_pub; - ros::Publisher map_pub; - bool deprecated; - - /** The map data is cached here, to be sent out to service callers - */ - nav_msgs::MapMetaData meta_data_message_; - nav_msgs::GetMap::Response map_resp_; - - /* - void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub) - { - pub.publish( meta_data_message_ ); - } - */ - -}; - -std::map< std::string, MapServer > msv; -std::string current_map; - -bool mapCallback(nav_msgs::GetMap::Request &req, - nav_msgs::GetMap::Response &res ) { - if(msv.find(current_map) != msv.end()) { - msv.find(current_map)->second.mapCallback(req,res); - } - return true; -} - -void MapSelect(const std_msgs::StringConstPtr& msg) { - if(msv.find(msg->data) != msv.end()) { - current_map = msg->data; - msv.find(current_map)->second.SwitchPublisher(true); - ROS_INFO("map[%s] is available.", msg->data.c_str()); - } -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "map_server", ros::init_options::AnonymousName); - - ros::NodeHandle n; - ros::ServiceServer service; - ros::Subscriber sub; - service = n.advertiseService("static_map", &mapCallback); - sub = n.subscribe("/map_reload", 1, MapSelect); - - try - { - for(int i=1; i < argc; i++) { - MapServer ms = MapServer(argv[i],0.0); - std::string id = ms.map_id; - msv.insert(make_pair(id, ms)); - if(i==1) { - current_map = id; - msv.find(id)->second.SwitchPublisher(true); - } - } - - ros::spin(); - } - catch(std::runtime_error& e) - { - ROS_ERROR("map_server exception: %s", e.what()); - return -1; - } - - return 0; -} - From 2b05913105d40f1e178d28c8cfe558be5aa8ab2d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 12 Jun 2015 16:31:09 +0900 Subject: [PATCH 2/4] add jsk_3rdparty meta package --- jsk_3rdprty/CMakeLists.txt | 4 ++++ jsk_3rdprty/package.xml | 39 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+) create mode 100644 jsk_3rdprty/CMakeLists.txt create mode 100644 jsk_3rdprty/package.xml diff --git a/jsk_3rdprty/CMakeLists.txt b/jsk_3rdprty/CMakeLists.txt new file mode 100644 index 000000000..781717721 --- /dev/null +++ b/jsk_3rdprty/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_3rdparty) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/jsk_3rdprty/package.xml b/jsk_3rdprty/package.xml new file mode 100644 index 000000000..d2cec243e --- /dev/null +++ b/jsk_3rdprty/package.xml @@ -0,0 +1,39 @@ + + + jsk_3rdparty + 1.0.72 + +

Metapackage that contains commonly used 3rdparty toolset for jsk-ros-pkg

+ + + Kei Okada + Kei Okada + + BSD + http://ros.org/wiki/jsk_3rdparty + https://github.com/jsk-ros-pkg/jsk_3rdparty + https://github.com/jsk-ros-pkg/jsk_3rdparty/issues + + catkin + + opt_camera + rosping + rospatlite + nlopt + voice_text + sklearn + assimp_devel + ff + mini_maxwell + libsiftfast + ffha + downward + rostwitter + bayesian_belief_networks + julius + collada_urdf_jsk_patch + laser_filters_jsk_patch + + + + From c59d67fe13ca81dc8b13f62e03a5c0f9865cb376 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 12 Jun 2015 16:44:01 +0900 Subject: [PATCH 3/4] [.travis.yml] add .travis and .travis.yml --- .gitmodules | 3 +++ .travis | 1 + .travis.yml | 16 ++++++++++++++++ 3 files changed, 20 insertions(+) create mode 100644 .gitmodules create mode 160000 .travis create mode 100644 .travis.yml diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..e24492940 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule ".travis"] + path = .travis + url = git://github.com/jsk-ros-pkg/jsk_travis diff --git a/.travis b/.travis new file mode 160000 index 000000000..889eedc4c --- /dev/null +++ b/.travis @@ -0,0 +1 @@ +Subproject commit 889eedc4c759170c9b0021b5aa1d5a1721051c67 diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 000000000..2154009a7 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,16 @@ +language: + - cpp + - python +python: + - "2.7" +compiler: + - gcc +sudo: required +env: + - ROS_DISTRO=hydro ROSWS=wstool BUILDER=catkin USE_DEB=true ROS_PARALLEL_JOBS="-j2 -l2" + - ROS_DISTRO=indigo ROSWS=wstool BUILDER=catkin USE_DEB=true ROS_PARALLEL_JOBS="-j8" +script: source .travis/travis.sh +notifications: + email: + on_success: always + on_failure: always From 733acd80f3063e34ae2911db30a028f8ddd6fce7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 12 Jun 2015 16:59:39 +0900 Subject: [PATCH 4/4] [ff/package.xml] add mk to build_depend --- 3rdparty/ff/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/3rdparty/ff/package.xml b/3rdparty/ff/package.xml index 77d0eb481..e057fcdc5 100644 --- a/3rdparty/ff/package.xml +++ b/3rdparty/ff/package.xml @@ -14,6 +14,7 @@ catkin + mk flex bison unzip