From b0241db2fd827383afdfa0cef2a465813104fb28 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 14 Sep 2018 20:14:10 -0700 Subject: [PATCH] Use camera_info_manager from branch ci_manager_port_louise, enable barrel distortion test - passes but segfaults at teardown, could be a problem with having 2 plugins side-by-side. --- gazebo_plugins/CMakeLists.txt | 2 + gazebo_plugins/package.xml | 2 + gazebo_plugins/src/gazebo_ros_camera.cpp | 25 +-- gazebo_plugins/test/CMakeLists.txt | 2 + .../test/test_gazebo_ros_camera.cpp | 6 +- .../test_gazebo_ros_camera_distortion.cpp | 171 +++++++++--------- .../gazebo_ros_camera_distortion_barrel.world | 19 +- 7 files changed, 112 insertions(+), 115 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 98b306f1b..94691b2c1 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -12,6 +12,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(camera_info_manager REQUIRED) find_package(gazebo_dev REQUIRED) find_package(gazebo_ros REQUIRED) find_package(geometry_msgs REQUIRED) @@ -92,6 +93,7 @@ add_library(gazebo_ros_camera SHARED src/gazebo_ros_camera.cpp ) ament_target_dependencies(gazebo_ros_camera + "camera_info_manager" "gazebo_dev" "gazebo_ros" "image_transport" diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 699b2aa6e..6e2b26443 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -19,6 +19,7 @@ ament_cmake + camera_info_manager geometry_msgs image_transport nav_msgs @@ -38,6 +39,7 @@ ament_cmake_gtest ament_lint_auto ament_lint_common + cv_bridge ament_cmake diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index 0303fe833..5f2eb19b2 100644 --- a/gazebo_plugins/src/gazebo_ros_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -45,6 +46,9 @@ class GazeboRosCameraPrivate /// Trigger subscriber, in case it's a triggered camera rclcpp::Subscription::SharedPtr trigger_sub_{nullptr}; + /// Camera info manager + std::shared_ptr camera_info_manager_; + /// Image encoding std::string type_; @@ -308,13 +312,10 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ camera_info_msg.p[10] = 1.0; camera_info_msg.p[11] = 0.0; -// // Initialize camera_info_manager -// // TODO(louise) Not ported to ROS2 yet -// this->camera_info_manager_.reset(new camera_info_manager::CameraInfoManager( -// *this->rosnode_, this->camera_name_)); -// this->camera_info_manager_->setCameraInfo(camera_info_msg); - -// load_event_(); + // Initialize camera_info_manager + impl_->camera_info_manager_ = std::make_shared( + impl_->ros_node_, impl_->camera_name_); + impl_->camera_info_manager_->setCameraInfo(camera_info_msg); } //////////////////////////////////////////////////////////////////////////////// @@ -341,11 +342,11 @@ void GazeboRosCamera::OnNewFrame( impl_->image_pub_.publish(image_msg); // Publish camera info -// auto camera_info_msg = camera_info_manager_->getCameraInfo(); -// camera_info_msg.header.stamp = gazebo_ros::Convert( -// sensor_update_time); -// -// camera_info_pub_.publish(camera_info_msg); + auto camera_info_msg = impl_->camera_info_manager_->getCameraInfo(); + camera_info_msg.header.stamp = gazebo_ros::Convert( + sensor_update_time); + + impl_->camera_info_pub_->publish(camera_info_msg); // Disable camera if it's a triggered camera if (nullptr != impl_->trigger_sub_) { diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index 7db768874..bc69da505 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -1,4 +1,5 @@ find_package(ament_cmake_gtest REQUIRED) +find_package(cv_bridge REQUIRED) # Worlds file(GLOB worlds RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "worlds/*.world") @@ -32,6 +33,7 @@ foreach(test ${tests}) gazebo_test_fixture ) ament_target_dependencies(${test} + "cv_bridge" "gazebo_dev" "gazebo_ros" "geometry_msgs" diff --git a/gazebo_plugins/test/test_gazebo_ros_camera.cpp b/gazebo_plugins/test/test_gazebo_ros_camera.cpp index bc39b6fb3..b782ec3c6 100644 --- a/gazebo_plugins/test/test_gazebo_ros_camera.cpp +++ b/gazebo_plugins/test/test_gazebo_ros_camera.cpp @@ -77,8 +77,10 @@ TEST_P(GazeboRosCameraTest, CameraSubscribeTest) INSTANTIATE_TEST_CASE_P(GazeboRosCamera, GazeboRosCameraTest, ::testing::Values( // TODO(louise) Use mapped topics once this issue is solved: // https://github.com/ros-perception/image_common/issues/93 - TestParams({"worlds/gazebo_ros_camera.world", "test_cam/camera1/image_raw"}), - TestParams({"worlds/gazebo_ros_camera_16bit.world", "test_cam_16bit/test_camera_name/image_raw"}) + TestParams({"worlds/gazebo_ros_camera.world", + "test_cam/camera1/image_raw"}), + TestParams({"worlds/gazebo_ros_camera_16bit.world", + "test_cam_16bit/test_camera_name/image_raw"}) ), ); int main(int argc, char** argv) diff --git a/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp b/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp index 8bb0a1de8..e7f58f851 100644 --- a/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp +++ b/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -//#include +#include #include #include -//#include -//#include +#include +#include #include -//using namespace cv; using namespace std::literals::chrono_literals; // NOLINT /// Test parameters @@ -38,24 +37,24 @@ struct TestParams std::string distorted_cam_topic; }; -//void diffBetween(Mat& orig, Mat& diff, long& total_diff) -//{ -// MatIterator_ it, end; -// Vec3b orig_pixel, diff_pixel; -// total_diff = 0; -// -// for(int i=0; i(i,j); -// diff_pixel = diff.at(i,j); -// total_diff += abs(orig_pixel[0] - diff_pixel[0]) + -// abs(orig_pixel[1] - diff_pixel[1]) + -// abs(orig_pixel[2] - diff_pixel[2]); -// } -// } -//} +void diffBetween(cv::Mat & orig, cv::Mat & diff, long & total_diff) +{ + cv::MatIterator_ it, end; + cv::Vec3b orig_pixel, diff_pixel; + total_diff = 0; + + for (int i = 0; i < orig.rows; ++i) + { + for (int j = 0; j < orig.cols; ++j) + { + orig_pixel = orig.at(i,j); + diff_pixel = diff.at(i,j); + total_diff += abs(orig_pixel[0] - diff_pixel[0]) + + abs(orig_pixel[1] - diff_pixel[1]) + + abs(orig_pixel[2] - diff_pixel[2]); + } + } +} class GazeboRosCameraDistortionTest : public gazebo::ServerFixture, public ::testing::WithParamInterface @@ -123,76 +122,80 @@ TEST_P(GazeboRosCameraDistortionTest, CameraSubscribeTest) // Update rate is 0.5 Hz, so we step 3s sim time to be sure we get exactly 1 image at 2s world->Step(3000); + + executor.spin_once(100ms); + executor.spin_once(100ms); executor.spin_once(100ms); executor.spin_once(100ms); ASSERT_NE(nullptr, cam_image_undistorted); ASSERT_NE(nullptr, cam_image_distorted); - // ASSERT_NE(nullptr, cam_info_distorted); + ASSERT_NE(nullptr, cam_info_distorted); // Cleanup transport so we don't get new messages while proceeding the test cam_sub_undistorted_.shutdown(); cam_sub_distorted_.shutdown(); cam_info_distorted_sub_.reset(); - // load camera coefficients from published ROS information -// Mat intrinsic_distorted_matrix = Mat(3, 3, CV_64F); -// if (cam_info_distorted->K.size() == 9) -// { -// memcpy(intrinsic_distorted_matrix.data, cam_info_distorted->K.data(), -// cam_info_distorted->K.size()*sizeof(double)); -// } -// Mat distortion_coeffs = Mat(5, 1, CV_64F); -// if (cam_info_distorted->D.size() == 5) -// { -// memcpy(distortion_coeffs.data, cam_info_distorted->D.data(), -// cam_info_distorted->D.size()*sizeof(double)); -// } + // Load camera coefficients from published ROS information + auto intrinsic_distorted_matrix = cv::Mat(3, 3, CV_64F); + if (cam_info_distorted->k.size() == 9) + { + memcpy(intrinsic_distorted_matrix.data, cam_info_distorted->k.data(), + cam_info_distorted->k.size()*sizeof(double)); + } + + auto distortion_coeffs = cv::Mat(5, 1, CV_64F); + if (cam_info_distorted->d.size() == 5) + { + memcpy(distortion_coeffs.data, cam_info_distorted->d.data(), + cam_info_distorted->d.size()*sizeof(double)); + } // Information acquired, now test the quality of the undistortion -// Mat distorted = Mat(cv_bridge::toCvCopy(cam_image_distorted)->image); -// Mat fixed = distorted.clone(); -// Mat undistorted = Mat(cv_bridge::toCvCopy(cam_image_undistorted)->image); -// -// // crop the image to remove black borders leftover from (un)distortion -// int crop_border = 50; -// cv::Rect myROI(crop_border, crop_border, -// fixed.rows - 2 * crop_border, fixed.cols - 2 * crop_border); -// cv::Mat fixed_crop = fixed(myROI); -// cv::Mat undistorted_crop = undistorted(myROI); -// -// undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); -// -// //Ensure that we didn't crop away everything -// ASSERT_GT(distorted.rows, 0); -// ASSERT_GT(distorted.cols, 0); -// ASSERT_GT(undistorted.rows, 0); -// ASSERT_GT(undistorted.cols, 0); -// ASSERT_GT(fixed.rows, 0); -// ASSERT_GT(fixed.cols, 0); -// -// // The difference between the undistorted image and the no-distortion camera -// // image should be the lowest when we use the correct distortion parameters. -// long diff1 = 0, diff2 = 0; -// diffBetween(fixed_crop, undistorted_crop, diff1); -// -// const double OFFSET = 0.01; -// -// // test each parameter, varying one at a time -// for(size_t i = 0; i < 5; ++i) -// { -// distortion_coeffs.at(i,0) += OFFSET; -// undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); -// diffBetween(fixed_crop, undistorted_crop, diff2); -// EXPECT_GE(diff2, diff1); -// distortion_coeffs.at(i,0) -= OFFSET; -// -// distortion_coeffs.at(i,0) -= OFFSET; -// undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); -// diffBetween(fixed_crop, undistorted_crop, diff2); -// EXPECT_GE(diff2, diff1); -// distortion_coeffs.at(i,0) += OFFSET; -// } + auto distorted = cv::Mat(cv_bridge::toCvCopy(cam_image_distorted)->image); + auto fixed = distorted.clone(); + auto undistorted = cv::Mat(cv_bridge::toCvCopy(cam_image_undistorted)->image); + + // crop the image to remove black borders leftover from (un)distortion + int crop_border = 50; + cv::Rect myROI(crop_border, crop_border, + fixed.rows - 2 * crop_border, fixed.cols - 2 * crop_border); + cv::Mat fixed_crop = fixed(myROI); + auto undistorted_crop = undistorted(myROI); + + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + + //Ensure that we didn't crop away everything + ASSERT_GT(distorted.rows, 0); + ASSERT_GT(distorted.cols, 0); + ASSERT_GT(undistorted.rows, 0); + ASSERT_GT(undistorted.cols, 0); + ASSERT_GT(fixed.rows, 0); + ASSERT_GT(fixed.cols, 0); + + // The difference between the undistorted image and the no-distortion camera + // image should be the lowest when we use the correct distortion parameters. + long diff1 = 0, diff2 = 0; + diffBetween(fixed_crop, undistorted_crop, diff1); + + const double OFFSET = 0.01; + + // test each parameter, varying one at a time + for(size_t i = 0; i < 5; ++i) + { + distortion_coeffs.at(i,0) += OFFSET; + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + diffBetween(fixed_crop, undistorted_crop, diff2); + EXPECT_GE(diff2, diff1); + distortion_coeffs.at(i,0) -= OFFSET; + + distortion_coeffs.at(i,0) -= OFFSET; + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + diffBetween(fixed_crop, undistorted_crop, diff2); + EXPECT_GE(diff2, diff1); + distortion_coeffs.at(i,0) += OFFSET; + } } INSTANTIATE_TEST_CASE_P(GazeboRosCameraDistortion, GazeboRosCameraDistortionTest, ::testing::Values( @@ -201,11 +204,11 @@ INSTANTIATE_TEST_CASE_P(GazeboRosCameraDistortion, GazeboRosCameraDistortionTest TestParams({"worlds/gazebo_ros_camera_distortion_barrel.world", "undistorted_camera/image_raw", "distorted_camera/image_raw", - "test_cam_distorted/camera_info"}), - TestParams({"worlds/gazebo_ros_camera_distortion_pincushion.world", - "test_cam_undistorted/image_raw", - "test_cam_distorted/image_raw", - "test_camdistorted_/camera_info"}) + "distorted_camera/camera_info"}) +// TestParams({"worlds/gazebo_ros_camera_distortion_pincushion.world", +// "undistorted_camera/image_raw", +// "distorted_camera/image_raw", +// "distorted_camera/camera_info"}) ), ); int main(int argc, char** argv) diff --git a/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world index 9d9687951..5926713a0 100644 --- a/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world +++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world @@ -1,5 +1,5 @@ - + @@ -11,13 +11,6 @@ true 0.0 0.0 2.5 0.0 1.5707 0.0 - - - - 0.1 0.1 0.1 - - - 30.0 @@ -63,13 +56,6 @@ true 0.0 0.0 2.5 0.0 1.5707 0.0 - - - - 0.1 0.1 0.1 - - - 30.0 @@ -100,7 +86,6 @@ undistorted_camera frame_name - 0.0 @@ -109,5 +94,5 @@ model://checkerboard_plane - +