Parameter to disable ROS network interaction from/to Gazebo (lunar-devel)

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}

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<std::string, unsigned int> access_count_get_model_state_;
+
+  /// \brief enable the communication of gazebo information using ROS service/topics
+  bool enable_ros_network_;
 };
 }
 #endif

gazebo_ros/launch/empty_world.launch

@@ -17,6 +17,7 @@
   <arg name="respawn_gazebo" default="false"/>
   <arg name="use_clock_frequency" default="false"/>
   <arg name="pub_clock_frequency" default="100"/>
+  <arg name="enable_ros_network" default="true" />
 
   <!-- set use_sim_time flag -->
   <param name="/use_sim_time" value="$(arg use_sim_time)"/>
@@ -35,6 +36,9 @@
   <group if="$(arg use_clock_frequency)">
     <param name="gazebo/pub_clock_frequency" value="$(arg pub_clock_frequency)" />
   </group>
+  <group>
+    <param name="gazebo/enable_ros_network" value="$(arg enable_ros_network)" />
+  </group>
   <node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="$(arg respawn_gazebo)" output="screen"
 	args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)" />
 

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,21 @@ 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<rosgraph_msgs::Clock>("/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));
@@ -217,6 +234,12 @@ void GazeboRosApiPlugin::gazeboQueueThread()
 
 void GazeboRosApiPlugin::advertiseServices()
 {
+  if (! enable_ros_network_)
+  {
+    ROS_INFO_NAMED("api_plugin", "ROS gazebo topics/services are disabled");
+    return;
+  }
+
   // publish clock for simulated ros time
   pub_clock_ = nh_->advertise<rosgraph_msgs::Clock>("/clock",10);
 

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
+)

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

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