Merge from SRai22 fork

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "mrpt_local_obstacles/submodules/mola-common"]
+	path = mrpt_local_obstacles/submodules/mola-common
+	url = https://github.com/MOLAorg/mola-common.git
+[submodule "mrpt_local_obstacles/submodules/mp2p_icp"]
+	path = mrpt_local_obstacles/submodules/mp2p_icp
+	url = https://github.com/MOLAorg/mp2p_icp.git

mrpt_local_obstacles/CMakeLists.txt

@@ -1,29 +1,35 @@
-cmake_minimum_required(VERSION 3.1)
+cmake_minimum_required(VERSION 3.5)
 project(mrpt_local_obstacles)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  dynamic_reconfigure
-  roscpp
-  sensor_msgs
-  tf2
-  visualization_msgs
-  tf2_geometry_msgs
-)
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 find_package(mrpt-maps REQUIRED)
 find_package(mrpt-obs REQUIRED)
 find_package(mrpt-gui REQUIRED)
-find_package(mrpt-ros1bridge REQUIRED)
+find_package(mrpt-ros2bridge REQUIRED)
+
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
 
 if (CMAKE_COMPILER_IS_GNUCXX)
 	# High level of warnings.
 	# The -Wno-long-long is required in 64bit systems when including sytem headers.
 	# The -Wno-variadic-macros was needed for Eigen3, StdVector.h
-	add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros)
+	add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros -Wextra -Wpedantic)
 	# Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221
 	if (NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0")
 		add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context)
@@ -33,59 +39,62 @@ IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug")
   add_compile_options(-O3)
 ENDIF()
 
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-  LIBRARIES ${PROJECT_NAME}
-  CATKIN_DEPENDS dynamic_reconfigure roscpp sensor_msgs tf2 tf2_geometry_msgs visualization_msgs
-  # DEPENDS mrpt
-)
-
-###########
-## Build ##
-###########
+# TODO: Remove and move as an independent ROS package:
+add_subdirectory(submodules/mola-common)
+set(mola-common_DIR ${CMAKE_BINARY_DIR} CACHE PATH "path to mola-common-config.cmake" FORCE)
 
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-)
+add_subdirectory(submodules/mp2p_icp)
 
-## Declare a cpp executable
 add_executable(${PROJECT_NAME}_node
-  src/mrpt_local_obstacles_node.cpp
+              src/mrpt_local_obstacles_node.cpp
+              include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp)
+
+target_include_directories(${PROJECT_NAME}_node
+                           PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+                            $<INSTALL_INTERFACE:include>
 )
 
-# Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_node
-  ${catkin_LIBRARIES}
+target_link_libraries(
+  ${PROJECT_NAME}_node
   mrpt::maps
   mrpt::obs
   mrpt::gui
-  mrpt::ros1bridge
+  mrpt::ros2bridge
+  mp2p_icp_filters
 )
 
-#############
-## Install ##
-#############
+ament_target_dependencies(
+  ${PROJECT_NAME}_node
+  "rclcpp"
+  "rclcpp_components"
+  "nav_msgs"
+  "sensor_msgs"
+  "tf2"
+  "tf2_geometry_msgs"
+  "visualization_msgs"
+)
 
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
-## Mark executables and/or libraries for installation
 install(TARGETS ${PROJECT_NAME}_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
-#############
-## Testing ##
-#############
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+

mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp

@@ -0,0 +1,177 @@
+/***********************************************************************************
+ * Revised BSD License *
+ * Copyright (c) 2014-2023, Jose-Luis Blanco <jlblanco@ual.es> *
+ * 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 the Vienna University of Technology 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 Markus Bader 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. *
+ ***********************************************************************************/
+
+/* mrpt deps*/
+#include <mrpt/config/CConfigFile.h>
+#include <mrpt/system/filesystem.h>
+#include <mrpt/containers/yaml.h>
+#include <mrpt/gui/CDisplayWindow3D.h>
+#include <mrpt/maps/COccupancyGridMap2D.h>
+#include <mrpt/maps/CSimplePointsMap.h>
+#include <mrpt/obs/CObservation2DRangeScan.h>
+#include <mrpt/obs/CObservationPointCloud.h>
+#include <mrpt/obs/CSensoryFrame.h>
+#include <mrpt/opengl/CGridPlaneXY.h>
+#include <mrpt/opengl/COpenGLScene.h>
+#include <mrpt/opengl/CPointCloud.h>
+#include <mrpt/opengl/stock_objects.h>
+#include <mrpt/ros2bridge/laser_scan.h>
+#include <mrpt/ros2bridge/point_cloud2.h>
+#include <mrpt/ros2bridge/pose.h>
+#include <mrpt/system/CTimeLogger.h>
+#include <mrpt/system/string_utils.h>
+#include <mp2p_icp/icp_pipeline_from_yaml.h>
+#include <mp2p_icp_filters/FilterDecimateVoxels.h>
+
+/* ros2 deps */
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+
+#include <chrono>
+#include <map>
+#include <mutex>
+
+using namespace mrpt::system;
+using namespace mrpt::config;
+using namespace mrpt::img;
+using namespace mrpt::maps;
+using namespace mrpt::obs;
+
+class LocalObstaclesNode: public rclcpp::Node
+{
+    public:
+    /* Ctor*/
+    explicit LocalObstaclesNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+    /* Dtor*/
+    ~LocalObstaclesNode(){}
+
+    private:
+    /* Read parameters from the node handle*/
+    void read_parameters();
+
+    /* Callback: On recalc local map & publish it*/
+    void on_do_publish();
+
+    /* Callback: On new sensor data*/
+    void on_new_sensor_laser_2d(const sensor_msgs::msg::LaserScan::SharedPtr& scan);
+
+    /* Callback: On new pointcloud data*/
+    void on_new_sensor_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr& pts);
+
+    /**
+	 * @brief Subscribe to a variable number of topics.
+	 * @param lstTopics String with list of topics separated with ","
+	 * @param subs[in,out] List of subscribers will be here at return.
+	 * @return The number of topics subscribed to.
+	 */
+    template <typename MessageT, typename CallbackMethodType>
+    size_t subscribe_to_multiple_topics(const std::string& lstTopics,
+            std::vector<typename rclcpp::Subscription<MessageT>::SharedPtr>& subscriptions,
+            CallbackMethodType callback)
+    {
+        size_t num_subscriptions = 0;
+        std::vector<std::string> lstSources;
+        mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources);
+
+        // Error handling: check if lstSources is empty
+        if (lstSources.empty()) 
+        {
+            RCLCPP_ERROR(this->get_logger(), "List of topics is empty.");
+            return 0; // Return early with 0 subscriptions
+        }
+        for (const auto& source : lstSources) 
+        {
+            const auto sub = this->create_subscription<MessageT>(source, 1, callback);
+            subscriptions.push_back(sub);  // 1 is the queue size
+            num_subscriptions++;
+        }
+
+        // Return the number of subscriptions created
+        return num_subscriptions;
+    }
+
+    // member variables
+    CTimeLogger m_profiler;
+    bool m_show_gui = false;
+    std::string m_frameid_reference = "odom"; //!< type:"odom" 
+    std::string m_frameid_robot = "base_link"; //!< type: "base_link"
+    std::string m_topic_local_map_pointcloud = "local_map_pointcloud"; //!< Default: "local_map_pointcloud"
+    std::string m_topics_source_2dscan = "scan, laser1"; //!< Default: "scan, laser1"
+    std::string m_topics_source_pointclouds = "";
+
+    double m_time_window = 0.20; //!< In secs (default: 0.2). Can't be smaller than m_publish_period
+    double m_publish_period = 0.05; //!< In secs (default: 0.05). Can't be larger than m_time_window
+
+    rclcpp::TimerBase::SharedPtr m_timer_publish;
+
+    // Sensor data:
+	struct TInfoPerTimeStep
+	{
+		CObservation::Ptr observation;
+		mrpt::poses::CPose3D robot_pose;
+	};
+	typedef std::multimap<double, TInfoPerTimeStep> TListObservations;
+
+	TListObservations m_hist_obs;  //!< The history of past observations during
+	                               //! the interest time window.
+
+	std::mutex m_hist_obs_mtx;   //!< mutex
+
+    CSimplePointsMap::Ptr m_localmap_pts = CSimplePointsMap::Create();
+
+	mrpt::gui::CDisplayWindow3D::Ptr m_gui_win;
+
+    /// Used for example to run voxel grid decimation, etc.
+	/// Refer to mp2p_icp docs
+	mp2p_icp_filters::FilterPipeline m_filter_pipeline;
+	std::string m_filter_output_layer_name;	 //!< mp2p_icp output layer name
+    std::string m_filter_yaml_file;
+
+    /**
+     * @name ROS2 pubs/subs
+     * @{
+     */
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_pub_local_map_pointcloud;
+    std::vector<rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr> m_subs_2dlaser;
+    std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr> m_subs_pointclouds;
+
+    std::shared_ptr<tf2_ros::Buffer> m_tf_buffer;
+    std::shared_ptr<tf2_ros::TransformListener> m_tf_listener;
+
+};
+
+