Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[gazebo_plugins] Fix for issue #408 - correct the timestamp used by the camera #410

Merged
merged 10 commits into from
Feb 13, 2017
Merged
19 changes: 19 additions & 0 deletions gazebo_plugins/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 2.8.3)
project(gazebo_plugins)

option(ENABLE_DISPLAY_TESTS "Enable the building of tests that requires a display" OFF)

find_package(catkin REQUIRED COMPONENTS
message_generation
gazebo_msgs
Expand Down Expand Up @@ -362,11 +364,28 @@ install(DIRECTORY test
)

# Tests
# These need to be run with -j1 flag because gazebo can't be run
# in parallel.
if (CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest_gtest(set_model_state-test
test/set_model_state_test/set_model_state_test.test
test/set_model_state_test/set_model_state_test.cpp)
add_rostest(test/range/range_plugin.test)
target_link_libraries(set_model_state-test ${catkin_LIBRARIES})

if (ENABLE_DISPLAY_TESTS)
add_rostest_gtest(depth_camera-test
test/camera/depth_camera.test
test/camera/depth_camera.cpp)
target_link_libraries(depth_camera-test ${catkin_LIBRARIES})
add_rostest_gtest(multicamera-test
test/camera/multicamera.test
test/camera/multicamera.cpp)
target_link_libraries(multicamera-test ${catkin_LIBRARIES})
add_rostest_gtest(camera-test
test/camera/camera.test
test/camera/camera.cpp)
target_link_libraries(camera-test ${catkin_LIBRARIES})
endif()
endif()
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,8 @@ namespace gazebo

std::vector<GazeboRosCameraUtils*> utils;

protected: void OnNewFrame(const unsigned char *_image,
GazeboRosCameraUtils* util);
/// \brief Update the controller
/// FIXME: switch to function vectors
protected: virtual void OnNewFrameLeft(const unsigned char *_image,
Expand Down
20 changes: 13 additions & 7 deletions gazebo_plugins/src/gazebo_ros_camera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,9 +77,9 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
const std::string &_format)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
# endif

if (!this->parentSensor->IsActive())
Expand All @@ -92,12 +92,18 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
{
if ((*this->image_connect_count_) > 0)
{
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_update_time_ >= this->update_period_)
// OnNewFrame is triggered at the gazebo sensor <update_rate>
// while there is also a plugin <updateRate> that can throttle the
// rate down further (but then why not reduce the sensor rate?
// what is the use case?).
// Setting the <updateRate> to zero will make this plugin
// update at the gazebo sensor <update_rate>, update_period_ will be
// zero and the conditional always will be true.
if (sensor_update_time - this->last_update_time_ >= this->update_period_)
{
this->PutCameraData(_image);
this->PublishCameraInfo();
this->last_update_time_ = cur_time;
this->PutCameraData(_image, sensor_update_time);
this->PublishCameraInfo(sensor_update_time);
this->last_update_time_ = sensor_update_time;
}
}
}
Expand Down
28 changes: 16 additions & 12 deletions gazebo_plugins/src/gazebo_ros_depth_camera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -192,9 +192,9 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image,
return;

# if GAZEBO_MAJOR_VERSION >= 7
this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
# else
this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
# endif
if (this->parentSensor->IsActive())
{
Expand Down Expand Up @@ -232,9 +232,9 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
return;

# if GAZEBO_MAJOR_VERSION >= 7
this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
# else
this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
# endif
if (!this->parentSensor->IsActive())
{
Expand Down Expand Up @@ -302,9 +302,9 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,

//ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor->LastUpdateTime();
this->sensor_update_time_ = this->parentSensor->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
this->sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
# endif

if (!this->parentSensor->IsActive())
Expand All @@ -316,7 +316,11 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,
else
{
if ((*this->image_connect_count_) > 0)
{
this->PutCameraData(_image);
// TODO(lucasw) publish camera info with depth image
// this->PublishCameraInfo(sensor_update_time);
}
}
}

Expand Down Expand Up @@ -502,15 +506,15 @@ void GazeboRosDepthCamera::PublishCameraInfo()
if (this->depth_info_connect_count_ > 0)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
# endif
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
this->sensor_update_time_ = sensor_update_time;
if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
{
this->PublishCameraInfo(this->depth_image_camera_info_pub_);
this->last_depth_image_camera_info_update_time_ = cur_time;
this->PublishCameraInfo(this->depth_image_camera_info_pub_); // , sensor_update_time);
this->last_depth_image_camera_info_update_time_ = sensor_update_time;
}
}
}
Expand Down
47 changes: 18 additions & 29 deletions gazebo_plugins/src/gazebo_ros_multicamera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,52 +102,41 @@ void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent,
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
unsigned int _width, unsigned int _height, unsigned int _depth,
const std::string &_format)

void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image,
GazeboRosCameraUtils* util)
{
GazeboRosCameraUtils* util = this->utils[0];
# if GAZEBO_MAJOR_VERSION >= 7
util->sensor_update_time_ = util->parentSensor_->LastUpdateTime();
common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime();
# else
util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime();
common::Time sensor_update_time = util->parentSensor_->GetLastMeasurementTime();
# endif

if (util->parentSensor_->IsActive())
{
common::Time cur_time = util->world_->GetSimTime();
if (cur_time - util->last_update_time_ >= util->update_period_)
if (sensor_update_time - util->last_update_time_ >= util->update_period_)
{
util->PutCameraData(_image);
util->PublishCameraInfo();
util->last_update_time_ = cur_time;
util->PutCameraData(_image, sensor_update_time);
util->PublishCameraInfo(sensor_update_time);
util->last_update_time_ = sensor_update_time;
}
}
}

// Update the controller
void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
unsigned int _width, unsigned int _height, unsigned int _depth,
const std::string &_format)
{
OnNewFrame(_image, this->utils[0]);
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image,
unsigned int _width, unsigned int _height, unsigned int _depth,
const std::string &_format)
{
GazeboRosCameraUtils* util = this->utils[1];
# if GAZEBO_MAJOR_VERSION >= 7
util->sensor_update_time_ = util->parentSensor_->LastUpdateTime();
# else
util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime();
# endif

if (util->parentSensor_->IsActive())
{
common::Time cur_time = util->world_->GetSimTime();
if (cur_time - util->last_update_time_ >= util->update_period_)
{
util->PutCameraData(_image);
util->PublishCameraInfo();
util->last_update_time_ = cur_time;
}
}
OnNewFrame(_image, this->utils[1]);
}
}
72 changes: 72 additions & 0 deletions gazebo_plugins/test/camera/camera.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
#include <gtest/gtest.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>

class CameraTest : public testing::Test
{
protected:
virtual void SetUp()
{
has_new_image_ = false;
}

ros::NodeHandle nh_;
image_transport::Subscriber cam_sub_;
bool has_new_image_;
ros::Time image_stamp_;
public:
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
image_stamp_ = msg->header.stamp;
has_new_image_ = true;
}
};

// Test if the camera image is published at all, and that the timestamp
// is not too long in the past.
TEST_F(CameraTest, cameraSubscribeTest)
{
image_transport::ImageTransport it(nh_);
cam_sub_ = it.subscribe("camera1/image_raw", 1,
&CameraTest::imageCallback,
dynamic_cast<CameraTest*>(this));

#if 0
// wait for gazebo to start publishing
// TODO(lucasw) this isn't really necessary since this test
// is purely passive
bool wait_for_topic = true;
while (wait_for_topic)
{
// @todo this fails without the additional 0.5 second sleep after the
// publisher comes online, which means on a slower or more heavily
// loaded system it may take longer than 0.5 seconds, and the test
// would hang until the timeout is reached and fail.
if (cam_sub_.getNumPublishers() > 0)
wait_for_topic = false;
ros::Duration(0.5).sleep();
}
#endif

while (!has_new_image_)
{
ros::spinOnce();
ros::Duration(0.1).sleep();
}

// This check depends on the update period being much longer
// than the expected difference between now and the received image time
// TODO(lucasw)
// this likely isn't that robust - what if the testing system is really slow?
double time_diff = (ros::Time::now() - image_stamp_).toSec();
ROS_INFO_STREAM(time_diff);
EXPECT_LT(time_diff, 1.0);
cam_sub_.shutdown();
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "gazebo_camera_test");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
17 changes: 17 additions & 0 deletions gazebo_plugins/test/camera/camera.test
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<arg name="gui" default="false" />

<param name="/use_sim_time" value="true" />

<node name="gazebo" pkg="gazebo_ros" type="gzserver"
respawn="false" output="screen"
args="-r $(find gazebo_plugins)/test/camera/camera.world" />

<group if="$(arg gui)">
<node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>
</group>

<test test-name="camera" pkg="gazebo_plugins" type="camera-test"
clear_params="true" time-limit="30.0" />
</launch>
Loading