virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& pnh = getPrivateNodeHandle();
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
int queueSize = 5;
bool approxSync = false;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("rate", max_update_rate_, max_update_rate_);
pnh.param("queue_size", queueSize, queueSize);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(max_update_rate_ == 0.0)
{
ROS_WARN("Parameter \"rate\" (Hz) is not set!");
}
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
}
imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1);
imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1);
infoLeftPub_ = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info")+"_throttle", 1);
infoRightPub_ = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info")+"_throttle", 1);
};
int main(int argc, char** argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
ros::init(argc, argv, "uvc_stereo_camera");
ros::NodeHandle pnh("~");
double rate = 0.0;
std::string id = "camera";
std::string frameId = "camera_link";
double scale = 1.0;
pnh.param("rate", rate, rate);
pnh.param("camera_id", id, id);
pnh.param("frame_id", frameId, frameId);
pnh.param("scale", scale, scale);
rtabmap::CameraStereoVideo camera(0, false, rate);
if(camera.init(UDirectory::homeDir() + "/.ros/camera_info", id))
{
ros::NodeHandle nh;
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::Publisher imageLeftPub = left_it.advertise(left_nh.resolveName("image_raw"), 1);
image_transport::Publisher imageRightPub = right_it.advertise(right_nh.resolveName("image_raw"), 1);
ros::Publisher infoLeftPub = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info"), 1);
ros::Publisher infoRightPub = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info"), 1);
while(ros::ok())
{
rtabmap::SensorData data = camera.takeImage();
ros::Time currentTime = ros::Time::now();
cv_bridge::CvImage imageLeft;
imageLeft.header.frame_id = frameId;
imageLeft.header.stamp = currentTime;
imageLeft.encoding = "bgr8";
cv::resize(data.imageRaw(), imageLeft.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
imageLeftPub.publish(imageLeft.toImageMsg());
cv_bridge::CvImage imageRight;
imageRight.header = imageLeft.header;
imageRight.encoding = "mono8";
cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
imageRightPub.publish(imageRight.toImageMsg());
sensor_msgs::CameraInfo infoLeft, infoRight;
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left().scaled(scale), infoLeft);
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right().scaled(scale), infoRight);
infoLeft.header = imageLeft.header;
infoRight.header = imageLeft.header;
infoLeftPub.publish(infoLeft);
infoRightPub.publish(infoRight);
ros::spinOnce();
}
}
else
{
ROS_ERROR("Could not initialize the camera!");
}
return 0;
}