diff --git a/mujoco_ros_laser/CMakeLists.txt b/mujoco_ros_laser/CMakeLists.txt index 75c8fc5..4676e15 100644 --- a/mujoco_ros_laser/CMakeLists.txt +++ b/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() diff --git a/mujoco_ros_laser/package.xml b/mujoco_ros_laser/package.xml index 44c4826..bf371ec 100644 --- a/mujoco_ros_laser/package.xml +++ b/mujoco_ros_laser/package.xml @@ -17,6 +17,8 @@ mujoco_ros_sensors sensor_msgs + rostest + diff --git a/mujoco_ros_laser/src/laser.cpp b/mujoco_ros_laser/src/laser.cpp index 676c6b4..53cfce6 100644 --- a/mujoco_ros_laser/src/laser.cpp +++ b/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!"); diff --git a/mujoco_ros_laser/test/CMakeLists.txt b/mujoco_ros_laser/test/CMakeLists.txt new file mode 100644 index 0000000..f4c42af --- /dev/null +++ b/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 +# ) diff --git a/mujoco_ros_laser/test/launch/mujoco_ros_laser.test b/mujoco_ros_laser/test/launch/mujoco_ros_laser.test new file mode 100644 index 0000000..360db00 --- /dev/null +++ b/mujoco_ros_laser/test/launch/mujoco_ros_laser.test @@ -0,0 +1,13 @@ + + + + + + + + + + + + diff --git a/mujoco_ros_laser/test/mujoco_env_wrapper.h b/mujoco_ros_laser/test/mujoco_env_wrapper.h new file mode 100644 index 0000000..caba900 --- /dev/null +++ b/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 +#include +#include +#include + +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(); + } +}; diff --git a/mujoco_ros_laser/test/mujoco_ros_laser_test.cpp b/mujoco_ros_laser/test/mujoco_ros_laser_test.cpp new file mode 100644 index 0000000..a77b263 --- /dev/null +++ b/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 + +#include + +#include "mujoco_env_wrapper.h" + +#include +#include +#include +#include + +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 nh; + LaserPlugin *laser_plugin; + MujocoEnvTestWrapper *env_ptr; + + void SetUp() override + { + nh = std::make_unique("~"); + 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(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!"; +}