From 0a3955103604b960e405b11b1de3184acaf9d7df Mon Sep 17 00:00:00 2001
From: David Leins <dleins@techfak.uni-bielefeld.de>
Date: Tue, 7 Nov 2023 14:21:38 +0100
Subject: [PATCH] fix: repair sigint handler callback

---
 mujoco_ros/CHANGELOG.md |  3 ++
 mujoco_ros/src/main.cpp | 62 +++++++++++++++++++++--------------------
 2 files changed, 35 insertions(+), 30 deletions(-)

diff --git a/mujoco_ros/CHANGELOG.md b/mujoco_ros/CHANGELOG.md
index 8244c835..4f43a09b 100644
--- a/mujoco_ros/CHANGELOG.md
+++ b/mujoco_ros/CHANGELOG.md
@@ -1,6 +1,9 @@
 <a name="unreleased"></a>
 ## Unreleased
 
+### Fixed
+* Repaired SIGINT handler callback. `C-c` in the roslaunch terminal now shuts down the MuJoCo ROS node instead of escalating to SIGTERM.
+
 ### Changed
 * replaced `boost::shared_ptr` with `std::shared_ptr` or `std::unique_ptr` wherever possible (ROS 1 fast intra-process message-passing requires boost::shared_ptr).
 * replaced `shared_ptr` with `unique_ptr` wherever possible.
diff --git a/mujoco_ros/src/main.cpp b/mujoco_ros/src/main.cpp
index 8d9d0299..457341f0 100644
--- a/mujoco_ros/src/main.cpp
+++ b/mujoco_ros/src/main.cpp
@@ -47,9 +47,12 @@
 
 namespace {
 
+std::unique_ptr<mujoco_ros::MujocoEnv> env;
+
 void sigint_handler(int /*sig*/)
 {
-	ros::shutdown();
+	std::printf("Registered C-c. Shutting down MuJoCo ROS Server ...\n");
+	env->settings_.exit_request.store(1);
 }
 
 namespace po  = boost::program_options;
@@ -131,40 +134,39 @@ int main(int argc, char **argv)
 	}
 
 	// TODO(dleins): Should MuJoCo Plugins be loaded?
-	{
-		mujoco_ros::MujocoEnv env(admin_hash);
+	env = std::make_unique<mujoco_ros::MujocoEnv>(admin_hash);
 
-		bool no_x;
-		nh.param("no_x", no_x, false);
+	bool no_x;
+	nh.param("no_x", no_x, false);
 
-		if (!no_x) {
-			nh.param("headless", no_x, false);
-		}
+	if (!no_x) {
+		nh.param("headless", no_x, false);
+	}
 
-		// const char *filename = nullptr;
-		if (!filename.empty()) {
-			mju::strcpy_arr(env.queued_filename_, filename.c_str());
-			env.settings_.load_request = 2;
-		}
+	// const char *filename = nullptr;
+	if (!filename.empty()) {
+		mju::strcpy_arr(env->queued_filename_, filename.c_str());
+		env->settings_.load_request = 2;
+	}
 
-		env.startPhysicsLoop();
-		env.startEventLoop();
-
-		if (!no_x) {
-			ROS_INFO("Launching viewer");
-			// Start simulation UI loop (blocking)
-			// auto viewer = std::make_unique<mujoco_ros::Viewer>(std::make_unique<mujoco_ros::GlfwAdapter>(), &env,
-			//    /* fully_managed = */ true);
-			auto viewer = std::make_unique<mujoco_ros::Viewer>(
-			    std::unique_ptr<mujoco_ros::PlatformUIAdapter>(env.gui_adapter_), &env, /* fully_managed = */ true);
-			viewer->RenderLoop();
-		} else {
-			ROS_INFO("Running headless");
-		}
+	env->startPhysicsLoop();
+	env->startEventLoop();
+
+	if (!no_x) {
+		ROS_INFO("Launching viewer");
+		// Start simulation UI loop (blocking)
+		// auto viewer = std::make_unique<mujoco_ros::Viewer>(std::make_unique<mujoco_ros::GlfwAdapter>(), &env,
+		//    /* fully_managed = */ true);
+		auto viewer = std::make_unique<mujoco_ros::Viewer>(
+		    std::unique_ptr<mujoco_ros::PlatformUIAdapter>(env->gui_adapter_), env.get(), /* fully_managed = */ true);
+		viewer->RenderLoop();
+	} else {
+		ROS_INFO("Running headless");
+	}
 
-		env.waitForPhysicsJoin();
-		env.waitForEventsJoin();
-	} // end of scope for env for proper destruction
+	env->waitForPhysicsJoin();
+	env->waitForEventsJoin();
+	env.reset();
 
 	ROS_INFO("MuJoCo ROS Simulation Server node is terminating");