fix: reverts clang-tidy patch breaking lasers

mujoco_ros_laser/CMakeLists.txt

@@ -67,3 +67,7 @@ install(FILES
 install(DIRECTORY launch config assets
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+if (CATKIN_ENABLE_TESTING)
+  add_subdirectory(test)
+endif()

mujoco_ros_laser/package.xml

@@ -17,6 +17,8 @@
   <depend>mujoco_ros_sensors</depend>
   <depend>sensor_msgs</depend>
 
+  <test_depend>rostest</test_depend>
+
   <export>
     <mujoco_ros plugin="${prefix}/mujoco_laser_plugin.xml" />
   </export>

mujoco_ros_laser/src/laser.cpp

@@ -120,9 +120,9 @@ bool LaserPlugin::load(const mjModel *m, mjData *d)
 
 	if (rosparam_config_["sensors"].getType() == XmlRpc::XmlRpcValue::TypeArray) {
 		// iterate through configs
-		ROS_WARN_NAMED("lasers", "Is array");
-		for (auto &i : rosparam_config_["sensors"]) {
-			initSensor(m, i.second);
+		// NOLINTNEXTLINE(modernize-loop-convert) range-based for loop throws XmlRpcValue Exception
+		for (uint i = 0; i < rosparam_config_["sensors"].size(); i++) {
+			initSensor(m, rosparam_config_["sensors"][i]);
 		}
 	} else {
 		ROS_ERROR_NAMED("lasers", "Sensors config is not an array!");

mujoco_ros_laser/test/CMakeLists.txt

@@ -0,0 +1,21 @@
+find_package(rostest REQUIRED)
+
+add_rostest_gtest(mujoco_ros_laser_test
+  launch/mujoco_ros_laser.test
+  mujoco_ros_laser_test.cpp
+)
+
+add_dependencies(mujoco_ros_laser_test
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(mujoco_ros_laser_test
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+# install(FILES
+#   empty_world.xml
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+# )

mujoco_ros_laser/test/launch/mujoco_ros_laser.test

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+
+  <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${logger}] [${node}]: ${message}"/>
+  <env name="ROSCONSOLE_CONFIG_FILE"
+       value="$(find mujoco_ros)/config/rosconsole.config"/>
+
+  <param name="/use_sim_time" value="true"/>
+
+  <rosparam file="$(find mujoco_ros_laser)/config/laser_example_config.yaml" />
+
+  <test test-name="mujoco_ros_laser_test" pkg="mujoco_ros_laser" type="mujoco_ros_laser_test" time-limit="200.0"/>
+</launch>

mujoco_ros_laser/test/mujoco_env_wrapper.h

@@ -0,0 +1,87 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023-2024, Bielefeld University
+ *  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 Bielefeld University 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.
+ *********************************************************************/
+
+/* Authors: David P. Leins */
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <mujoco_ros/mujoco_env.h>
+
+using namespace mujoco_ros;
+namespace mju = ::mujoco::sample_util;
+
+class MujocoEnvTestWrapper : public MujocoEnv
+{
+public:
+	MujocoEnvTestWrapper(const std::string &admin_hash = std::string()) : MujocoEnv(admin_hash) {}
+	mjModel *getModelPtr() { return model_.get(); }
+	mjData *getDataPtr() { return data_.get(); }
+	MujocoEnvMutex *getMutexPtr() { return &physics_thread_mutex_; }
+	int getPendingSteps() { return num_steps_until_exit_; }
+
+	void setEvalMode(bool eval_mode) { settings_.eval_mode = eval_mode; }
+	void setAdminHash(const std::string &hash) { mju::strcpy_arr(settings_.admin_hash, hash.c_str()); }
+
+	std::string getFilename() { return { filename_ }; }
+	int isPhysicsRunning() { return is_physics_running_; }
+	int isEventRunning() { return is_event_running_; }
+	int isRenderingRunning() { return is_rendering_running_; }
+
+	int getNumCBReadyPlugins() { return cb_ready_plugins_.size(); }
+	void notifyGeomChange() { notifyGeomChanged(0); }
+
+	void load_filename(const std::string &filename)
+	{
+		mju::strcpy_arr(queued_filename_, filename.c_str());
+		settings_.load_request = 2;
+	}
+
+	void shutdown()
+	{
+		settings_.exit_request = 1;
+		waitForPhysicsJoin();
+		waitForEventsJoin();
+	}
+
+	const std::string &getHandleNamespace() { return nh_->getNamespace(); }
+
+	void startWithXML(const std::string &xml_path)
+	{
+		mju::strcpy_arr(queued_filename_, xml_path.c_str());
+		settings_.load_request = 2;
+		startPhysicsLoop();
+		startEventLoop();
+	}
+};

mujoco_ros_laser/test/mujoco_ros_laser_test.cpp

@@ -0,0 +1,112 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, Bielefeld University
+ *  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 Bielefeld University 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.
+ *********************************************************************/
+
+/* Authors: David P. Leins */
+
+#include <gtest/gtest.h>
+
+#include <ros/package.h>
+
+#include "mujoco_env_wrapper.h"
+
+#include <mujoco_ros_laser/laser.h>
+#include <mujoco_ros/mujoco_env.h>
+#include <mujoco_ros/plugin_utils.h>
+#include <string>
+
+using namespace mujoco_ros::sensors::laser;
+
+int main(int argc, char **argv)
+{
+	testing::InitGoogleTest(&argc, argv);
+	ros::init(argc, argv, "mujoco_ros_laser_test");
+
+	// Create spinner to communicate with ROS
+	ros::AsyncSpinner spinner(1);
+	spinner.start();
+	ros::NodeHandle nh;
+	int ret = RUN_ALL_TESTS();
+
+	// Stop spinner and shutdown ROS before returning
+	spinner.stop();
+	ros::shutdown();
+	return ret;
+}
+
+class LoadedPluginFixture : public ::testing::Test
+{
+protected:
+	std::unique_ptr<ros::NodeHandle> nh;
+	LaserPlugin *laser_plugin;
+	MujocoEnvTestWrapper *env_ptr;
+
+	void SetUp() override
+	{
+		nh = std::make_unique<ros::NodeHandle>("~");
+		nh->setParam("unpause", false);
+		nh->setParam("no_x", true);
+		nh->setParam("use_sim_time", true);
+
+		env_ptr              = new MujocoEnvTestWrapper();
+		std::string xml_path = ros::package::getPath("mujoco_ros_laser") + "/assets/laser_world.xml";
+		env_ptr->startWithXML(xml_path);
+
+		float seconds = 0;
+		while (env_ptr->getOperationalStatus() != 0 && seconds < 2) {
+			std::this_thread::sleep_for(std::chrono::milliseconds(1));
+			seconds += 0.001;
+		}
+		EXPECT_LT(seconds, 2) << "Env loading ran into 2 seconds timeout!";
+
+		auto &plugins = env_ptr->getPlugins();
+		for (const auto &p : plugins) {
+			laser_plugin = dynamic_cast<LaserPlugin *>(p.get());
+			if (laser_plugin != nullptr) {
+				break;
+			}
+		}
+	}
+
+	void TearDown() override
+	{
+		laser_plugin = nullptr;
+		env_ptr->shutdown();
+		delete env_ptr;
+	}
+};
+
+TEST_F(LoadedPluginFixture, ControlCallback)
+{
+	EXPECT_NE(laser_plugin, nullptr) << "Plugin loading failed!";
+}