feat: adds laser plugin

ubi-agni · Aug 2, 2024 · 27abfbc · 27abfbc
1 parent 7ab6ad0
commit 27abfbc
Show file tree

Hide file tree

Showing 10 changed files with 722 additions and 0 deletions.
diff --git a/mujoco_ros/CHANGELOG.md b/mujoco_ros/CHANGELOG.md
@@ -7,6 +7,7 @@ If a number above 1 is used, multithreading is enabled. MuJoCo will then use the
 The default is `min(#available_threads - 1, 4)`.
 * Added profiling of plugin load and callback execution times. For each callback an EMA with sensitivity of 1000 steps is computed. In case a plugin has lower frequency than simulation step size, the callback should set `skip_ema_ = true` when skipping computations.
 Loading and reset times are reported in the server debug log. All plugin stats can be retrieved by the `get_plugin_stats` service call.
+* Added ros laser plugin.
 
 ### Fixed
 * Added missing call to render callbacks in viewer. While the callbacks were still being run for offscreen rendering, the viewer did not render additional geoms added by plugins.

diff --git a/mujoco_ros_laser/CMakeLists.txt b/mujoco_ros_laser/CMakeLists.txt
@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.0.2)
+
+# TODO: Set the actual project name
+project(mujoco_ros_laser)
+
+# Optional: change/add more flags
+set( CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--no-undefined" )
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+  message(STATUS "CMAKE_BUILD_TYPE not set. Defaulting to 'Release'.")
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+set(CMAKE_CXX_FLAGS_SANITIZE "-fsanitize=address -g -O1 -fno-inline -fno-omit-frame-pointer -fno-optimize-sibling-calls")
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  pluginlib
+  mujoco_ros
+  mujoco_ros_sensors
+  sensor_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    roscpp
+    pluginlib
+    mujoco_ros
+    mujoco_ros_sensors
+    sensor_msgs
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+)
+
+add_library(${PROJECT_NAME}
+  src/laser.cpp
+)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  ${PROJECT_SOURCE_DIR}/include
+  ${catkin_INCLUDE_DIRS}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(FILES
+  mujoco_laser_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY launch config assets
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/mujoco_ros_laser/README.md b/mujoco_ros_laser/README.md
@@ -0,0 +1,13 @@
+# MuJoCo ROS Laser Plugin
+
+Provides a CPU-based Laser sensor, using MuJoCo's `ray_collision` function.
+
+This plugin uses MuJoCo's threadpool, if using more than 1 thread is configured in the server.
+
+## Configuration Example
+
+https://github.com/ubi-agni/mujoco_ros/blob/noetic-devel/mujoco_ros_laser/config/laser_example_config.yaml?plain=1
+
+## License
+
+This repository is licensed under the [BSD License](../mujoco_ros/LICENSE).
diff --git a/mujoco_ros_laser/assets/laser_world.xml b/mujoco_ros_laser/assets/laser_world.xml
@@ -0,0 +1,38 @@
+<mujoco model="Laser Example">
+    <option timestep="0.001" gravity="0 0 -9.81" cone="elliptic" />
+    <compiler angle="radian" />
+
+    <visual>
+        <headlight ambient="0.4 0.4 0.4" diffuse="0.4 0.4 0.4" specular="0.0 0.0 0.0" active="1" />
+    </visual>
+
+    <asset>
+        <texture builtin="checker" height="512" name="texplane" rgb1=".2 .3 .4" rgb2=".1 .15 .2" type="2d" width="512" />
+        <material name="MatPlane" reflectance="0.5" shininess="0.01" specular="0.1" texrepeat="1 1" texture="texplane" texuniform="true" />
+    </asset>
+
+    <worldbody>
+        <light pos="0 0 1000" castshadow="false" />
+        <geom name="ground_plane" type="plane" size="10 10 10" material="MatPlane" rgba="1 1 1 1"/>
+
+        <body name="obs0" pos="5. -2.5 0.2" euler="0 0 1.2">
+            <freejoint />
+            <geom type="box" size="0.2 0.2 0.2" rgba="1. 0 0. 1." />
+        </body>
+
+        <body name="obs1" pos="7. 0. 0.2">
+            <freejoint />
+            <geom type="box" size="0.2 0.2 0.2" rgba="1. 0 0. 1." />
+        </body>
+
+        <body name="obs2" pos="6. 2.5 0.2" euler="0 0 .9">
+            <freejoint />
+            <geom type="box" size="0.2 0.2 0.2" rgba="1. 0 0. 1." />
+        </body>
+
+        <body name="laser" pos="0 0 .3">
+            <geom type="box" size=".2 .2 .3" rgba="0.8 0.8 0.8 0.2" />
+            <site pos=".21 0 -0.1" name="laser_site" size="0.05" />
+        </body>
+    </worldbody>
+</mujoco>
diff --git a/mujoco_ros_laser/config/laser_example_config.yaml b/mujoco_ros_laser/config/laser_example_config.yaml
@@ -0,0 +1,13 @@
+MujocoPlugins:
+  - type: mujoco_ros_laser/LaserPlugin
+    sensors:
+    - site_attached: laser_site
+      frame_id: scan
+      visualize: true
+      update_rate: 10.
+      min_range: 0.1
+      max_range: 30.
+      range_resolution: 0.01
+      angular_resolution: 0.02
+      min_angle: -1.57
+      max_angle: 1.57
diff --git a/mujoco_ros_laser/include/mujoco_ros_laser/laser.h b/mujoco_ros_laser/include/mujoco_ros_laser/laser.h
@@ -0,0 +1,157 @@
+/*********************************************************************
+ * 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 */
+
+#pragma once
+
+#include <mujoco_ros/plugin_utils.h>
+#include <mujoco_ros/common_types.h>
+#include <mujoco_ros/mujoco_env.h>
+
+#include <mujoco_ros_sensors/mujoco_sensor_handler_plugin.h>
+
+#include <ros/ros.h>
+
+#include <random>
+
+using namespace mujoco_ros;
+using namespace mujoco_ros::sensors;
+
+namespace mujoco_ros::sensors::laser {
+
+// Defaults
+static bool DEFAULT_VISUALIZE            = false;
+static double DEFAULT_UPDATE_RATE        = 10.;
+static double DEFAULT_MIN_RANGE          = 0.1;
+static double DEFAULT_MAX_RANGE          = 30.;
+static double DEFAULT_RANGE_RESOLUTION   = 0.01;
+static double DEFAULT_ANGULAR_RESOLUTION = 0.02;
+static double DEFAULT_MIN_ANGLE          = -1.57;
+static double DEFAULT_MAX_ANGLE          = 1.57;
+static double DEFAULT_SENSOR_STD         = 0.005;
+
+struct LaserConfig : public SensorConfig
+{
+public:
+	LaserConfig(const XmlRpc::XmlRpcValue &config, const std::string &frame_id, const std::string &name,
+	            int site_attached); // : SensorConfig(std::move(frame_id)), name(std::move(name)),
+	                                // site_attached(site_attached);
+	LaserConfig(std::string frame_id, std::string name, int site_attached, bool visualize, double update_rate,
+	            double min_range, double max_range, double range_resolution, double angular_resolution, double min_angle,
+	            double max_angle)
+	    : SensorConfig(std::move(frame_id))
+	    , name(std::move(name))
+	    , site_attached(site_attached)
+	    , visualize(visualize)
+	    , update_rate(update_rate)
+	    , min_range(min_range)
+	    , max_range(max_range)
+	    , range_resolution(range_resolution)
+	    , angular_resolution(angular_resolution)
+	    , min_angle(min_angle)
+	    , max_angle(max_angle){};
+
+	// Sensor name
+	std::string name;
+	// Site in the model to attach the laser to
+	int site_attached;
+	// Whether to visualize the laser scan in the simulation
+	bool visualize;
+	// The update rate of the laser scan in Hz
+	double update_rate;
+	// The minimum range of the laser scan
+	mjtNum min_range;
+	// The maximum range of the laser scan
+	mjtNum max_range;
+	// The range resolution of the laser scan in meters
+	double range_resolution;
+	// The angular resolution of the laser scan in radians
+	double angular_resolution;
+	// The minimum and maximum angle of the laser scan in radians
+	double min_angle;
+	double max_angle;
+
+	uint nrays;
+	mjtNum *rays;
+
+	mjtNum cur_xpos[3];
+};
+
+class LaserPlugin : public MujocoPlugin
+{
+public:
+	~LaserPlugin() override;
+
+	bool load(const mjModel *m, mjData *d) override;
+	void reset() override;
+
+	void renderCallback(const mjModel *model, mjData *data, mjvScene *scene) override;
+	void lastStageCallback(const mjModel *model, mjData *data) override;
+
+private:
+	// The env_ptr_ (shared_ptr) in the parent class ensures mjModel and mjData are not destroyed
+	const mjModel *m_;
+	mjData *d_;
+
+	// Laser sensor configurations
+	std::vector<LaserConfig> laser_configs_;
+	// Handle for publishers of laser scan messages
+	ros::NodeHandle lasers_nh_;
+
+	// Last sim time the laser was computed
+	ros::Time last_update_time_;
+
+	// Initialize the laser sensor configuration
+	bool initSensor(const mjModel *model, const XmlRpc::XmlRpcValue &config);
+
+	// Laser computation
+	void computeLasers(const mjModel *model, mjData *data);
+	// Multi-threaded computation of laser rays
+	void computeLasersMultithreaded(const mjModel *model, mjData *data);
+
+	// Laser visualization geoms
+	mjvGeom *laser_geoms_;
+	// Number of laser geoms
+	int ngeom_ = 0;
+
+	// Whether at least one sensor is to be rendered
+	bool has_render_data_ = false;
+
+	// Random number generator for sensor noise
+	std::mt19937 rand_generator = std::mt19937(std::random_device{}());
+	// Normal distribution for sensor noise
+	std::normal_distribution<double> noise_dist;
+};
+} // namespace mujoco_ros::sensors::laser
diff --git a/mujoco_ros_laser/launch/laser_plugin_example.launch b/mujoco_ros_laser/launch/laser_plugin_example.launch
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+  <launch>
+  <arg name="modelfile"            default="$(find mujoco_ros_laser)/assets/laser_world.xml" doc="MuJoCo xml file to load. Should define robot model and world." />
+  <arg name="mujoco_plugin_config" default="$(find mujoco_ros_laser)/config/laser_example_config.yaml"                                        doc="Optionally provide the path to a yaml with plugin configurations to load" />
+  <arg name="verbose" default="false" />
+
+  <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${logger}] [${node}]: ${message}"/>
+  <env if="$(arg verbose)" name="ROSCONSOLE_CONFIG_FILE"
+       value="$(find mujoco_ros)/rosconsole.config"/>
+
+  <include file="$(find mujoco_ros)/launch/launch_server.launch" pass_all_args="true">
+    <arg name="use_sim_time" value="true" />
+  </include>
+</launch>
diff --git a/mujoco_ros_laser/mujoco_laser_plugin.xml b/mujoco_ros_laser/mujoco_laser_plugin.xml
@@ -0,0 +1,10 @@
+<library path="lib/libmujoco_ros_laser">
+  <class
+    name="mujoco_ros_laser/LaserPlugin"
+    type="mujoco_ros::sensors::laser::LaserPlugin"
+    base_class_type="mujoco_ros::MujocoPlugin">
+    <description>
+      MuJoCo ROS Laser Plugin
+    </description>
+  </class>
+</library>
diff --git a/mujoco_ros_laser/package.xml b/mujoco_ros_laser/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mujoco_ros_laser</name>
+  <version>0.9.0</version>
+  <description>MuJoCo ROS Laser Plugin</description>
+
+
+  <maintainer email="[email protected]">David Leins</maintainer>
+  <author>David P. Leins</author>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>pluginlib</depend>
+  <depend>mujoco_ros</depend>
+  <depend>mujoco_ros_sensors</depend>
+  <depend>sensor_msgs</depend>
+
+  <export>
+    <mujoco_ros plugin="${prefix}/mujoco_laser_plugin.xml" />
+  </export>
+</package>