Skip to content

Commit

Permalink
Removing image_view node and replacing with image_view that loads nod…
Browse files Browse the repository at this point in the history 
…elet. (#479)
  • Loading branch information
Joshua Whitley authored Nov 25, 2019
1 parent 1cdc128 commit 5d81e51
Showing 1 changed file with 9 additions and 184 deletions.
193 changes: 9 additions & 184 deletions image_view/src/nodes/image_view.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* Copyright (c) 2008, 2019 Willow Garage, Inc., Joshua Whitley
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
Expand Down Expand Up @@ -31,197 +31,22 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <image_view/ImageViewConfig.h>

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>
#include <nodelet/loader.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <boost/format.hpp>
#include <boost/thread.hpp>
#include <boost/filesystem.hpp>

int g_count;
cv::Mat g_last_image;
boost::format g_filename_format;
boost::mutex g_image_mutex;
std::string g_window_name;
bool g_gui;
ros::Publisher g_pub;
bool g_do_dynamic_scaling;
int g_colormap;
double g_min_image_value;
double g_max_image_value;

void reconfigureCb(image_view::ImageViewConfig &config, uint32_t level)
{
boost::mutex::scoped_lock lock(g_image_mutex);
g_do_dynamic_scaling = config.do_dynamic_scaling;
g_colormap = config.colormap;
g_min_image_value = config.min_image_value;
g_max_image_value = config.max_image_value;
}

void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
boost::mutex::scoped_lock lock(g_image_mutex);

// Convert to OpenCV native BGR color
cv_bridge::CvImageConstPtr cv_ptr;
try {
cv_bridge::CvtColorForDisplayOptions options;
options.do_dynamic_scaling = g_do_dynamic_scaling;
options.colormap = g_colormap;
// Set min/max value for scaling to visualize depth/float image.
if (g_min_image_value == g_max_image_value) {
// Not specified by rosparam, then set default value.
// Because of current sensor limitation, we use 10m as default of max range of depth
// with consistency to the configuration in rqt_image_view.
options.min_image_value = 0;
if (msg->encoding == "32FC1") {
options.max_image_value = 10; // 10 [m]
} else if (msg->encoding == "16UC1") {
options.max_image_value = 10 * 1000; // 10 * 1000 [mm]
}
} else {
options.min_image_value = g_min_image_value;
options.max_image_value = g_max_image_value;
}
cv_ptr = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options);
g_last_image = cv_ptr->image;
} catch (cv_bridge::Exception& e) {
ROS_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
msg->encoding.c_str(), e.what());
}
if (g_gui && !g_last_image.empty()) {
const cv::Mat &image = g_last_image;
cv::imshow(g_window_name, image);
cv::waitKey(1);
}
if (g_pub.getNumSubscribers() > 0) {
g_pub.publish(cv_ptr);
}
}

static void mouseCb(int event, int x, int y, int flags, void* param)
{
if (event == cv::EVENT_LBUTTONDOWN) {
ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
return;
} else if (event != cv::EVENT_RBUTTONDOWN) {
return;
}

boost::mutex::scoped_lock lock(g_image_mutex);

const cv::Mat &image = g_last_image;

if (image.empty()) {
ROS_WARN("Couldn't save image, no data!");
return;
}

std::string filename = (g_filename_format % g_count).str();
if (cv::imwrite(filename, image)) {
ROS_INFO("Saved image %s", filename.c_str());
g_count++;
} else {
boost::filesystem::path full_path = boost::filesystem::complete(filename);
ROS_ERROR_STREAM("Failed to save image. Have permission to write there?: " << full_path);
}
}

static void guiCb(const ros::WallTimerEvent&)
{
// Process pending GUI events and return immediately
cv::waitKey(1);
}

int main(int argc, char **argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_view", ros::init_options::AnonymousName);
if (ros::names::remap("image") == "image") {
ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
"\t$ rosrun image_view image_view image:=<image topic> [transport]");
}

ros::NodeHandle nh;
ros::NodeHandle local_nh("~");
ros::WallTimer gui_timer;

// Default window name is the resolved topic name
std::string topic = nh.resolveName("image");
local_nh.param("window_name", g_window_name, topic);
local_nh.param("gui", g_gui, true); // gui/no_gui mode

if (g_gui) {
std::string format_string;
local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
g_filename_format.parse(format_string);

// Handle window size
bool autosize;
local_nh.param("autosize", autosize, false);
cv::namedWindow(g_window_name, autosize ? (cv::WINDOW_AUTOSIZE | cv::WINDOW_KEEPRATIO | cv::WINDOW_GUI_EXPANDED) : 0);
cv::setMouseCallback(g_window_name, &mouseCb);

if(autosize == false)
{
if(local_nh.hasParam("width") && local_nh.hasParam("height"))
{
int width;
local_nh.getParam("width", width);
int height;
local_nh.getParam("height", height);
cv::resizeWindow(g_window_name, width, height);
}
}

// Since cv::startWindowThread() triggers a crash in cv::waitKey()
// if OpenCV is compiled against GTK, we call cv::waitKey() from
// the ROS event loop periodically, instead.
/*cv::startWindowThread();*/
gui_timer = local_nh.createWallTimer(ros::WallDuration(0.1), guiCb);
}
ros::init(argc, argv, "image_view");

// Handle transport
// priority:
// 1. command line argument
// 2. rosparam '~image_transport'
std::string transport;
local_nh.param("image_transport", transport, std::string("raw"));
ros::V_string myargv;
ros::removeROSArgs(argc, argv, myargv);
for (size_t i = 1; i < myargv.size(); ++i) {
if (myargv[i][0] != '-') {
transport = myargv[i];
break;
}
}
ROS_INFO_STREAM("Using transport \"" << transport << "\"");
image_transport::ImageTransport it(nh);
image_transport::TransportHints hints(transport, ros::TransportHints(), local_nh);
image_transport::Subscriber sub = it.subscribe(topic, 1, imageCb, hints);
g_pub = local_nh.advertise<sensor_msgs::Image>("output", 1);
nodelet::Loader nodelet;
nodelet::M_string remap(ros::names::getRemappings());
nodelet::V_string nargv;

dynamic_reconfigure::Server<image_view::ImageViewConfig> srv;
dynamic_reconfigure::Server<image_view::ImageViewConfig>::CallbackType f =
boost::bind(&reconfigureCb, _1, _2);
srv.setCallback(f);
std::string nodelet_name = ros::this_node::getName();
nodelet.load(nodelet_name, "image_view/image", remap, nargv);

ros::spin();

if (g_gui) {
cv::destroyWindow(g_window_name);
}
// The publisher is a global variable, and therefore its scope exceeds those
// of the node handles in main(). Unfortunately, this will cause a crash
// when the publisher tries to shut down and all node handles are gone
// already. Therefore, we shut down the publisher now and avoid the annoying
// mutex assertion.
g_pub.shutdown();
return 0;
}

0 comments on commit 5d81e51

Please sign in to comment.