32 #include <sensor_msgs/Image.h> 33 #include <sensor_msgs/CompressedImage.h> 35 #include <sensor_msgs/CameraInfo.h> 45 #include <opencv2/highgui/highgui.hpp> 47 #include <boost/thread.hpp> 49 #include "rtabmap_ros/RGBDImage.h" 90 bool approxSync =
false;
91 double approxSyncMaxInterval = 0.0;
92 pnh.
param(
"approx_sync", approxSync, approxSync);
93 pnh.
param(
"approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
94 pnh.
param(
"queue_size", queueSize, queueSize);
99 NODELET_INFO(
"%s: approx_sync_max_interval = %f",
getName().c_str(), approxSyncMaxInterval);
109 if(approxSyncMaxInterval > 0.0)
127 std::string subscribedTopicsMsg =
uFormat(
"\n%s subscribed to (%s sync%s):\n %s \\\n %s",
129 approxSync?
"approx":
"exact",
138 void warningLoop(
const std::string & subscribedTopicsMsg,
bool approxSync)
146 ROS_WARN(
"%s: Did not receive data since 5 seconds! Make sure the input topics are " 147 "published (\"$ rostopic hz my_topic\") and the timestamps in their " 148 "header are set. %s%s",
150 approxSync?
"":
"Parameter \"approx_sync\" is false, which means that input " 151 "topics should have all the exact timestamp for the callback to be called.",
152 subscribedTopicsMsg.c_str());
158 const sensor_msgs::ImageConstPtr& image,
159 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
164 double stamp = image->header.stamp.toSec();
166 rtabmap_ros::RGBDImage msg;
167 msg.header.frame_id = cameraInfo->header.frame_id;
168 msg.header.stamp = image->header.stamp;
169 msg.rgb_camera_info = *cameraInfo;
173 bool publishCompressed =
true;
179 publishCompressed =
false;
183 if(publishCompressed)
187 rtabmap_ros::RGBDImage msgCompressed = msg;
190 imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed,
cv_bridge::JPG);
202 if( stamp != image->header.stamp.toSec())
204 NODELET_ERROR(
"Input stamps changed between the beginning and the end of the callback! Make " 205 "sure the node publishing the topics doesn't override the same data after publishing them. A " 206 "solution is to use this node within another nodelet manager. Stamps: " 208 stamp, image->header.stamp.toSec());
boost::thread * warningThread_
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::CameraInfo > MyApproxSyncPolicy
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::string uFormat(const char *fmt,...)
#define NODELET_ERROR(...)
void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::CameraInfoConstPtr &cameraInfo)
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::CameraInfo > MyExactSyncPolicy
message_filters::Synchronizer< MyExactSyncPolicy > * exactSync_
std::string getTopic() const
ros::NodeHandle & getNodeHandle() const
void warningLoop(const std::string &subscribedTopicsMsg, bool approxSync)
ros::NodeHandle & getPrivateNodeHandle() const
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoSub_
const std::string & getName() const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
std::string resolveName(const std::string &name, bool remap=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
image_transport::SubscriberFilter imageSub_
#define NODELET_INFO(...)
ros::Time lastCompressedPublished_
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Synchronizer< MyApproxSyncPolicy > * approxSync_
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
std::string getTopic() const
uint32_t getNumSubscribers() const
#define NODELET_DEBUG(...)
ros::Publisher rgbdImageCompressedPub_
ros::Publisher rgbdImagePub_