4343#include < sensor_msgs/Image.h>
4444#include < sensor_msgs/CameraInfo.h>
4545#include < sensor_msgs/distortion_models.h>
46- #include < cv_bridge/cv_bridge.h>
4746#include < sensor_msgs/image_encodings.h>
4847#include < image_transport/image_transport.h>
4948#include < dynamic_reconfigure/server.h>
6160#include < opencv2/imgproc/imgproc.hpp>
6261#include < opencv2/calib3d/calib3d.hpp>
6362
63+ // Boost includes
64+ #include < boost/make_shared.hpp>
65+
6466// PCL includes
6567#include < sensor_msgs/PointCloud2.h>
6668#include < pcl_conversions/pcl_conversions.h>
@@ -89,8 +91,42 @@ ros::Time point_cloud_time;
8991 * \param name : the path to the file
9092 */
9193bool file_exist (const std::string& name) {
92- struct stat buffer;
93- return (stat (name.c_str (), &buffer) == 0 );
94+ struct stat buffer;
95+ return (stat (name.c_str (), &buffer) == 0 );
96+ }
97+
98+ /* \brief Image to ros message conversion
99+ * \param img : the image to publish
100+ * \param encodingType : the sensor_msgs::image_encodings encoding type
101+ * \param frameId : the id of the reference frame of the image
102+ * \param t : the ros::Time to stamp the image
103+ */
104+ sensor_msgs::ImagePtr imageToROSmsg (cv::Mat img, const std::string encodingType, std::string frameId, ros::Time t){
105+ sensor_msgs::ImagePtr ptr = boost::make_shared<sensor_msgs::Image>();
106+ sensor_msgs::Image& imgMessage = *ptr;
107+ imgMessage.header .stamp =t;
108+ imgMessage.header .frame_id =frameId;
109+ imgMessage.height = img.rows ;
110+ imgMessage.width = img.cols ;
111+ imgMessage.encoding =encodingType;
112+ int num = 1 ; // for endianness detection
113+ imgMessage.is_bigendian = !(*(char *)&num == 1 );
114+ imgMessage.step = img.cols * img.elemSize ();
115+ size_t size = imgMessage.step * img.rows ;
116+ imgMessage.data .resize (size);
117+
118+ if (img.isContinuous ())
119+ memcpy ((char *)(&imgMessage.data [0 ]), img.data , size);
120+ else {
121+ uchar* opencvData = img.data ;
122+ uchar* rosData = (uchar*)(&imgMessage.data [0 ]);
123+ for (unsigned int i = 0 ; i < img.rows ; i++) {
124+ memcpy (rosData, opencvData, imgMessage.step );
125+ rosData += imgMessage.step ;
126+ opencvData += img.step ;
127+ }
128+ }
129+ return ptr;
94130}
95131
96132/* \brief Publish the pose of the camera with a ros Publisher
@@ -105,10 +141,10 @@ void publishOdom(Eigen::Matrix4f Path, ros::Publisher &pub_odom, string odom_fra
105141 odom.header .frame_id = odom_frame_id;
106142 // odom.child_frame_id = "zed_optical_frame";
107143
108- odom.pose .pose .position .y = -Path (0 , 3 );
109- odom.pose .pose .position .z = Path (1 , 3 );
110- odom.pose .pose .position .x = -Path (2 , 3 );
111- Eigen::Quaternionf quat (Path.block <3 , 3 >(0 , 0 ));
144+ odom.pose .pose .position .y = -Path (0 ,3 );
145+ odom.pose .pose .position .z = Path (1 ,3 );
146+ odom.pose .pose .position .x = -Path (2 ,3 );
147+ Eigen::Quaternionf quat (Path.block <3 ,3 >(0 ,0 ));
112148 odom.pose .pose .orientation .x = -quat.z ();
113149 odom.pose .pose .orientation .y = -quat.x ();
114150 odom.pose .pose .orientation .z = quat.y ();
@@ -128,10 +164,10 @@ void publishTrackedFrame(Eigen::Matrix4f Path, tf2_ros::TransformBroadcaster &tr
128164 transformStamped.header .stamp = ros::Time::now ();
129165 transformStamped.header .frame_id = " zed_initial_frame"
130166 transformStamped.child_frame_id = odometry_transform_frame_id;
131- transformStamped.transform .translation .x = -Path (0 , 3 );
132- transformStamped.transform .translation .y = Path (1 , 3 );
133- transformStamped.transform .translation .z = -Path (2 , 3 );
134- Eigen::Quaternionf quat (Path.block <3 , 3 >(0 , 0 ));
167+ transformStamped.transform .translation .x = -Path (0 ,3 );
168+ transformStamped.transform .translation .y = Path (1 ,3 );
169+ transformStamped.transform .translation .z = -Path (2 ,3 );
170+ Eigen::Quaternionf quat (Path.block <3 ,3 >(0 ,0 ));
135171 transformStamped.transform .rotation .x = -quat.z ();
136172 transformStamped.transform .rotation .y = -quat.x ();
137173 transformStamped.transform .rotation .z = quat.y ();
@@ -146,12 +182,10 @@ void publishTrackedFrame(Eigen::Matrix4f Path, tf2_ros::TransformBroadcaster &tr
146182 * \param t : the ros::Time to stamp the image
147183 */
148184void publishImage (cv::Mat img, image_transport::Publisher &pub_img, string img_frame_id, ros::Time t) {
149- cv_bridge::CvImage img_im;
150- img_im.image = img;
151- img_im.encoding = sensor_msgs::image_encodings::BGR8;
152- img_im.header .frame_id = img_frame_id;
153- img_im.header .stamp = t;
154- pub_img.publish (img_im.toImageMsg ());
185+ pub_img.publish (imageToROSmsg (img
186+ , sensor_msgs::image_encodings::BGR8
187+ , img_frame_id
188+ , t ));
155189}
156190
157191/* \brief Publish a cv::Mat depth image with a ros Publisher
@@ -161,18 +195,19 @@ void publishImage(cv::Mat img, image_transport::Publisher &pub_img, string img_f
161195 * \param t : the ros::Time to stamp the depth image
162196 */
163197void publishDepth (cv::Mat depth, image_transport::Publisher &pub_depth, string depth_frame_id, ros::Time t) {
164- cv_bridge::CvImage depth_im ;
165- if (openniDepthMode) {
198+ string encoding ;
199+ if (openniDepthMode){
166200 depth *= 1000 .0f ;
167201 depth.convertTo (depth, CV_16UC1); // in mm, rounded
168- depth_im.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
169- } else {
170- depth_im.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
202+ encoding = sensor_msgs::image_encodings::TYPE_16UC1;
203+ }
204+ else {
205+ encoding = sensor_msgs::image_encodings::TYPE_32FC1;
171206 }
172- depth_im. image = depth;
173- depth_im. header . frame_id = depth_frame_id;
174- depth_im. header . stamp = t;
175- pub_depth. publish (depth_im. toImageMsg ( ));
207+ pub_depth. publish ( imageToROSmsg ( depth
208+ , encoding
209+ , depth_frame_id
210+ , t ));
176211}
177212
178213/* \brief Publish a pointCloud with a ros Publisher
@@ -328,7 +363,7 @@ int main(int argc, char **argv) {
328363 string right_frame_id = " /zed_tracked_frame"
329364
330365 string depth_topic = " depth/"
331- if (openniDepthMode)
366+ if (openniDepthMode)
332367 depth_topic += " image_raw"
333368 else
334369 depth_topic += img_topic;
@@ -356,7 +391,7 @@ int main(int argc, char **argv) {
356391 nh_ns.getParam (" frame_rate"
357392 nh_ns.getParam (" odometry_DB"
358393 nh_ns.getParam (" openni_depth_mode"
359- if (openniDepthMode)
394+ if (openniDepthMode)
360395 ROS_INFO_STREAM (" Openni depth mode activated"
361396
362397 nh_ns.getParam (" rgb_topic"
@@ -406,7 +441,7 @@ int main(int argc, char **argv) {
406441 std::this_thread::sleep_for (std::chrono::milliseconds (2000 ));
407442 }
408443
409- zed->grab (static_cast <sl::zed::SENSING_MODE> (sensing_mode), true , true , true ); // call the first grab
444+ zed->grab (static_cast <sl::zed::SENSING_MODE> (sensing_mode), true , true , true );// call the first grab
410445
411446 // Tracking variables
412447 sl::zed::TRACKING_STATE track_state;
@@ -491,14 +526,15 @@ int main(int argc, char **argv) {
491526 bool runLoop = (rgb_SubNumber + left_SubNumber + right_SubNumber + depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0 ;
492527 // Run the loop only if there is some subscribers
493528 if (runLoop) {
494- if (odom_SubNumber > 0 && !tracking_activated) { // Start the tracking
495- if (odometry_DB != " " file_exist (odometry_DB)) {
529+ if (odom_SubNumber> 0 && !tracking_activated) { // Start the tracking
530+ if (odometry_DB != " " file_exist (odometry_DB)){
496531 odometry_DB = " "
497532 ROS_WARN (" odometry_DB path doesn't exist or is unreachable."
498533 }
499534 zed->enableTracking (Path, true , odometry_DB);
500535 tracking_activated = true ;
501- } else if (odom_SubNumber == 0 && tracking_activated) { // Stop the tracking
536+ }
537+ else if (odom_SubNumber==0 && tracking_activated) { // Stop the tracking
502538 zed->stopTracking ();
503539 tracking_activated = false ;
504540 }
@@ -534,10 +570,10 @@ int main(int argc, char **argv) {
534570 ROS_INFO_STREAM (errcode2str (err));
535571 std::this_thread::sleep_for (std::chrono::milliseconds (2000 ));
536572 }
537- zed->grab (static_cast <sl::zed::SENSING_MODE> (sensing_mode), true , true , cloud_SubNumber > 0 ); // call the first grab
573+ zed->grab (static_cast <sl::zed::SENSING_MODE> (sensing_mode), true , true , cloud_SubNumber > 0 );// call the first grab
538574 tracking_activated = false ;
539- if (odom_SubNumber > 0 ) { // Start the tracking
540- if (odometry_DB != " " file_exist (odometry_DB)) {
575+ if (odom_SubNumber> 0 ) { // Start the tracking
576+ if (odometry_DB != " " file_exist (odometry_DB)){
541577 odometry_DB = " "
542578 ROS_WARN (" odometry_DB path doesn't exist or is unreachable."
543579 }
@@ -600,11 +636,12 @@ int main(int argc, char **argv) {
600636 ros::spinOnce ();
601637 loop_rate.sleep ();
602638 } else {
639+
603640 publishTrackedFrame (Path, transform_odom_broadcaster, odometry_transform_frame_id, ros::Time::now ()); // publish the tracked Frame before the sleep
604641 std::this_thread::sleep_for (std::chrono::milliseconds (10 )); // No subscribers, we just wait
605642 }
606643
607-
644+
608645 }
609646 } catch (...) {
610647 if (pointCloudThread && pointCloudThreadRunning) {
0 commit comments