Hello,
I tried to write a small package to subscribe to stereo images of
bumblebee2 and publish them with my own name.
The source code of my source file, (which uses stereo_image_proc) is
attached.
I ran this package like this from the prompt:
shell$ roslaunch bumblebee2 Bumblebee2.launch
shell$ ROS_NAMESPACE=bumblebee2 rosrun stereo_image_proc stereo_image_proc
shell$ rosrun trial1 trial1
(from 'rostopic list -v' i can see the topic "/Yucong/left/image_rect" is
published).
However, after using ' rosrun image_view image_view
image:=/Yucong/left/image_rect '
I see only a window without an image in it.
How can I see the image? Any help would be appreciated, especially since I
am pretty new to the ROS environment.
Thanks,
--
Yucong Lin
Graduate student
School of Earth and Space Exploration
Arizona State University
PSH 470
--
Yucong Lin
Graduate student
School of Earth and Space Exploration
Arizona State University
PSH 470
#include <ros/ros.h>
#include <ros/names.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
class MyTrialNode
{
private:
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
// Subscriptions
image_transport::SubscriberFilter image_sub_l_, image_sub_r_;
image_transport::SubscriberFilter sub_rect_l_, sub_rect_r_;
message_filters::Subscriber<stereo_msgs::DisparityImage> sub_disparity_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_l_, info_sub_r_;
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo,
sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;
int subscriber_count_;
// Publications
std::string left_ns_, right_ns_, yucong_left_ns_ , yucong_right_ns_ ;
image_transport::Publisher pub_rect_l_;
image_transport::Publisher pub_rect_r_;
ros::Publisher pub_disparity_;
//image_transport::CameraPublisher left_image_pub_;
//image_transport::CameraPublisher right_image_pub_;
int left_received_, right_received_, both_received_;
public:
MyTrialNode() : it_(nh_), sync_(3),
subscriber_count_(0),
left_received_(0), right_received_(0), both_received_(0)
{
left_ns_ = nh_.resolveName("bumblebee2/left");
right_ns_ = nh_.resolveName("bumblebee2/right");
yucong_left_ns_ = nh_.resolveName("Yucong/left");
yucong_right_ns_ = nh_.resolveName("Yucong/right");
//image_sub_l_.subscribe(it_, left_ns_ + "/image_raw", 1);
//image_sub_r_.subscribe(it_, right_ns_ + "/image_raw", 1);
info_sub_l_.subscribe(nh_, left_ns_ + "/camera_info", 1);
info_sub_r_.subscribe(nh_, right_ns_ + "/camera_info", 1);
sub_rect_l_.subscribe(it_, left_ns_ +"/image_rect",1);
sub_rect_r_.subscribe(it_, right_ns_ +"/image_rect",1);
sub_disparity_.subscribe(nh_, "bumblebee2/disparity",1);
image_transport::SubscriberStatusCallback img_connect = boost::bind(&MyTrialNode::connectCb, this);
image_transport::SubscriberStatusCallback img_disconnect = boost::bind(&MyTrialNode::disconnectCb, this);
pub_rect_l_ = it_.advertise(yucong_left_ns_+"/image_rect", 1, img_connect, img_disconnect);
pub_rect_r_ = it_.advertise(yucong_right_ns_+"/image_rect", 1, img_connect, img_disconnect);
ros::SubscriberStatusCallback msg_connect = boost::bind(&MyTrialNode::connectCb, this);
ros::SubscriberStatusCallback msg_disconnect = boost::bind(&MyTrialNode::disconnectCb, this);
pub_disparity_ = nh_.advertise<stereo_msgs::DisparityImage>("Yucong/disparity", 1, msg_connect, msg_disconnect);
sync_.connectInput(image_sub_l_, info_sub_l_, image_sub_r_, info_sub_r_);
}
void connectCb()
{
if (subscriber_count_++ == 0) {
ROS_DEBUG("[trial1] Subscribing to camera topics");
/// @todo Make left and right subscriptions independent. Not easily possible with current synch tools.
image_sub_l_.subscribe(it_, left_ns_ + "/image_raw", 1);
image_sub_r_.subscribe(it_, right_ns_ + "/image_raw", 1);
info_sub_l_.subscribe(nh_, left_ns_ + "/camera_info", 1);
info_sub_r_.subscribe(nh_, right_ns_ + "/camera_info", 1);
sub_rect_l_.subscribe(it_, left_ns_ +"/image_rect",1);
sub_rect_r_.subscribe(it_, right_ns_ +"/image_rect",1);
sub_disparity_.subscribe(nh_, "bumblebee2/disparity",1);
}
}
void disconnectCb()
{
subscriber_count_--;
if (subscriber_count_ == 0) {
ROS_DEBUG("[trial1] Unsubscribing from camera topics");
image_sub_l_ .unsubscribe();
image_sub_r_ .unsubscribe();
info_sub_l_ .unsubscribe();
sub_rect_l_ .unsubscribe();
info_sub_r_ .unsubscribe();
sub_rect_r_ .unsubscribe();
sub_disparity_ .unsubscribe();
}
}
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "trial1", ros::init_options::AnonymousName);
MyTrialNode my_node;
ros::spin();
return 0;
}