diff --git a/src/image_transport_snippets.xml b/src/image_transport_snippets.xml new file mode 100644 index 0000000..738407e --- /dev/null +++ b/src/image_transport_snippets.xml @@ -0,0 +1,22 @@ + + + Image Transport Snippets + Collection of snippets for using image transport + kinetic + cv_bridge + image_transport + image_geometry + roscpp + 2 + + cpp + rosrun image_transport_snippets camera_subscriber_snippet + src/camera_subscriber_snippet.cpp + + + terminal + + + nodes + + diff --git a/src/image_transport_snippets/CMakeLists.txt b/src/image_transport_snippets/CMakeLists.txt new file mode 100644 index 0000000..cc81890 --- /dev/null +++ b/src/image_transport_snippets/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 2.8.3) +project(image_transport_snippets) + +add_definitions(-std=c++11) +find_package(catkin REQUIRED COMPONENTS + cv_bridge + image_transport + image_geometry + roscpp + tf2_ros +) + +catkin_package( +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(camera_subscriber_snippet src/camera_subscriber_snippet.cpp) +target_link_libraries(camera_subscriber_snippet ${catkin_LIBRARIES}) + +## Install C++ Examples +install(TARGETS camera_subscriber_snippet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Install Other Resources +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/src/image_transport_snippets/package.xml b/src/image_transport_snippets/package.xml new file mode 100644 index 0000000..de39481 --- /dev/null +++ b/src/image_transport_snippets/package.xml @@ -0,0 +1,20 @@ + + + image_transport_snippets + 0.0.0 + Code snippets for nodes using image_transport + + Steffen Fuchs + Steffen Fuchs + BSD + + + catkin + + cv_bridge + image_transport + image_geometry + roscpp + tf2_ros + + diff --git a/src/image_transport_snippets/src/camera_subscriber_snippet.cpp b/src/image_transport_snippets/src/camera_subscriber_snippet.cpp new file mode 100644 index 0000000..3912b58 --- /dev/null +++ b/src/image_transport_snippets/src/camera_subscriber_snippet.cpp @@ -0,0 +1,97 @@ +#include +#include +#include +#include + +class CameraSubscriberNode +{ +public: + CameraSubscriberNode(const ros::NodeHandle& nh=ros::NodeHandle(), + const ros::NodeHandle& nh_private=ros::NodeHandle("~")) + : active_(false) + , nh_(nh) + , nh_priv_(nh_private) + {} + + // We follow the design for managed nodes: + // http://design.ros2.org/articles/node_lifecycle.html + bool configure(); + bool cleanup(); + bool activate(); + bool deactivate(); + + void imageCb(const sensor_msgs::ImageConstPtr& img_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg); + +private: + bool active_; + + ros::NodeHandle nh_; + ros::NodeHandle nh_priv_; + std::unique_ptr it_; + image_transport::CameraSubscriber sub_img_; + image_transport::Publisher pub_img_; + image_geometry::PinholeCameraModel camera_model_; +}; + +// Definitions + +bool CameraSubscriberNode::configure() +{ + it_.reset( new image_transport::ImageTransport(nh_) ); + sub_img_ = it_->subscribeCamera("image_input", 5, &CameraSubscriberNode::imageCb, this); + pub_img_ = it_->advertise("image_output", 1); + + return true; +} + +bool CameraSubscriberNode::cleanup() +{ + sub_img_.shutdown(); + it_.reset(); + + return true; +} + +bool CameraSubscriberNode::activate() +{ + return active_ = true; +} + +bool CameraSubscriberNode::deactivate() +{ + return !(active_ = false); +} + +void CameraSubscriberNode::imageCb( + const sensor_msgs::ImageConstPtr& img_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg) +{ + if (!active_) return; + + camera_model_.fromCameraInfo(info_msg); + // do somthing read only + const cv::Mat img = cv_bridge::toCvShare(img_msg)->image; + // .. + + // modifiy the image + cv::Mat img_modified; + img.copyTo(img_modified); + pub_img_.publish( + cv_bridge::CvImage( + img_msg->header, + img_msg->encoding, + img_modified).toImageMsg()); +} + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "camera_subscriber_node"); + CameraSubscriberNode node; + // because there is no lifecycle manager, we need to trigger + // transitions manually + node.configure(); + node.activate(); + ros::spin(); +}