diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 485928832..d335c9fed 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -81,6 +81,10 @@ set_target_properties(gazebo_ros_paths_plugin PROPERTIES COMPILE_FLAGS "${cxx_fl set_target_properties(gazebo_ros_paths_plugin PROPERTIES LINK_FLAGS "${ld_flags}") target_link_libraries(gazebo_ros_paths_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +## Tests + +add_subdirectory(test) + # Install Gazebo System Plugins install(TARGETS gazebo_ros_api_plugin gazebo_ros_paths_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h b/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h index b35b00821..9c475da1f 100644 --- a/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h +++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h @@ -413,6 +413,9 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief index counters to count the accesses on models via GetModelState std::map access_count_get_model_state_; + + /// \brief enable the communication of gazebo information using ROS service/topics + bool enable_ros_network_; }; } #endif diff --git a/gazebo_ros/launch/empty_world.launch b/gazebo_ros/launch/empty_world.launch index 2abae931f..e663d848f 100644 --- a/gazebo_ros/launch/empty_world.launch +++ b/gazebo_ros/launch/empty_world.launch @@ -17,6 +17,7 @@ + @@ -35,6 +36,9 @@ + + + diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 2459ac08f..9f3591dcc 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -34,7 +34,8 @@ GazeboRosApiPlugin::GazeboRosApiPlugin() : plugin_loaded_(false), pub_link_states_connection_count_(0), pub_model_states_connection_count_(0), - pub_clock_frequency_(0) + pub_clock_frequency_(0), + enable_ros_network_(true) { robot_namespace_.clear(); } @@ -152,6 +153,8 @@ void GazeboRosApiPlugin::Load(int argc, char** argv) // below needs the world to be created first load_gazebo_ros_api_plugin_event_ = gazebo::event::Events::ConnectWorldCreated(boost::bind(&GazeboRosApiPlugin::loadGazeboRosApiPlugin,this,_1)); + nh_->getParam("enable_ros_network", enable_ros_network_); + plugin_loaded_ = true; ROS_INFO_NAMED("api_plugin", "Finished loading Gazebo ROS API Plugin."); } @@ -193,7 +196,20 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name) pub_model_states_connection_count_ = 0; /// \brief advertise all services - advertiseServices(); + if (enable_ros_network_) + advertiseServices(); + + // Manage clock for simulated ros time + pub_clock_ = nh_->advertise("/clock",10); + // set param for use_sim_time if not set by user already + if(!(nh_->hasParam("/use_sim_time"))) + nh_->setParam("/use_sim_time", true); + nh_->getParam("pub_clock_frequency", pub_clock_frequency_); +#if GAZEBO_MAJOR_VERSION >= 8 + last_pub_clock_time_ = world_->SimTime(); +#else + last_pub_clock_time_ = world_->GetSimTime(); +#endif // hooks for applying forces, publishing simtime on /clock wrench_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::wrenchBodySchedulerSlot,this)); @@ -498,18 +514,7 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); reset_world_service_ = nh_->advertiseService(reset_world_aso); - - // set param for use_sim_time if not set by user already - if(!(nh_->hasParam("/use_sim_time"))) - nh_->setParam("/use_sim_time", true); - // todo: contemplate setting environment variable ROBOT=sim here??? - nh_->getParam("pub_clock_frequency", pub_clock_frequency_); -#if GAZEBO_MAJOR_VERSION >= 8 - last_pub_clock_time_ = world_->SimTime(); -#else - last_pub_clock_time_ = world_->GetSimTime(); -#endif } void GazeboRosApiPlugin::onLinkStatesConnect() diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt new file mode 100644 index 000000000..ba34f9c4e --- /dev/null +++ b/gazebo_ros/test/CMakeLists.txt @@ -0,0 +1,22 @@ +set (rostests_python + ros_network/ros_network_default.test + ros_network/ros_network_disabled.test +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + foreach (rostest ${rostests_python}) + # We don't set a timeout here because we trust rostest to enforce the + # timeout specified in each .test file. + add_rostest(${rostest} rostest ${CMAKE_CURRENT_SOURCE_DIR}/${rostest}) + # Check for test result file and create one if needed. rostest can fail to + # generate a file if it throws an exception. + add_test(check_${rostest} rosrun rosunit check_test_ran.py + --rostest ${ROS_PACKAGE_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/${rostest}) + endforeach() +endif() + +install(PROGRAMS + ros_network/ros_api_checker + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test +) diff --git a/gazebo_ros/test/ros_network/gazebo_network_api.yaml b/gazebo_ros/test/ros_network/gazebo_network_api.yaml new file mode 100644 index 000000000..b681d3267 --- /dev/null +++ b/gazebo_ros/test/ros_network/gazebo_network_api.yaml @@ -0,0 +1,140 @@ +strict: true + +########################################################## +# Published topics +topics: + # System + - topic: /clock + type: rosgraph_msgs/Clock + num_publishers: 1 + num_subscribers: -1 + + - topic: /rosout + type: rosgraph_msgs/Log + num_publishers: -1 + num_subscribers: -1 + + # Gazebo + - topic: /gazebo/set_model_state + type: gazebo_msgs/ModelState + num_publishers: 0 + num_subscribers: -1 + + - topic: /gazebo/set_link_state + type: gazebo_msgs/LinkState + num_publishers: 0 + num_subscribers: -1 + + - topic: /gazebo/link_states + type: gazebo_msgs/LinkStates + num_publishers: 1 + num_subscribers: -1 + + - topic: /gazebo/model_states + type: gazebo_msgs/ModelStates + num_publishers: 1 + num_subscribers: -1 + + - topic: /gazebo/parameter_descriptions + type: dynamic_reconfigure/ConfigDescription + num_publishers: 1 + num_subscribers: -1 + + - topic: /gazebo/parameter_updates + type: dynamic_reconfigure/Config + num_publishers: 1 + num_subscribers: -1 + +########################################################## +# Published services +services: + # Gazebo + - service: /gazebo/apply_joint_effort + type: gazebo_msgs/ApplyJointEffort + + - service: /gazebo/get_physics_properties + type: gazebo_msgs/GetPhysicsProperties + + - service: /gazebo/set_link_state + type: gazebo_msgs/SetLinkState + + - service: /gazebo/set_joint_properties + type: gazebo_msgs/SetJointProperties + + - service: /gazebo/reset_world + type: std_srvs/Empty + + - service: /gazebo/set_model_configuration + type: gazebo_msgs/SetModelConfiguration + + - service: /gazebo/get_world_properties + type: gazebo_msgs/GetWorldProperties + + - service: /gazebo/delete_light + type: gazebo_msgs/DeleteLight + + - service: /gazebo/set_parameters + type: dynamic_reconfigure/Reconfigure + + - service: /gazebo/spawn_sdf_model + type: gazebo_msgs/SpawnModel + + - service: /gazebo/unpause_physics + type: std_srvs/Empty + + - service: /gazebo/pause_physics + type: std_srvs/Empty + + - service: /gazebo/get_joint_properties + type: gazebo_msgs/GetJointProperties + + - service: /gazebo/set_logger_level + type: roscpp/SetLoggerLevel + + - service: /gazebo/get_light_properties + type: gazebo_msgs/GetLightProperties + + - service: /gazebo/clear_body_wrenches + type: gazebo_msgs/BodyRequest + + - service: /gazebo/clear_joint_forces + type: gazebo_msgs/JointRequest + + - service: /gazebo/set_physics_properties + type: gazebo_msgs/SetPhysicsProperties + + - service: /gazebo/get_model_state + type: gazebo_msgs/GetModelState + + - service: /gazebo/reset_simulation + type: std_srvs/Empty + + - service: /gazebo/delete_model + type: gazebo_msgs/DeleteModel + + - service: /gazebo/spawn_urdf_model + type: gazebo_msgs/SpawnModel + + - service: /gazebo/set_link_properties + type: gazebo_msgs/SetLinkProperties + + - service: /gazebo/set_model_state + type: gazebo_msgs/SetModelState + + - service: /gazebo/apply_body_wrench + type: gazebo_msgs/ApplyBodyWrench + + - service: /gazebo/get_link_state + type: gazebo_msgs/GetLinkState + + - service: /gazebo/get_loggers + type: roscpp/GetLoggers + + - service: /gazebo/get_model_properties + type: gazebo_msgs/GetModelProperties + + - service: /gazebo/set_light_properties + type: gazebo_msgs/SetLightProperties + + - service: /gazebo/get_link_properties + type: gazebo_msgs/GetLinkProperties diff --git a/gazebo_ros/test/ros_network/no_gazebo_network_api.yaml b/gazebo_ros/test/ros_network/no_gazebo_network_api.yaml new file mode 100644 index 000000000..e4fc57c05 --- /dev/null +++ b/gazebo_ros/test/ros_network/no_gazebo_network_api.yaml @@ -0,0 +1,25 @@ +strict: true + +########################################################## +# Published topics +topics: + # System + - topic: /clock + type: rosgraph_msgs/Clock + num_publishers: 1 + num_subscribers: -1 + + - topic: /rosout + type: rosgraph_msgs/Log + num_publishers: -1 + num_subscribers: -1 + +########################################################## +# Published services +services: + # System + - service: /gazebo/set_logger_level + type: roscpp/SetLoggerLevel + + - service: /gazebo/get_loggers + type: roscpp/GetLoggers diff --git a/gazebo_ros/test/ros_network/ros_api_checker b/gazebo_ros/test/ros_network/ros_api_checker new file mode 100755 index 000000000..e0ce820d0 --- /dev/null +++ b/gazebo_ros/test/ros_network/ros_api_checker @@ -0,0 +1,196 @@ +#!/usr/bin/env python + +# The script was originally written by Brian Gerkey under +# the works of the Virtual Robotics Challenge +# +# Copyright Open Source Robotics Foundation +# + +from __future__ import print_function +import unittest +import rostest +import subprocess +import sys +import time +import re +import rospy + +class Tester(unittest.TestCase): + + def _test_extra_topics(self, topics): + cmd = ['rostopic', 'list'] + po = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + out, err = po.communicate() + self.assertEqual(po.returncode, 0, 'rostopic failed (%s). stdout: %s stderr: %s'%(cmd, out, err)) + topics_actual = set(out.split('\n')) - set(['']) + topics_expected = set([x['topic'] for x in topics]) + topics_extra = topics_actual - topics_expected + self.assertEqual(topics_extra, set([])) + + def _test_extra_services(self, services): + cmd = ['rosservice', 'list'] + po = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + out, err = po.communicate() + self.assertEqual(po.returncode, 0, 'rosservice failed (%s). stdout: %s stderr: %s'%(cmd, out, err)) + services_actual = set(out.split('\n')) - set(['']) + services_expected = set([x['service'] for x in services]) + services_extra = services_actual - services_expected + self.assertEqual(services_extra, set([])) + + def _test_topic(self, t): + self.assertIn('topic', t) + self.assertIn('type', t) + self.assertIn('num_publishers', t) + self.assertIn('num_subscribers', t) + + cmd = ['rostopic', 'info', t['topic']] + po = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + out, err = po.communicate() + self.assertEqual(po.returncode, 0, 'rostopic failed (%s). stdout: %s stderr: %s'%(cmd, out, err)) + self._parse_rostopic(t, out) + + def _parse_rostopic(self, t, out): + # Should probably do this through a library API instead... + + # Step 0: make sure we have enough output + outsplit = out.split('\n') + self.assertTrue(len(outsplit) >= 5) + + type_re = re.compile('\w*Type: (.*)') + pub_start_re = re.compile('Publishers:.*') + sub_start_re = re.compile('Subscribers:.*') + pub_sub_re = re.compile(' *\* *([^ ]*).*') + + # Step 1: check type + m = type_re.match(outsplit[0]) + self.assertEqual(len(m.groups()), 1) + self.assertEqual(m.groups()[0], t['type']) + + # Step 2: check num_publishers and num_subscribers + state = None + pubs = 0 + subs = 0 + for l in outsplit: + if pub_start_re.match(l): + state = 'in_pubs' + elif sub_start_re.match(l): + state = 'in_subs' + else: + m = pub_sub_re.match(l) + if m and len(m.groups()) == 1: + if state == 'in_pubs': + pubs += 1 + elif state == 'in_subs': + subs += 1 + if t['num_publishers'] >= 0: + self.assertEqual(pubs, t['num_publishers']) + if t['num_subscribers'] >= 0: + self.assertEqual(subs, t['num_subscribers']) + + def _test_service(self, s): + self.assertIn('service', s) + self.assertIn('type', s) + + cmd = ['rosservice', 'info', s['service']] + po = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + out, err = po.communicate() + self.assertEqual(po.returncode, 0, 'rosservice failed (%s). stdout: %s stderr: %s'%(cmd, out, err)) + self._parse_rosservice(s, out) + + def _parse_rosservice(self, s, out): + # Should probably do this through a library API instead... + + # Step 0: make sure we have enough output + outsplit = out.split('\n') + self.assertTrue(len(outsplit) >= 4) + + type_re = re.compile('Type: (.*)') + + # Step 1: check type + m = type_re.match(outsplit[2]) + self.assertEqual(len(m.groups()), 1) + self.assertEqual(m.groups()[0], s['type']) + +def load_config(files): + import yaml + topics = [] + services = [] + strict = False + for f in files: + # Ignore args passed in by rostest + if f[:2] == '--' or f[:2] == '__': + continue + # Let parsing exceptions leak out; we'll catch them by noticing the + # absence of a test result file. + y = yaml.load(open(f)) + for t in y['topics']: + topics.append(t) + if 'services' in y: + for s in y['services']: + services.append(s) + + # TODO: This logic will enforce strictness if any of the provided files + # sets strict to true, which isn't necessarily the right thing. + if 'strict' in y and y['strict']: + strict = True + return topics, services, strict + +def generate_topic_test(t): + def test_func(self): + self._test_topic(t) + return test_func + +def generate_service_test(t): + def test_func(self): + self._test_service(t) + return test_func + +def generate_test_extra_topics(topics): + def test_func(self): + self._test_extra_topics(topics) + return test_func + +def generate_test_extra_services(services): + def test_func(self): + self._test_extra_services(services) + return test_func + +def add_tests(topics, services, strict): + for t in topics: + test_func = generate_topic_test(t) + test_name = "test_topic_%s"%(t['topic'].replace('/','_')) + setattr(Tester, test_name, test_func) + for s in services: + test_func = generate_service_test(s) + test_name = "test_service_%s"%(s['service'].replace('/','_')) + setattr(Tester, test_name, test_func) + if strict: + test_func = generate_test_extra_topics(topics) + test_name = "test_extra_topics" + setattr(Tester, test_name, test_func) + test_func = generate_test_extra_services(services) + test_name = "test_extra_services" + setattr(Tester, test_name, test_func) + +if __name__ == '__main__': + rospy.init_node('rosapi_checker', anonymous=True) + + # Dynamically generate test cases and stuff them into the Tester class + topics, services, strict = load_config(sys.argv[1:]) + # The rostest node itself will advertise a couple of services + services.append({'service': '%s/get_loggers'%(rospy.get_name()), + 'type': 'roscpp/GetLoggers'}) + services.append({'service': '%s/set_logger_level'%(rospy.get_name()), + 'type': 'roscpp/SetLoggerLevel'}) + add_tests(topics, services, strict) + + # Wait until /clock is being published; this can take an unpredictable + # amount of time when we're downloading models. + while rospy.Time.now().to_sec() == 0.0: + print('Waiting for Gazebo to start...') + time.sleep(1.0) + # Take an extra nap, to allow plugins to be loaded + time.sleep(5.0) + print('OK, starting test.') + + rostest.run('gazebo_ros', 'api_check', Tester, sys.argv) diff --git a/gazebo_ros/test/ros_network/ros_network_default.test b/gazebo_ros/test/ros_network/ros_network_default.test new file mode 100644 index 000000000..9814b9078 --- /dev/null +++ b/gazebo_ros/test/ros_network/ros_network_default.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + diff --git a/gazebo_ros/test/ros_network/ros_network_disabled.test b/gazebo_ros/test/ros_network/ros_network_disabled.test new file mode 100644 index 000000000..27e2200b5 --- /dev/null +++ b/gazebo_ros/test/ros_network/ros_network_disabled.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + +