How to use image_transport with nodelets?
What I need help with:
I am having a lot of trouble getting inter-nodelet image_transport to work. I tried making my code work like one of the nodelets in the image_proc
package, and I got code that compiles but breaks when it tries do image passing. I suspect I am generating a null pointer, but I'm not sure how to do it correctly.
I will attach my code, which consists of two files, "Read.cpp" and "Write.cpp". My goal is for the Read nodelet to read an image, pass it to the Write nodelet, which writes the image to a file. It does not work.
Code:
Here is Read.cpp
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <stdio.h>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
namespace tensor_nodelets{
class Read : public nodelet::Nodelet{
public:
Read() {}
private:
virtual void onInit(){
ros::NodeHandle& private_nh = getPrivateNodeHandle();
ros::NodeHandle nh_in (private_nh, "camera");
ros::NodeHandle nh_out (private_nh, "camera_out");
it_in_.reset(new image_transport::ImageTransport(nh_in));
it_out_.reset(new image_transport::ImageTransport(nh_out));
sub = private_nh.subscribe("in", 10, &Read::callback, this);
image_transport::SubscriberStatusCallback connect_cb = boost::bind(&Read::connect_cb, this);
ros::SubscriberStatusCallback connect_cb_info = boost::bind(&Read::connect_cb, this);
pub = it_out_->advertiseCamera("image_raw",1, connect_cb,connect_cb, connect_cb_info,connect_cb_info);
cv_im = cv::imread("/home/user/testim.png");
cv::imwrite("/home/user/imout.png",cv_im);
im_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", cv_im).toImageMsg();
}
void callback(const std_msgs::Float64::ConstPtr& input){
ROS_INFO("Read callback called, writing image\n");
sensor_msgs::CameraInfoPtr info_msg = sensor_msgs::CameraInfoPtr();
pub.publish(im_msg, info_msg);
}
void connect_cb() {} // I don't understand what this is here for
ros::Subscriber sub;
cv::Mat cv_im;
sensor_msgs::ImagePtr im_msg;
image_transport::CameraPublisher pub;
boost::shared_ptr<image_transport::ImageTransport> it_in_, it_out_;
};
PLUGINLIB_DECLARE_CLASS(tensor_nodelets, Read, tensor_nodelets::Read, nodelet::Nodelet);
}
Here is Write.cpp
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <stdio.h>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
namespace tensor_nodelets{
class Write : public nodelet::Nodelet{
public:
Write() {}
private:
virtual void onInit(){
ros::NodeHandle& private_nh = getPrivateNodeHandle();
ros::NodeHandle nh_in (private_nh, "camera");
ros::NodeHandle nh_out (private_nh, "camera_out");
it_in_.reset(new image_transport::ImageTransport(nh_in));
it_out_.reset(new image_transport::ImageTransport(nh_out));
image_transport::TransportHints hints("raw",ros::TransportHints(), private_nh);
sub = it_in_->subscribeCamera("image_raw", 1, &Write::im_callback, this, hints);
}
void im_callback(const sensor_msgs::ImageConstPtr& image_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg)
{
// do stuff here
ROS_INFO("Write callback called!\n");
}
image_transport::CameraSubscriber sub;
boost::shared_ptr<image_transport::ImageTransport> it_in_, it_out_;
};
PLUGINLIB_DECLARE_CLASS(tensor_nodelets, Write, tensor_nodelets::Write, nodelet::Nodelet);
} // namespace tensor_nodelets
Here is test.launch
<launch>
<!-- Nodelet manager -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<!-- Read nodelet -->
<node name="Read"
pkg="nodelet" type="nodelet"
args="load tensor_nodelets/Read standalone_nodelet"
output="screen">
<remap from="/Read/camera_out" to="/Write/camera"/>
</node>
<!-- Write nodelet -->
<node name="Write"
pkg="nodelet" type="nodelet"
args="load tensor_nodelets/Write standalone_nodelet"
output="screen">
</node>
</launch>
Here is the error when I rostopic pub /Read/in ...