From c52c326655828a60cf8e40040462b9127d5ea607 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 3 Jun 2015 11:41:59 -0400 Subject: [PATCH 1/4] Update demo_turtlebot_mapping.launch backward compatibility with 0.8.3 (hydro binaries) --- launch/demo/demo_turtlebot_mapping.launch | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/launch/demo/demo_turtlebot_mapping.launch b/launch/demo/demo_turtlebot_mapping.launch index f8603ec2..8574a9de 100644 --- a/launch/demo/demo_turtlebot_mapping.launch +++ b/launch/demo/demo_turtlebot_mapping.launch @@ -25,6 +25,7 @@ + @@ -48,8 +49,8 @@ - - + + @@ -69,7 +70,7 @@ - + @@ -79,6 +80,11 @@ + + + + + From 831ec5a1a261d9226d4c2bb9c036632243435802 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 16 Jun 2015 19:27:04 -0400 Subject: [PATCH 2/4] Updated rgbd_mapping_kinect2.launch with latest version of kinect2_bridge --- launch/rgbd_mapping_kinect2.launch | 46 +++++++++++++++-------------- src/CoreWrapper.cpp | 9 ++++-- src/GuiWrapper.cpp | 6 ++-- src/RGBDOdometryNode.cpp | 9 ++++-- src/nodelets/point_cloud_xyzrgb.cpp | 3 +- 5 files changed, 42 insertions(+), 31 deletions(-) diff --git a/launch/rgbd_mapping_kinect2.launch b/launch/rgbd_mapping_kinect2.launch index efa45765..968a6d76 100644 --- a/launch/rgbd_mapping_kinect2.launch +++ b/launch/rgbd_mapping_kinect2.launch @@ -5,19 +5,21 @@ Install Kinect2 : Follow ALL directives at https://github.com/code-iai/iai_kinect2 Make sure it is calibrated! Run: + $ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true $ roslaunch rtabmap_ros rgbd_mapping_kinect2.launch - $ rosrun kinect2_bridge kinect2_bridge _publish_tf:=true - - Prefixes: - sd: rgb_lowres / depth_lowres - hd: rgb_rect / depth_highres - ir: ir_rect / depth_rect --> - - + + + - + + + + + + @@ -38,7 +40,7 @@ - + @@ -49,9 +51,9 @@ - - - + + + @@ -74,9 +76,9 @@ - - - + + + @@ -91,9 +93,9 @@ - - - + + + @@ -103,9 +105,9 @@ - - - + + + diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 5cb4a6b7..b8d32602 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -672,7 +672,8 @@ void CoreWrapper::commonDepthCallback( const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg) { - if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || @@ -753,7 +754,11 @@ void CoreWrapper::commonDepthCallback( } cv_bridge::CvImageConstPtr ptrImage; - if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + if(imageMsg, imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + ptrImage = cv_bridge::toCvShare(imageMsg); + } + else if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) { ptrImage = cv_bridge::toCvShare(imageMsg, "mono8"); diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 06f1d962..308adc6d 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -434,7 +434,7 @@ void GuiWrapper::depthCallback( return; } - cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8"); + cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg); image_geometry::PinholeCameraModel model; @@ -502,7 +502,7 @@ void GuiWrapper::depthOdomInfoCallback( return; } - cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8"); + cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg); image_geometry::PinholeCameraModel model; @@ -579,7 +579,7 @@ void GuiWrapper::depthScanCallback( pcl::fromROSMsg(scanOut, pclScan); cv::Mat scan = util3d::laserScanFromPointCloud(pclScan); - cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8"); + cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg); image_geometry::PinholeCameraModel model; diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp index 84ddfe67..7db5bfeb 100644 --- a/src/RGBDOdometryNode.cpp +++ b/src/RGBDOdometryNode.cpp @@ -93,7 +93,8 @@ public: { if(!this->isPaused()) { - if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || @@ -101,7 +102,9 @@ public: depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1,32FC1,mono16"); + ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 " + "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s", + image->encoding.c_str(), depth->encoding.c_str()); return; } else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0) @@ -142,7 +145,7 @@ public: float fy = model.fy(); float cx = model.cx(); float cy = model.cy(); - cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8"); + cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth); rtabmap::SensorData data( diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index 1cc01b23..985665b2 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -156,7 +156,8 @@ private: const sensor_msgs::ImageConstPtr& imageDepth, const sensor_msgs::CameraInfoConstPtr& cameraInfo) { - if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) && From 317f0ed53fb6efbeff08a1798a02d24bb565f699 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 16 Jun 2015 19:33:27 -0400 Subject: [PATCH 3/4] fixed point_cloud_xyzrgb nodelet conversion of TYPE_8UC1 --- src/nodelets/point_cloud_xyzrgb.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index 985665b2..48517ca8 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -171,7 +171,7 @@ private: if(cloudPub_.getNumSubscribers()) { - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, "bgr8"); + cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); image_geometry::PinholeCameraModel model; From fa06d3050df0eb8d107a12e0c0159919891e1964 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 17 Jun 2015 22:52:35 -0400 Subject: [PATCH 4/4] Explicitly find OpenCV package to link with libraries built in /usr/local if OpenCV is built from source --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 900bf734..efc2a935 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,8 @@ find_package(octomap_ros) # find_package(Boost REQUIRED COMPONENTS system) find_package(RTABMap 0.9.0 REQUIRED) +find_package(OpenCV REQUIRED) + #Qt stuff FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) INCLUDE(${QT_USE_FILE}) @@ -102,11 +104,13 @@ catkin_package( include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/include ${RTABMap_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) # libraries SET(Libraries + ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${RTABMap_LIBRARIES} )