Skip to content

Commit

Permalink
Fix for issue ros-simulation#408. The last measurement time is the ti…
Browse files Browse the repository at this point in the history 
…me that gazebo generated the sensor data, so ought to be used. updateRate doesn't seem that useful.

The other cameras need similar fixes to have the proper timestamps.
  • Loading branch information
@lucasw
lucasw committed Oct 18, 2016
1 parent 8a390af commit d00bd47
Show file tree
Hide file tree
Showing 5 changed files with 233 additions and 7 deletions.
6 changes: 6 additions & 0 deletions gazebo_plugins/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -369,4 +369,10 @@ if (CATKIN_ENABLE_TESTING)
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})

# Can't run this and the above test together
# add_rostest_gtest(camera-test
# test/camera/camera.test
# test/camera/camera.cpp)
# target_link_libraries(camera-test ${catkin_LIBRARIES})
endif()
20 changes: 13 additions & 7 deletions gazebo_plugins/src/gazebo_ros_camera.cpp
View file
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
72 changes: 72 additions & 0 deletions gazebo_plugins/test/camera/camera.cpp
View file
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, 0.5);
cam_sub_.shutdown();
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "gazebo_camera_test");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
11 changes: 11 additions & 0 deletions gazebo_plugins/test/camera/camera.test
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<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" />

<test test-name="camera" pkg="gazebo_plugins" type="camera-test"
clear_params="true" time-limit="30.0" />
</launch>
131 changes: 131 additions & 0 deletions gazebo_plugins/test/camera/camera.world
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
<?xml version="1.0" ?>
<sdf version="1.4">

<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>

<!-- Global light source -->
<include>
<uri>model://sun</uri>
</include>

<!-- Focus camera on tall pendulum -->
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>

<model name="model_1">
<static>false</static>
<pose>0.0 2.0 2.0 0.0 0.0 0.0</pose>
<link name="link_1">
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<inertial>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<inertia>
<ixx>1.0</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>1.0</iyy>
<iyz>0.0</iyz>
<izz>1.0</izz>
</inertia>
<mass>10.0</mass>
</inertial>
<visual name="visual_sphere">
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<material>
<ambient>0.03 0.5 0.5 1.0</ambient>
<script>Gazebo/Green</script>
</material>
<cast_shadows>true</cast_shadows>
<laser_retro>100.0</laser_retro>
</visual>
<collision name="collision_sphere">
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<max_contacts>250</max_contacts>
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>0.5</mu>
<mu2>0.2</mu2>
<fdir1>1.0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1000000.0</threshold>
</bounce>
<contact>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e15</kp>
<kd>1e13</kd>
<max_vel>100.0</max_vel>
<min_depth>0.0001</min_depth>
</ode>
</contact>
</surface>
<laser_retro>100.0</laser_retro>
</collision>

<sensor type="camera" name="camera1">
<update_rate>0.5</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<!-- Keep this zero, update_rate will control the frame rate -->
<updateRate>0.0</updateRate>
<cameraName>camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</link>
</model>

</world>
</sdf>

0 comments on commit d00bd47

Please sign in to comment.