[ros2] make transient local reliable

gazebo_ros/CMakeLists.txt

@@ -1,7 +1,6 @@
 cmake_minimum_required(VERSION 3.5)
 project(gazebo_ros)
 
-
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
@@ -14,18 +13,14 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
-find_package(rcl REQUIRED)
-find_package(rclcpp REQUIRED)
 find_package(gazebo_dev REQUIRED)
 find_package(gazebo_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(rcl REQUIRED)
+find_package(rclcpp REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(tinyxml_vendor REQUIRED)
 
-include_directories(
-  include
-  ${gazebo_dev_INCLUDE_DIRS}
-)
 link_directories(
   ${gazebo_dev_LIBRARY_DIRS}
 )
@@ -37,6 +32,13 @@ add_library(gazebo_ros_node SHARED
   src/executor.cpp
   src/node.cpp
 )
+target_include_directories(gazebo_ros_node
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+    ${gazebo_dev_INCLUDE_DIRS}
+)
+
 ament_target_dependencies(gazebo_ros_node
   "gazebo_dev"
   "rclcpp"
@@ -48,6 +50,11 @@ ament_export_libraries(gazebo_ros_node)
 add_library(gazebo_ros_utils SHARED
   src/utils.cpp
 )
+target_include_directories(gazebo_ros_utils
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
 ament_target_dependencies(gazebo_ros_utils
   "gazebo_dev"
   "rclcpp"

gazebo_ros/include/gazebo_ros/node.hpp

@@ -138,7 +138,7 @@ Node::SharedPtr Node::CreateWithArgs(Args && ... args)
   // TODO(chapulina): use rclcpp::is_initialized() once that's available, see
   // https://github.com/ros2/rclcpp/issues/518
   Node::SharedPtr node;
-  if (!rclcpp::is_initialized()) {
+  if (!rclcpp::ok()) {
     rclcpp::init(0, nullptr);
     RCLCPP_INFO(internal_logger(), "ROS was initialized without arguments.");
   }

gazebo_ros/package.xml

@@ -40,6 +40,8 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>geometry_msgs</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>ros2run</test_depend>
   <test_depend>sensor_msgs</test_depend>
   <test_depend>std_msgs</test_depend>
 

gazebo_ros/scripts/spawn_entity.py

@@ -32,6 +32,7 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSDurabilityPolicy
+from rclpy.qos import QoSProfile
 from std_msgs.msg import String
 from std_srvs.srv import Empty
 
@@ -159,9 +160,11 @@ def entity_xml_cb(msg):
                 nonlocal entity_xml
                 entity_xml = msg.data
 
+            latched_qos = QoSProfile(
+                depth=1,
+                durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
             self.subscription = self.create_subscription(
-                String, self.args.topic, entity_xml_cb,
-                QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
+                String, self.args.topic, entity_xml_cb, latched_qos)
 
             while rclpy.ok() and entity_xml == '':
                 self.get_logger().info('Waiting for entity xml on %s' % self.args.topic)

gazebo_ros/src/gazebo_ros_force_system.cpp

@@ -22,6 +22,7 @@
 #include <gazebo_msgs/srv/link_request.hpp>
 #include <gazebo_ros/node.hpp>
 
+#include <algorithm>
 #include <memory>
 #include <string>
 #include <vector>
@@ -354,18 +355,17 @@ void GazeboRosForceSystemPrivate::ClearLinkWrenches(
   gazebo_msgs::srv::LinkRequest::Request::SharedPtr _req,
   gazebo_msgs::srv::LinkRequest::Response::SharedPtr /*_res*/)
 {
-  bool found = false;
   std::lock_guard<std::mutex> scoped_lock(lock_);
-  for (auto link_wrench_task = link_wrench_tasks_.begin();
-    link_wrench_task != link_wrench_tasks_.end(); ++link_wrench_task)
-  {
-    if ((*link_wrench_task)->link->GetScopedName() == _req->link_name) {
-      link_wrench_tasks_.erase(link_wrench_task--);
-      RCLCPP_INFO(ros_node_->get_logger(), "Deleted wrench on [%s]", _req->link_name.c_str());
-      found = true;
-    }
-  }
-  if (!found) {
+  auto prev_end = link_wrench_tasks_.end();
+  link_wrench_tasks_.erase(std::remove_if(link_wrench_tasks_.begin(), link_wrench_tasks_.end(),
+    [_req, this](auto & link_wrench_task) {
+      if (link_wrench_task->link->GetScopedName() == _req->link_name) {
+        RCLCPP_INFO(ros_node_->get_logger(), "Deleted wrench on [%s]", _req->link_name.c_str());
+        return true;
+      }
+      return false;
+    }), link_wrench_tasks_.end());
+  if (prev_end == link_wrench_tasks_.end()) {
     RCLCPP_WARN(ros_node_->get_logger(), "No applied wrenches on [%s]", _req->link_name.c_str());
   }
 }
@@ -401,18 +401,17 @@ void GazeboRosForceSystemPrivate::ClearJointEfforts(
   gazebo_msgs::srv::JointRequest::Request::SharedPtr _req,
   gazebo_msgs::srv::JointRequest::Response::SharedPtr /*_res*/)
 {
-  bool found = false;
   std::lock_guard<std::mutex> scoped_lock(lock_);
-  for (auto joint_effort_task = joint_effort_tasks_.begin();
-    joint_effort_task != joint_effort_tasks_.end(); ++joint_effort_task)
-  {
-    if ((*joint_effort_task)->joint->GetName() == _req->joint_name) {
-      joint_effort_tasks_.erase(joint_effort_task--);
-      RCLCPP_INFO(ros_node_->get_logger(), "Deleted effort on [%s]", _req->joint_name.c_str());
-      found = true;
-    }
-  }
-  if (!found) {
+  auto prev_end = joint_effort_tasks_.end();
+  joint_effort_tasks_.erase(std::remove_if(joint_effort_tasks_.begin(), joint_effort_tasks_.end(),
+    [_req, this](auto & joint_effort_task) {
+      if (joint_effort_task->joint->GetName() == _req->joint_name) {
+        RCLCPP_INFO(ros_node_->get_logger(), "Deleted effort on [%s]", _req->joint_name.c_str());
+        return true;
+      }
+      return false;
+    }), joint_effort_tasks_.end());
+  if (prev_end == joint_effort_tasks_.end()) {
     RCLCPP_WARN(ros_node_->get_logger(), "No applied efforts on [%s]", _req->joint_name.c_str());
   }
 }

gazebo_ros/src/gazebo_ros_init.cpp

@@ -119,7 +119,7 @@ GazeboRosInit::~GazeboRosInit()
 void GazeboRosInit::Load(int argc, char ** argv)
 {
   // Initialize ROS with arguments
-  if (!rclcpp::is_initialized()) {
+  if (!rclcpp::ok()) {
     rclcpp::init(argc, argv);
     impl_->ros_node_ = gazebo_ros::Node::Get();
   } else {

gazebo_ros/test/CMakeLists.txt

@@ -2,6 +2,7 @@ find_package(ament_cmake_gtest REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(gazebo_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(launch_testing_ament_cmake REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
@@ -68,3 +69,13 @@ foreach(test ${tests})
   )
 endforeach()
 
+add_executable(mock_robot_state_publisher
+  mock_robot_state_publisher/robot_state_publisher.cpp
+)
+ament_target_dependencies(mock_robot_state_publisher
+  "rclcpp"
+  "std_msgs"
+)
+add_launch_test(entity_spawner.test.py
+  ARGS "mock_robot_state_publisher:=${CMAKE_CURRENT_BINARY_DIR}/mock_robot_state_publisher"
+)

gazebo_ros/test/entity_spawner.test.py

@@ -0,0 +1,67 @@
+# Copyright 2020 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import unittest
+
+import ament_index_python
+
+import launch
+import launch.actions
+
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import launch_testing
+import launch_testing.asserts
+import launch_testing.markers
+import launch_testing.tools
+import pytest
+
+
+@pytest.mark.launch_test
+@launch_testing.markers.keep_alive
+def generate_test_description(ready_fn):
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(
+            'mock_robot_state_publisher',
+            description='Path to mock robot state publisher executable',
+        ),
+        launch.actions.ExecuteProcess(
+            cmd=[launch.substitutions.LaunchConfiguration('mock_robot_state_publisher')],
+            output='screen'
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                ament_index_python.get_package_share_directory('gazebo_ros'),
+                '/launch', '/gzserver.launch.py'
+                ])
+        ),
+        launch.actions.OpaqueFunction(function=lambda context: ready_fn())
+        # launch_testing.actions.ReadyToTest()
+    ])
+
+
+class TestTerminatingProc(unittest.TestCase):
+
+    def test_spawn_entity(self, launch_service, proc_info, proc_output):
+        """Test terminating_proc without command line arguments."""
+        print('Running spawn entity test on /mock_robot_description topic')
+        entity_spawner_action = launch.actions.ExecuteProcess(
+            cmd=['ros2', 'run', 'gazebo_ros', 'spawn_entity.py', '-entity',
+                 'mock_robot_state_entity', '-topic', '/mock_robot_description'],
+            output='screen'
+        )
+        with launch_testing.tools.launch_process(
+              launch_service, entity_spawner_action, proc_info, proc_output) as command:
+            assert command.wait_for_shutdown(timeout=10)
+        assert command.exit_code == launch_testing.asserts.EXIT_OK

gazebo_ros/test/mock_robot_state_publisher/robot_state_publisher.cpp

@@ -0,0 +1,51 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <iostream>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("mock_robot_state_publisher");
+  auto publisher = node->create_publisher<std_msgs::msg::String>(
+    "/mock_robot_description", rclcpp::QoS(1).transient_local());
+
+  std_msgs::msg::String message;
+  message.data = "<?xml version='1.0' ?>"
+    "<sdf version='1.5'>"
+    "<model name='ignored'>"
+    "<static>true</static>"
+    "<link name='link'>"
+    "<visual name='visual'>"
+    "<geometry>"
+    "<sphere><radius>1.0</radius></sphere>"
+    "</geometry>"
+    "</visual>"
+    "</link>"
+    "</model>"
+    "</sdf>";
+
+  RCLCPP_INFO(node->get_logger(), "Publishing on /mock_robot_description");
+  publisher->publish(message);
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

gazebo_ros/test/test_conversions.cpp

@@ -12,9 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <gtest/gtest.h>
+
 #include <gazebo_ros/conversions/builtin_interfaces.hpp>
 #include <gazebo_ros/conversions/geometry_msgs.hpp>
-#include <gtest/gtest.h>
 
 TEST(TestConversions, Vector3)
 {
@@ -117,9 +118,3 @@ TEST(TestConversions, Time)
     EXPECT_EQ(100, gazebo_time.nsec);
   }
 }
-
-int main(int argc, char ** argv)
-{
-  ::testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}

gazebo_ros/test/test_plugins.cpp

@@ -76,6 +76,7 @@ INSTANTIATE_TEST_CASE_P(Plugins, TestPlugins, ::testing::Values(
     TestParams({{"-s", "libgazebo_ros_init.so", "-s", "libgazebo_ros_factory.so",
       "worlds/ros_world_plugin.world"}, {"test"}}),
     TestParams({{"-s", "libgazebo_ros_init.so", "worlds/sdf_node_plugin.world"}, {"/foo/my_topic"}})
+    // cppcheck-suppress syntaxError
   ), );
 
 int main(int argc, char ** argv)