Skip to content

Commit

Permalink
Use camera_info_manager from branch ci_manager_port_louise, enable ba…
Browse files Browse the repository at this point in the history 
…rrel distortion test - passes but segfaults at teardown, could be a problem with having 2 plugins side-by-side.
  • Loading branch information
@chapulina
chapulina committed Oct 24, 2018
1 parent 19d1b1c commit b0241db
Show file tree
Hide file tree
Showing 7 changed files with 112 additions and 115 deletions.
2 changes: 2 additions & 0 deletions gazebo_plugins/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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"
Expand Down
2 changes: 2 additions & 0 deletions gazebo_plugins/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>camera_info_manager</depend>
<depend>geometry_msgs</depend>
<depend>image_transport</depend>
<depend>nav_msgs</depend>
Expand All @@ -38,6 +39,7 @@
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>cv_bridge</test_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
25 changes: 13 additions & 12 deletions gazebo_plugins/src/gazebo_ros_camera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include <gazebo/rendering/Distortion.hh>
#include <ignition/math/Helpers.hh>

#include <camera_info_manager/camera_info_manager.h>
#include <gazebo_plugins/gazebo_ros_camera.hpp>
#include <gazebo_ros/conversions/builtin_interfaces.hpp>
#include <gazebo_ros/node.hpp>
Expand Down Expand Up @@ -45,6 +46,9 @@ class GazeboRosCameraPrivate
/// Trigger subscriber, in case it's a triggered camera
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr trigger_sub_{nullptr};

/// Camera info manager
std::shared_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;

/// Image encoding
std::string type_;

Expand Down Expand Up @@ -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<camera_info_manager::CameraInfoManager>(
impl_->ros_node_, impl_->camera_name_);
impl_->camera_info_manager_->setCameraInfo(camera_info_msg);
}

////////////////////////////////////////////////////////////////////////////////
Expand All @@ -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<builtin_interfaces::msg::Time>(
// 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<builtin_interfaces::msg::Time>(
sensor_update_time);

impl_->camera_info_pub_->publish(camera_info_msg);

// Disable camera if it's a triggered camera
if (nullptr != impl_->trigger_sub_) {
Expand Down
2 changes: 2 additions & 0 deletions gazebo_plugins/test/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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")
Expand Down Expand Up @@ -32,6 +33,7 @@ foreach(test ${tests})
gazebo_test_fixture
)
ament_target_dependencies(${test}
"cv_bridge"
"gazebo_dev"
"gazebo_ros"
"geometry_msgs"
Expand Down
6 changes: 4 additions & 2 deletions gazebo_plugins/test/test_gazebo_ros_camera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
171 changes: 87 additions & 84 deletions gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.

//#include <cv_bridge/cv_bridge.h>
#include <cv_bridge/cv_bridge.h>
#include <gazebo/test/ServerFixture.hh>
#include <image_transport/image_transport.h>
//#include <opencv2/core/core.hpp>
//#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <rclcpp/rclcpp.hpp>

//using namespace cv;
using namespace std::literals::chrono_literals; // NOLINT

/// Test parameters
Expand All @@ -38,24 +37,24 @@ struct TestParams
std::string distorted_cam_topic;
};

//void diffBetween(Mat& orig, Mat& diff, long& total_diff)
//{
// MatIterator_<Vec3b> it, end;
// 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<cv::Vec3b>(i,j);
// diff_pixel = diff.at<cv::Vec3b>(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_<cv::Vec3b> 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<cv::Vec3b>(i,j);
diff_pixel = diff.at<cv::Vec3b>(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<TestParams>
Expand Down Expand Up @@ -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<double>(i,0) += OFFSET;
// undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
// diffBetween(fixed_crop, undistorted_crop, diff2);
// EXPECT_GE(diff2, diff1);
// distortion_coeffs.at<double>(i,0) -= OFFSET;
//
// distortion_coeffs.at<double>(i,0) -= OFFSET;
// undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
// diffBetween(fixed_crop, undistorted_crop, diff2);
// EXPECT_GE(diff2, diff1);
// distortion_coeffs.at<double>(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<double>(i,0) += OFFSET;
undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
diffBetween(fixed_crop, undistorted_crop, diff2);
EXPECT_GE(diff2, diff1);
distortion_coeffs.at<double>(i,0) -= OFFSET;

distortion_coeffs.at<double>(i,0) -= OFFSET;
undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
diffBetween(fixed_crop, undistorted_crop, diff2);
EXPECT_GE(diff2, diff1);
distortion_coeffs.at<double>(i,0) += OFFSET;
}
}

INSTANTIATE_TEST_CASE_P(GazeboRosCameraDistortion, GazeboRosCameraDistortionTest, ::testing::Values(
Expand All @@ -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)
Expand Down
19 changes: 2 additions & 17 deletions gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<sdf version="1.6">
<world name="default">

<!-- Global light source -->
Expand All @@ -11,13 +11,6 @@
<static>true</static>
<pose>0.0 0.0 2.5 0.0 1.5707 0.0</pose>
<link name="camera_link">
<visual name="camera_visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor type="camera" name="camera_distorted">
<update_rate>30.0</update_rate>
<camera name="head">
Expand Down Expand Up @@ -63,13 +56,6 @@
<static>true</static>
<pose>0.0 0.0 2.5 0.0 1.5707 0.0</pose>
<link name="camera_link">
<visual name="camera_visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor type="camera" name="camera_undistorted">
<update_rate>30.0</update_rate>
<camera name="head">
Expand Down Expand Up @@ -100,7 +86,6 @@
</ros>
<camera_name>undistorted_camera</camera_name>
<frame_name>frame_name</frame_name>
<hack_baseline>0.0</hack_baseline>
</plugin>
</sensor>
</link>
Expand All @@ -109,5 +94,5 @@
<include>
<uri>model://checkerboard_plane</uri>
</include>
</world>
</world>
</sdf>

0 comments on commit b0241db

Please sign in to comment.