Skip to content

Commit

Permalink
#408 Making a test for multicamra that shows the timestamps are curre…
Browse files Browse the repository at this point in the history 
…ntly outdated, will fix them similar to how the regular camera was fixed.
  • Loading branch information
@lucasw @j-rivero
lucasw authored and j-rivero committed Feb 16, 2017
1 parent d92a409 commit 42ad400
Show file tree
Hide file tree
Showing 4 changed files with 263 additions and 1 deletion.
6 changes: 5 additions & 1 deletion gazebo_plugins/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -365,7 +365,11 @@ if (CATKIN_ENABLE_TESTING)
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
# Can't run these and the above test together
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)
Expand Down
101 changes: 101 additions & 0 deletions gazebo_plugins/test/camera/multicamera.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,101 @@
#include <gtest/gtest.h>
// #include <image_transport/image_transport.h>
// #include <image_transport/subscriber_filter.h>
// #include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>

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

ros::NodeHandle nh_;
// image_transport::SubscriberFilter cam_left_sub_;
// image_transport::SubscriberFilter cam_right_sub_;
message_filters::Subscriber<sensor_msgs::Image> cam_left_sub_;
message_filters::Subscriber<sensor_msgs::Image> cam_right_sub_;

bool has_new_image_;
ros::Time image_left_stamp_;
ros::Time image_right_stamp_;

// typedef message_filters::sync_policies::ApproximateTime<
// sensor_msgs::Image, sensor_msgs::Image
// > MySyncPolicy;
// message_filters::Synchronizer< MySyncPolicy > sync_;


public:
void imageCallback(
const sensor_msgs::ImageConstPtr& left_msg,
const sensor_msgs::ImageConstPtr& right_msg)
{
image_left_stamp_ = left_msg->header.stamp;
image_right_stamp_ = right_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(MultiCameraTest, cameraSubscribeTest)
{
// image_transport::ImageTransport it(nh_);
// cam_left_sub_.subscribe(it, "stereo/camera/left/image_raw", 1);
// cam_right_sub_.subscribe(it, "stereo/camera/right/image_raw", 1);
// sync_ = message_filters::Synchronizer<MySyncPolicy>(
// MySyncPolicy(4), cam_left_sub_, cam_right_sub_);
cam_left_sub_.subscribe(nh_, "stereo/camera/left/image_raw", 1);
cam_right_sub_.subscribe(nh_, "stereo/camera/right/image_raw", 1);
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> sync(
cam_left_sub_, cam_right_sub_, 4);
sync.registerCallback(boost::bind(&MultiCameraTest::imageCallback,
dynamic_cast<MultiCameraTest*>(this), _1, _2));
#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();
}

double sync_diff = (image_left_stamp_ - image_right_stamp_).toSec();
EXPECT_EQ(sync_diff, 0.0);

// 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_left_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_multicamera_test");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
11 changes: 11 additions & 0 deletions gazebo_plugins/test/camera/multicamera.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/multicamera.world" />

<test test-name="multicamera" pkg="gazebo_plugins" type="multicamera-test"
clear_params="true" time-limit="15.0" />
</launch>
146 changes: 146 additions & 0 deletions gazebo_plugins/test/camera/multicamera.world
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,146 @@
<?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="multicamera" name="stereo_camera">
<update_rate>0.1</update_rate>
<camera name="left">
<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>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<camera name="right">
<pose>0 -0.07 0 0 0 0</pose>
<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>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>stereo/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>left_camera_optical_frame</frameName>
<!--<rightFrameName>right_camera_optical_frame</rightFrameName>-->
<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 42ad400

Please sign in to comment.