Skip to content

Commit

Permalink
Fix occasional crash during shutdown when explicitly calling ros::sta…
Browse files Browse the repository at this point in the history
…rt but not ros::shutdown (#2355)

* Fix occasional crash during shutdown

* add link to PR

* comment

* fix implementation

* add missing hasError = true;

* also call deInit

* only deInit once

* only deinit once

* yet more fixes

* add another test for init only

* revert

* preserve legacy behavior

* add gtest wrapper

* minimize code changes

* add test

* reduce changes even more

* add comment

* comment
  • Loading branch information
dgossow authored Nov 21, 2023
1 parent 030e132 commit 845f746
Show file tree
Hide file tree
Showing 7 changed files with 233 additions and 1 deletion.
11 changes: 11 additions & 0 deletions clients/roscpp/src/libros/init.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ static CallbackQueuePtr g_internal_callback_queue;
static bool g_initialized = false;
static bool g_started = false;
static bool g_atexit_registered = false;
static bool g_shutdown_registered = false;
static boost::mutex g_start_mutex;
static bool g_ok = false;
static uint32_t g_init_options = 0;
Expand Down Expand Up @@ -408,6 +409,16 @@ void start()
if (g_shutting_down) goto end;

g_internal_queue_thread = boost::thread(internalCallbackQueueThreadFunc);

// Ensure that shutdown() is always called when the program exits,
// but before singletons get destroyed, so that the internal queue thread is joined first
// and cannot access destroyed singletons.
if (!g_shutdown_registered)
{
g_shutdown_registered = true;
atexit(shutdown);
}

getGlobalCallbackQueue()->enable();

ROSCPP_LOG_DEBUG("Started node [%s], pid [%d], bound on [%s], xmlrpc port [%d], tcpros port [%d], using [%s] time",
Expand Down
4 changes: 4 additions & 0 deletions test/test_roscpp/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,10 @@ add_rostest(launch/service_call_zombie.xml)
# Repeatedly call ros::init() and ros::fini()
add_rostest(launch/multiple_init_fini.xml)

# Check that ros::shutdown is automatically called
# if ros::start has been called explcitly
add_rostest(launch/missing_call_to_shutdown.xml)

# Test node inspection functionality
add_rostest(launch/inspection.xml)

Expand Down
3 changes: 3 additions & 0 deletions test/test_roscpp/test/launch/missing_call_to_shutdown.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
<launch>
<test test-name="missing_call_to_shutdown" pkg="test_roscpp" type="test_roscpp-missing_call_to_shutdown"/>
</launch>
10 changes: 10 additions & 0 deletions test/test_roscpp/test/src/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,12 @@ add_dependencies(${PROJECT_NAME}-service_exception ${std_srvs_EXPORTED_TARGETS})
add_executable(${PROJECT_NAME}-multiple_init_fini EXCLUDE_FROM_ALL multiple_init_fini.cpp)
target_link_libraries(${PROJECT_NAME}-multiple_init_fini ${GTEST_LIBRARIES} ${catkin_LIBRARIES})

add_executable(${PROJECT_NAME}-missing_call_to_shutdown_impl EXCLUDE_FROM_ALL missing_call_to_shutdown_impl.cpp)
target_link_libraries(${PROJECT_NAME}-missing_call_to_shutdown_impl ${catkin_LIBRARIES})

add_executable(${PROJECT_NAME}-missing_call_to_shutdown EXCLUDE_FROM_ALL missing_call_to_shutdown.cpp)
target_link_libraries(${PROJECT_NAME}-missing_call_to_shutdown ${GTEST_LIBRARIES} ${catkin_LIBRARIES})

# Test node inspection functionality
add_executable(${PROJECT_NAME}-inspection EXCLUDE_FROM_ALL inspection.cpp)
target_link_libraries(${PROJECT_NAME}-inspection ${GTEST_LIBRARIES} ${catkin_LIBRARIES})
Expand Down Expand Up @@ -256,6 +262,8 @@ if(TARGET tests)
${PROJECT_NAME}-service_exception
${PROJECT_NAME}-service_call_repeatedly
${PROJECT_NAME}-multiple_init_fini
${PROJECT_NAME}-missing_call_to_shutdown
${PROJECT_NAME}-missing_call_to_shutdown_impl
${PROJECT_NAME}-inspection
${PROJECT_NAME}-service_adv_multiple
${PROJECT_NAME}-service_adv_a
Expand Down Expand Up @@ -324,6 +332,8 @@ add_dependencies(${PROJECT_NAME}-service_deadlock ${${PROJECT_NAME}_EXPORTED_TAR
add_dependencies(${PROJECT_NAME}-service_exception ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}-service_call_repeatedly ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}-multiple_init_fini ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}-missing_call_to_shutdown ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}-missing_call_to_shutdown_impl ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}-inspection ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}-service_adv_multiple ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}-service_adv_a ${${PROJECT_NAME}_EXPORTED_TARGETS})
Expand Down
52 changes: 52 additions & 0 deletions test/test_roscpp/test/src/missing_call_to_shutdown.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

/* Author: David Gossow */

#include <gtest/gtest.h>
#include <cstdlib>

TEST(roscpp, missingCallToShutdownInitOnly)
{
int exit_code = system("rosrun test_roscpp test_roscpp-missing_call_to_shutdown_impl 0");
EXPECT_EQ(0, exit_code);
}

TEST(roscpp, missingCallToShutdownInitAndStart)
{
int exit_code = system("rosrun test_roscpp test_roscpp-missing_call_to_shutdown_impl 1");
EXPECT_EQ(0, exit_code);
}

int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
140 changes: 140 additions & 0 deletions test/test_roscpp/test/src/missing_call_to_shutdown_impl.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,140 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

/* Author: David Gossow */

/*
* Call ros::start() explicitly, but never call ros::shutdown().
* ros::shutdown should be called automatically in this case.
*/

#include <cstdlib>
#include <ros/ros.h>

namespace ros
{
namespace console
{
extern bool g_shutting_down;
}
}

namespace
{

enum TestId
{
InitOnly = 0,
InitAndStart = 1
};

TestId test_id = InitOnly;

void atexitCallback()
{
bool hasError = false;

if (ros::ok())
{
std::cerr << "ERROR: ros::ok() returned true!" << std::endl;
hasError = true;
}
if (!ros::isShuttingDown())
{
std::cerr << "ERROR: ros::isShuttingDown() returned false!" << std::endl;
hasError = true;
}
if (ros::isStarted())
{
std::cerr << "ERROR: ros::isStarted() returned true!" << std::endl;
hasError = true;
}

if (!ros::isInitialized())
{
std::cerr << "ERROR: ros::isInitialized() returned false, although ros::init was called!" << std::endl;
std::cerr << "Due to legacy reasons, it should return true, even after ROS has been de-initialized." << std::endl;
hasError = true;
}

if (!ros::console::g_shutting_down)
{
std::cerr << "ERROR: ros::console::g_shutting_down returned false, but it should have been automatically shut down." << std::endl;
hasError = true;
}

if (hasError)
{
std::_Exit(1);
}
}
}

int
main(int argc, char** argv)
{
if ( argc > 1 )
{
test_id = static_cast<TestId>(atoi(argv[1]));
}

// Register atexit callbak which will be executed after ROS has been de-initialized.
if (atexit(atexitCallback) != 0)
{
std::cerr << "Failed to register atexit callback." << std::endl;
return 1;
}

switch (test_id)
{
case InitOnly:
// Test case 0: Call ros::init() explicitly, but never call ros::shutdown().
// ros::deInit should be called automatically in this case.
ros::init(argc, argv, "missing_call_to_shutdown" );
break;
case InitAndStart:
// Test case 1: Call ros::init() and ros::start() explicitly, but never call ros::shutdown().
// ros::shutdown should be called automatically in this case.
ros::init(argc, argv, "missing_call_to_shutdown" );
ros::start();
break;
default:
std::cerr << "Invalid test id: " << test_id << std::endl;
return 1;
break;
}

if (!ros::ok())
{
std::cerr << "Failed to start ROS." << std::endl;
return 1;
}

return 0;
}
14 changes: 13 additions & 1 deletion test/test_roscpp/test/src/multiple_init_fini.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,13 +41,18 @@
#include <stdlib.h>

#include "ros/ros.h"
#include "ros/callback_queue.h"

#include <test_roscpp/TestArray.h>

int g_argc;
char** g_argv;

bool g_got_callback = false;

void callback(const test_roscpp::TestArrayConstPtr&)
{
g_got_callback = true;
}

TEST(roscpp, multipleInitAndFini)
Expand All @@ -60,15 +65,22 @@ TEST(roscpp, multipleInitAndFini)

for ( int i = 0; i < try_count; ++i )
{
g_got_callback = false;

ros::init( g_argc, g_argv, "multiple_init_fini" );
ros::NodeHandle nh;

ros::Subscriber sub = nh.subscribe("test", 1, callback);
ASSERT_TRUE(sub);

ros::Publisher pub = nh.advertise<test_roscpp::TestArray>( "test2", 1 );
ros::Publisher pub = nh.advertise<test_roscpp::TestArray>( "test", 1 );
ASSERT_TRUE(pub);

pub.publish(test_roscpp::TestArray());
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1));

ASSERT_TRUE(g_got_callback) << "did not receive a message in iteration " << i;

ros::shutdown();
}
}
Expand Down

0 comments on commit 845f746

Please sign in to comment.