diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..3ad9c9e7 --- /dev/null +++ b/.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 diff --git a/mrpt_local_obstacles/CATKIN_IGNORE b/mrpt_local_obstacles/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/mrpt_local_obstacles/CMakeLists.txt b/mrpt_local_obstacles/CMakeLists.txt index a020c9ef..42ffafee 100644 --- a/mrpt_local_obstacles/CMakeLists.txt +++ b/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 + $ + $ ) -# 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() + diff --git a/mrpt_local_obstacles/COLCON_IGNORE b/mrpt_local_obstacles/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp new file mode 100644 index 00000000..7f194cc4 --- /dev/null +++ b/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 * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* ros2 deps */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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 + size_t subscribe_to_multiple_topics(const std::string& lstTopics, + std::vector::SharedPtr>& subscriptions, + CallbackMethodType callback) + { + size_t num_subscriptions = 0; + std::vector 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(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 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::SharedPtr m_pub_local_map_pointcloud; + std::vector::SharedPtr> m_subs_2dlaser; + std::vector::SharedPtr> m_subs_pointclouds; + + std::shared_ptr m_tf_buffer; + std::shared_ptr m_tf_listener; + +}; + + diff --git a/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py b/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py new file mode 100644 index 00000000..0b9a9b37 --- /dev/null +++ b/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py @@ -0,0 +1,74 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + + + lidar_topic_name_arg = DeclareLaunchArgument( + 'lidar_topic_name', + default_value='/laser1, /laser2' + ) + points_topic_name_arg = DeclareLaunchArgument( + 'points_topic_name', + default_value='/particlecloud' + ) + show_gui_arg = DeclareLaunchArgument( + 'show_gui', + default_value='True' + ) + time_window_arg = DeclareLaunchArgument( + 'time_window', + default_value='0.2' + ) + filter_yaml_file_arg = DeclareLaunchArgument( + 'filter_yaml_file', + default_value= os.path.join(os.path.dirname(__file__), 'local-obstacles-decimation-filter.yaml') + ) + filter_output_layer_name_arg = DeclareLaunchArgument( + 'filter_output_layer_name', + default_value='decimated' + ) + + # Node: Local obstacles builder + mrpt_local_obstacles_node = Node( + package='mrpt_local_obstacles', + executable='mrpt_local_obstacles_node', + name='mrpt_local_obstacles_node', + output='screen', + parameters=[ + {'source_topics_2dscan': LaunchConfiguration('lidar_topic_name')}, + {'source_topics_pointclouds': LaunchConfiguration('points_topic_name')}, + {'show_gui': LaunchConfiguration('show_gui')}, + {'filter_yaml_file': LaunchConfiguration('filter_yaml_file')}, + {'filter_output_layer_name': LaunchConfiguration('filter_output_layer_name')}, + {'time_window': LaunchConfiguration('time_window')}, + {'topic_local_map_pointcloud': '/local_map_pointcloud'}, + ], + ) + + mvsim_pkg_share_dir = get_package_share_directory('mvsim') + # Finding the launch file + launch_file_name = 'demo_jackal.launch.py' + mvsim_launch_file_path = os.path.join(mvsim_pkg_share_dir, 'mvsim_tutorial', launch_file_name) + + # Check if the launch file exists + if not os.path.isfile(mvsim_launch_file_path): + raise Exception(f"Launch file '{mvsim_launch_file_path}' does not exist!") + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(mvsim_launch_file_path) + ), + lidar_topic_name_arg, + points_topic_name_arg, + show_gui_arg, + time_window_arg, + filter_yaml_file_arg, + filter_output_layer_name_arg, + mrpt_local_obstacles_node + ]) diff --git a/mrpt_local_obstacles/launch/local-obstacles-decimation-filter.yaml b/mrpt_local_obstacles/launch/local-obstacles-decimation-filter.yaml new file mode 100644 index 00000000..b382e617 --- /dev/null +++ b/mrpt_local_obstacles/launch/local-obstacles-decimation-filter.yaml @@ -0,0 +1,15 @@ +- class_name: mp2p_icp_filters::FilterDecimateVoxels + params: + output_pointcloud_layer: 'decimated_pre' + voxel_filter_resolution: 0.05 # [m] + use_voxel_average: true + bounding_box_min: [ -5, -5, 0 ] + bounding_box_max: [ 10, 5, 1 ] +- class_name: mp2p_icp_filters::FilterBoundingBox + params: + input_pointcloud_layer: 'decimated_pre' + output_pointcloud_layer: 'decimated' + keep_bbox_contents: false + bounding_box_min: [ -0.22, -0.27, -1 ] + bounding_box_max: [ 0.22, 0.27, 1 ] + diff --git a/mrpt_local_obstacles/package.xml b/mrpt_local_obstacles/package.xml index f9fe0972..604d367a 100644 --- a/mrpt_local_obstacles/package.xml +++ b/mrpt_local_obstacles/package.xml @@ -1,5 +1,5 @@ - + mrpt_local_obstacles 1.0.3 Maintains a local obstacle map (point cloud, @@ -7,23 +7,29 @@ configurable time window. Jose-Luis Blanco-Claraco + Shravan S Rai + + Jose-Luis Blanco-Claraco + Shravan S Rai BSD https://wiki.ros.org/mrpt_local_obstacles - Jose-Luis Blanco-Claraco - catkin + ament_cmake mrpt2 dynamic_reconfigure - roscpp + rclcpp + rclcpp_components + nav_msgs sensor_msgs tf2 tf2_geometry_msgs visualization_msgs + ament_cmake diff --git a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp index 03899f91..f722d385 100644 --- a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp +++ b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp @@ -1,57 +1,6 @@ -/*********************************************************************************** - * Revised BSD License * - * Copyright (c) 2014-2015, Jose-Luis Blanco * - * 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. * - ***********************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include +#include +#include "rclcpp_components/register_node_macro.hpp" +#include using namespace mrpt::system; using namespace mrpt::config; @@ -59,396 +8,487 @@ using namespace mrpt::img; using namespace mrpt::maps; using namespace mrpt::obs; -// The ROS node -class LocalObstaclesNode + +LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) +: Node("mrpt_local_obstacles", options) { - private: - struct TAuxInitializer + read_parameters(); + + // Create publisher for local map point cloud + m_pub_local_map_pointcloud = + this->create_publisher(m_topic_local_map_pointcloud, 10); + + + // Init ROS subs: + // Subscribe to one or more laser sources: + size_t nSubsTotal = 0; + nSubsTotal += subscribe_to_multiple_topics( + m_topics_source_2dscan, m_subs_2dlaser, + [this](const sensor_msgs::msg::LaserScan::SharedPtr scan) + {this-> on_new_sensor_laser_2d(scan);}); + + nSubsTotal += subscribe_to_multiple_topics( + m_topics_source_pointclouds, m_subs_pointclouds, + [this](const sensor_msgs::msg::PointCloud2::SharedPtr pts) + {this->on_new_sensor_pointcloud(pts);}); + + RCLCPP_INFO(this->get_logger(), + "Total number of sensor subscriptions: %u", + static_cast(nSubsTotal)); + + if(!(nSubsTotal>0)) + RCLCPP_ERROR(this->get_logger(), + "*Error* It is mandatory to set at least one source topic for sensory information!"); + + // Local map params: + m_localmap_pts->insertionOptions.minDistBetweenLaserPoints = 0; + m_localmap_pts->insertionOptions.also_interpolate = false; + + // Create the tf2 buffer and listener + m_tf_buffer = std::make_shared(this->get_clock()); + m_tf_listener = std::make_shared(*m_tf_buffer); + + m_timer_publish = create_wall_timer( + std::chrono::duration(m_publish_period), + [this](){this->on_do_publish();}); +} //end ctor + + +/** Callback: On recalc local map & publish it */ +void LocalObstaclesNode::on_do_publish() +{ + CTimeLoggerEntry tle(m_profiler, "on_do_publish"); + + // Purge old observations & latch a local copy: + TListObservations obs; { - TAuxInitializer(int argc, char** argv) + CTimeLoggerEntry tle(m_profiler, "onDoPublish.removingOld"); + m_hist_obs_mtx.lock(); + + // Purge old obs: + if (!m_hist_obs.empty()) { - ros::init(argc, argv, "mrpt_local_obstacles_node"); + const double last_time = m_hist_obs.rbegin()->first; + TListObservations::iterator it_first_valid = + m_hist_obs.lower_bound(last_time - m_time_window); + const size_t nToRemove = + std::distance(m_hist_obs.begin(), it_first_valid); + RCLCPP_DEBUG(this->get_logger(), + "[onDoPublish] Removing %u old entries, last_time=%lf", + static_cast(nToRemove), last_time); + m_hist_obs.erase(m_hist_obs.begin(), it_first_valid); } - }; - - CTimeLogger m_profiler; - - TAuxInitializer m_auxinit; //!< Just to make sure ROS is init first - ros::NodeHandle m_nh; //!< The node handle - ros::NodeHandle m_localn; //!< "~" - bool m_show_gui; - std::string m_frameid_reference; //!< typ: "odom" - std::string m_frameid_robot; //!< typ: "base_link" - std::string - m_topic_local_map_pointcloud; //!< Default: "local_map_pointcloud" - std::string m_source_topics_2dscan; //!< Default: "scan,laser1" - double m_time_window; //!< In secs (default: 0.2). This can't be smaller - //! than m_publish_period - double m_publish_period; //!< In secs (default: 0.05). This can't be larger - //! than m_time_window - - ros::Timer m_timer_publish; - - // Sensor data: - struct TInfoPerTimeStep - { - CObservation::Ptr observation; - mrpt::poses::CPose3D robot_pose; - }; - typedef std::multimap TListObservations; - TListObservations m_hist_obs; //!< The history of past observations during - //! the interest time window. - boost::mutex m_hist_obs_mtx; - - /** The local maps */ - CSimplePointsMap m_localmap_pts; - // COccupancyGridMap2D m_localmap_grid; - - mrpt::gui::CDisplayWindow3D::Ptr m_gui_win; - - /** @name ROS pubs/subs - * @{ */ - ros::Publisher m_pub_local_map_pointcloud; - - //!< Subscriber to 2D laser scans - std::vector m_subs_2dlaser; - - tf2_ros::Buffer m_tf_buffer; - tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; - - /** @} */ - - /** - * @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 - size_t subscribeToMultipleTopics( - const std::string& lstTopics, std::vector& subs, - CALLBACK_METHOD_TYPE cb) - { - std::vector lstSources; - mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources); - subs.resize(lstSources.size()); - for (size_t i = 0; i < lstSources.size(); i++) - subs[i] = m_nh.subscribe(lstSources[i], 1, cb, this); - return lstSources.size(); + // Local copy in this thread: + obs = m_hist_obs; + m_hist_obs_mtx.unlock(); } - /** Callback: On new sensor data - */ - void onNewSensor_Laser2D(const sensor_msgs::LaserScanConstPtr& scan) + RCLCPP_DEBUG(this->get_logger(), + "Building local map with %u observations.", + static_cast(obs.size())); + + if (obs.empty()) return; + + // Build local map(s): + // ----------------------------------------------- + m_localmap_pts->clear(); + mrpt::poses::CPose3D curRobotPose; { - CTimeLoggerEntry tle(m_profiler, "onNewSensor_Laser2D"); + CTimeLoggerEntry tle2(m_profiler, "on_do_publish.buildLocalMap"); - ros::Duration timeout(1.0); + // Get the latest robot pose in the reference frame (typ: /odom -> + // /base_link) + // so we can build the local map RELATIVE to it: + rclcpp::Duration timeout(std::chrono::seconds(1)); - // Get the relative position of the sensor wrt the robot: - geometry_msgs::TransformStamped sensorOnRobot; try { - CTimeLoggerEntry tle2( - m_profiler, "onNewSensor_Laser2D.lookupTransform_sensor"); - - sensorOnRobot = m_tf_buffer.lookupTransform( - m_frameid_robot, scan->header.frame_id, scan->header.stamp, - timeout); + geometry_msgs::msg::TransformStamped tx; + tx = m_tf_buffer->lookupTransform( + m_frameid_reference, m_frameid_robot, tf2::TimePointZero, + tf2::durationFromSec(timeout.seconds())); + + tf2::Transform tfx; + tf2::fromMsg(tx.transform, tfx); + curRobotPose = mrpt::ros2bridge::fromROS(tfx); } - catch (const tf2::TransformException& ex) + catch (const tf2::ExtrapolationException& ex) { - ROS_ERROR("%s", ex.what()); + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); return; } - // Convert data to MRPT format: - const mrpt::poses::CPose3D sensorOnRobot_mrpt = [&]() { - tf2::Transform tx; - tf2::fromMsg(sensorOnRobot.transform, tx); - return mrpt::ros1bridge::fromROS(tx); - }(); + RCLCPP_DEBUG(this->get_logger(), + "[onDoPublish] Building local map relative to latest robot " + "pose: %s", + curRobotPose.asString().c_str()); + + // For each observation: compute relative robot pose & insert obs + // into map: + for (TListObservations::const_iterator it = obs.begin(); + it != obs.end(); ++it) + { + const TInfoPerTimeStep& ipt = it->second; + + // Relative pose in the past: + mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); + relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); + + // Insert obs: + m_localmap_pts->insertObservationPtr(ipt.observation, relPose); + + } // end for + } + + // Filtering: + mrpt::maps::CPointsMap::Ptr filteredPts; + + if (!m_filter_pipeline.empty()) + { + mp2p_icp::metric_map_t mm; + mm.layers[mp2p_icp::metric_map_t::PT_LAYER_RAW] = m_localmap_pts; + mp2p_icp_filters::apply_filter_pipeline(m_filter_pipeline, mm); + + filteredPts = mm.point_layer(m_filter_output_layer_name); + } + else + { + filteredPts = m_localmap_pts; + } + + // Publish them: + if (m_pub_local_map_pointcloud->get_subscription_count() > 0) + { + sensor_msgs::msg::PointCloud2 msg_pts; + msg_pts.header.frame_id = m_frameid_robot; + msg_pts.header.stamp = rclcpp::Time(obs.rbegin()->first); + + auto simplPts = + std::dynamic_pointer_cast( + filteredPts); + ASSERT_(simplPts); + + mrpt::ros2bridge::toROS(*simplPts, msg_pts.header, msg_pts); + m_pub_local_map_pointcloud->publish(msg_pts); + } + + // Show gui: + if (m_show_gui) + { + if (!m_gui_win) + { + m_gui_win = mrpt::gui::CDisplayWindow3D::Create( + "LocalObstaclesNode", 800, 600); + mrpt::opengl::COpenGLScene::Ptr& scene = + m_gui_win->get3DSceneAndLock(); + scene->insert(mrpt::opengl::CGridPlaneXY::Create()); + scene->insert( + mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 4.0)); + + auto gl_obs = mrpt::opengl::CSetOfObjects::Create(); + gl_obs->setName("obstacles"); + scene->insert(gl_obs); + + auto gl_rawpts = mrpt::opengl::CPointCloud::Create(); + gl_rawpts->setName("raw_points"); + gl_rawpts->setPointSize(1.0); + gl_rawpts->setColor_u8(TColor(0x00ff00)); + scene->insert(gl_rawpts); + + auto gl_pts = mrpt::opengl::CPointCloud::Create(); + gl_pts->setName("final_points"); + gl_pts->setPointSize(3.0); + gl_pts->setColor_u8(TColor(0x0000ff)); + scene->insert(gl_pts); + + m_gui_win->unlockAccess3DScene(); + } + + auto& scene = m_gui_win->get3DSceneAndLock(); + auto gl_obs = mrpt::ptr_cast::from( + scene->getByName("obstacles")); + //ROS_ASSERT(!!gl_obs); + gl_obs->clear(); + + auto glRawPts = mrpt::ptr_cast::from( + scene->getByName("raw_points")); + + auto glFinalPts = mrpt::ptr_cast::from( + scene->getByName("final_points")); + + for (const auto& o : obs) + { + const TInfoPerTimeStep& ipt = o.second; + // Relative pose in the past: + mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); + relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); + + mrpt::opengl::CSetOfObjects::Ptr gl_axis = + mrpt::opengl::stock_objects::CornerXYZSimple(0.9, 2.0); + gl_axis->setPose(relPose); + gl_obs->insert(gl_axis); + } // end for + + glRawPts->loadFromPointsMap(m_localmap_pts.get()); + glFinalPts->loadFromPointsMap(filteredPts.get()); + + m_gui_win->unlockAccess3DScene(); + m_gui_win->repaint(); + } + +} // onDoPublish + +void LocalObstaclesNode::on_new_sensor_laser_2d(const sensor_msgs::msg::LaserScan::SharedPtr& scan) +{ + CTimeLoggerEntry tle(m_profiler, "on_new_sensor_laser_2d"); + rclcpp::Duration timeout(std::chrono::seconds(1)); + + geometry_msgs::msg::TransformStamped sensorOnRobot; + try + { + CTimeLoggerEntry tle2( + m_profiler, "onNewSensor_Laser2D.lookupTransform_sensor"); + sensorOnRobot = m_tf_buffer->lookupTransform(m_frameid_robot, + scan->header.frame_id, scan->header.stamp, timeout); + } + catch(const tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; + } + + const mrpt::poses::CPose3D sensorOnRobot_mrpt = [&]() { + tf2::Transform tx; + tf2::fromMsg(sensorOnRobot.transform, tx); + return mrpt::ros2bridge::fromROS(tx); + }(); - // In MRPT, CObservation2DRangeScan holds both: sensor data + - // relative pose: - auto obsScan = CObservation2DRangeScan::Create(); - mrpt::ros1bridge::fromROS(*scan, sensorOnRobot_mrpt, *obsScan); + // In MRPT, CObservation2DRangeScan holds both: sensor data + + // relative pose: + auto obsScan = CObservation2DRangeScan::Create(); + mrpt::ros2bridge::fromROS(*scan, sensorOnRobot_mrpt, *obsScan); - ROS_DEBUG( - "[onNewSensor_Laser2D] %u rays, sensor pose on robot %s", + RCLCPP_DEBUG(this->get_logger(), + "[onNewSensor_Laser2D] %u rays, sensor pose on robot %s", static_cast(obsScan->getScanSize()), sensorOnRobot_mrpt.asString().c_str()); - // Get sensor timestamp: - const double timestamp = scan->header.stamp.toSec(); + // Get sensor timestamp: + auto stamp = scan->header.stamp; + const double timestamp = stamp.sec + static_cast(stamp.nanosec) / 1e9; - // Get robot pose at that time in the reference frame, typ: /odom -> - // /base_link - mrpt::poses::CPose3D robotPose; - try - { - CTimeLoggerEntry tle3( + // Get robot pose at that time in the reference frame, typ: /odom -> + // /base_link + mrpt::poses::CPose3D robotPose; + try + { + CTimeLoggerEntry tle3( m_profiler, "onNewSensor_Laser2D.lookupTransform_robot"); - geometry_msgs::TransformStamped robotTfStamp; - try - { - robotTfStamp = m_tf_buffer.lookupTransform( - m_frameid_reference, m_frameid_robot, scan->header.stamp, - timeout); - } - catch (const tf2::ExtrapolationException& ex) - { - ROS_ERROR("%s", ex.what()); - return; - } - - robotPose = [&]() { - tf2::Transform tx; - tf2::fromMsg(robotTfStamp.transform, tx); - return mrpt::ros1bridge::fromROS(tx); - }(); - - ROS_DEBUG( - "[onNewSensor_Laser2D] robot pose %s", - robotPose.asString().c_str()); + geometry_msgs::msg::TransformStamped robotTfStamp; + try + { + robotTfStamp = m_tf_buffer->lookupTransform( + m_frameid_reference, m_frameid_robot, scan->header.stamp, + timeout); } - catch (const tf2::TransformException& ex) + catch (const tf2::ExtrapolationException& ex) { - ROS_ERROR("%s", ex.what()); + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); return; } - // Insert into the observation history: - TInfoPerTimeStep ipt; - ipt.observation = obsScan; - ipt.robot_pose = robotPose; + robotPose = [&]() { + tf2::Transform tx; + tf2::fromMsg(robotTfStamp.transform, tx); + return mrpt::ros2bridge::fromROS(tx); + }(); - m_hist_obs_mtx.lock(); - m_hist_obs.insert( - m_hist_obs.end(), TListObservations::value_type(timestamp, ipt)); - m_hist_obs_mtx.unlock(); + RCLCPP_DEBUG(this->get_logger(), + "[onNewSensor_Laser2D] robot pose %s", + robotPose.asString().c_str()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; + } + + // Insert into the observation history: + TInfoPerTimeStep ipt; + ipt.observation = obsScan; + ipt.robot_pose = robotPose; + + m_hist_obs_mtx.lock(); + m_hist_obs.insert( + m_hist_obs.end(), TListObservations::value_type(timestamp, ipt)); + m_hist_obs_mtx.unlock(); - } // end onNewSensor_Laser2D +} // end on_new_sensor_laser_2d - /** Callback: On recalc local map & publish it */ - void onDoPublish(const ros::TimerEvent&) +void LocalObstaclesNode::on_new_sensor_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr& pts) +{ + CTimeLoggerEntry tle(m_profiler, "on_new_sensor_pointcloud"); + + rclcpp::Duration timeout(std::chrono::seconds(1)); + // Get the relative position of the sensor wrt the robot: + geometry_msgs::msg::TransformStamped sensorOnRobot; + try { - CTimeLoggerEntry tle(m_profiler, "onDoPublish"); + CTimeLoggerEntry tle2( + m_profiler, "on_new_sensor_pointcloud.lookupTransform_sensor"); - // Purge old observations & latch a local copy: - TListObservations obs; - { - CTimeLoggerEntry tle(m_profiler, "onDoPublish.removingOld"); - m_hist_obs_mtx.lock(); - - // Purge old obs: - if (!m_hist_obs.empty()) - { - const double last_time = m_hist_obs.rbegin()->first; - TListObservations::iterator it_first_valid = - m_hist_obs.lower_bound(last_time - m_time_window); - const size_t nToRemove = - std::distance(m_hist_obs.begin(), it_first_valid); - ROS_DEBUG( - "[onDoPublish] Removing %u old entries, last_time=%lf", - static_cast(nToRemove), last_time); - m_hist_obs.erase(m_hist_obs.begin(), it_first_valid); - } - // Local copy in this thread: - obs = m_hist_obs; - m_hist_obs_mtx.unlock(); - } + sensorOnRobot = m_tf_buffer->lookupTransform( + m_frameid_robot, pts->header.frame_id, pts->header.stamp, + timeout); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(),"%s", ex.what()); + return; + } - ROS_DEBUG( - "Building local map with %u observations.", - static_cast(obs.size())); - if (obs.empty()) return; + // Convert data to MRPT format: + const mrpt::poses::CPose3D sensorOnRobot_mrpt = [&]() { + tf2::Transform tx; + tf2::fromMsg(sensorOnRobot.transform, tx); + return mrpt::ros2bridge::fromROS(tx); + }(); + + // In MRPT, CObservationPointCloud holds both: sensor data + + // relative pose: + auto obsPts = CObservationPointCloud::Create(); + const auto ptsMap = mrpt::maps::CSimplePointsMap::Create(); + obsPts->pointcloud = ptsMap; + obsPts->sensorPose = sensorOnRobot_mrpt; + mrpt::ros2bridge::fromROS(*pts, *ptsMap); + + RCLCPP_DEBUG(this->get_logger(), + "[on_new_sensor_pointcloud] %u points, sensor pose on robot %s", + static_cast(ptsMap->size()), + sensorOnRobot_mrpt.asString().c_str()); + + // Get sensor timestamp: + auto stamp = pts->header.stamp; + const double timestamp = stamp.sec + static_cast(stamp.nanosec) / 1e9; + + // Get robot pose at that time in the reference frame, typ: /odom -> + // /base_link + mrpt::poses::CPose3D robotPose; + try + { + CTimeLoggerEntry tle3( + m_profiler, "onNewSensor_pointcloud.lookupTransform_robot"); - // Build local map(s): - // ----------------------------------------------- - m_localmap_pts.clear(); - mrpt::poses::CPose3D curRobotPose; + geometry_msgs::msg::TransformStamped robotTfStamp; + try { - CTimeLoggerEntry tle2(m_profiler, "onDoPublish.buildLocalMap"); - - // Get the latest robot pose in the reference frame (typ: /odom -> - // /base_link) - // so we can build the local map RELATIVE to it: - ros::Duration timeout(1.0); - - try - { - geometry_msgs::TransformStamped tx; - tx = m_tf_buffer.lookupTransform( - m_frameid_reference, m_frameid_robot, ros::Time(0), - timeout); - - tf2::Transform tfx; - tf2::fromMsg(tx.transform, tfx); - curRobotPose = mrpt::ros1bridge::fromROS(tfx); - } - catch (const tf2::ExtrapolationException& ex) - { - ROS_ERROR("%s", ex.what()); - return; - } - - ROS_DEBUG( - "[onDoPublish] Building local map relative to latest robot " - "pose: %s", - curRobotPose.asString().c_str()); - - // For each observation: compute relative robot pose & insert obs - // into map: - for (TListObservations::const_iterator it = obs.begin(); - it != obs.end(); ++it) - { - const TInfoPerTimeStep& ipt = it->second; - - // Relative pose in the past: - mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); - relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); - - // Insert obs: - m_localmap_pts.insertObservationPtr(ipt.observation, relPose); - - } // end for + robotTfStamp = m_tf_buffer->lookupTransform( + m_frameid_reference, m_frameid_robot, pts->header.stamp, + timeout); } - - // Publish them: - if (m_pub_local_map_pointcloud.getNumSubscribers() > 0) + catch (const tf2::ExtrapolationException& ex) { - sensor_msgs::PointCloudPtr msg_pts = - sensor_msgs::PointCloudPtr(new sensor_msgs::PointCloud); - msg_pts->header.frame_id = m_frameid_robot; - msg_pts->header.stamp = ros::Time(obs.rbegin()->first); - - mrpt::ros1bridge::toROS(m_localmap_pts, msg_pts->header, *msg_pts); - m_pub_local_map_pointcloud.publish(msg_pts); + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; } - // Show gui: - if (m_show_gui) - { - if (!m_gui_win) - { - m_gui_win = mrpt::gui::CDisplayWindow3D::Create( - "LocalObstaclesNode", 800, 600); - mrpt::opengl::COpenGLScene::Ptr& scene = - m_gui_win->get3DSceneAndLock(); - scene->insert(mrpt::opengl::CGridPlaneXY::Create()); - scene->insert( - mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 4.0)); - - auto gl_obs = mrpt::opengl::CSetOfObjects::Create(); - gl_obs->setName("obstacles"); - scene->insert(gl_obs); - - auto gl_pts = mrpt::opengl::CPointCloud::Create(); - gl_pts->setName("points"); - gl_pts->setPointSize(2.0); - gl_pts->setColor_u8(TColor(0x0000ff)); - scene->insert(gl_pts); - - m_gui_win->unlockAccess3DScene(); - } - - auto& scene = m_gui_win->get3DSceneAndLock(); - auto gl_obs = mrpt::ptr_cast::from( - scene->getByName("obstacles")); - ROS_ASSERT(!!gl_obs); - gl_obs->clear(); - - auto gl_pts = mrpt::ptr_cast::from( - scene->getByName("points")); - - for (const auto& o : obs) - { - const TInfoPerTimeStep& ipt = o.second; - // Relative pose in the past: - mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); - relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); - - mrpt::opengl::CSetOfObjects::Ptr gl_axis = - mrpt::opengl::stock_objects::CornerXYZSimple(0.9, 2.0); - gl_axis->setPose(relPose); - gl_obs->insert(gl_axis); - } // end for - - gl_pts->loadFromPointsMap(&m_localmap_pts); - - m_gui_win->unlockAccess3DScene(); - m_gui_win->repaint(); - } + robotPose = [&]() { + tf2::Transform tx; + tf2::fromMsg(robotTfStamp.transform, tx); + return mrpt::ros2bridge::fromROS(tx); + }(); - } // onDoPublish - - public: - /** Constructor: Inits ROS system */ - LocalObstaclesNode(int argc, char** argv) - : m_auxinit(argc, argv), - m_nh(), - m_localn("~"), - m_show_gui(true), - m_frameid_reference("odom"), - m_frameid_robot("base_link"), - m_topic_local_map_pointcloud("local_map_pointcloud"), - m_source_topics_2dscan("scan,laser1"), - m_time_window(0.2), - m_publish_period(0.05) + RCLCPP_DEBUG(this->get_logger(), + "[onNewSensor_pointcloud] robot pose %s", + robotPose.asString().c_str()); + } + catch (const tf2::TransformException& ex) { - // Load params: - m_localn.param("show_gui", m_show_gui, m_show_gui); - m_localn.param( - "frameid_reference", m_frameid_reference, m_frameid_reference); - m_localn.param("frameid_robot", m_frameid_robot, m_frameid_robot); - m_localn.param( - "topic_local_map_pointcloud", m_topic_local_map_pointcloud, - m_topic_local_map_pointcloud); - m_localn.param( - "source_topics_2dscan", m_source_topics_2dscan, - m_source_topics_2dscan); - m_localn.param("time_window", m_time_window, m_time_window); - m_localn.param("publish_period", m_publish_period, m_publish_period); - - ROS_ASSERT(m_time_window > m_publish_period); - ROS_ASSERT(m_publish_period > 0); - - // Init ROS publishers: - m_pub_local_map_pointcloud = m_nh.advertise( - m_topic_local_map_pointcloud, 10); - - // Init ROS subs: - // Subscribe to one or more laser sources: - size_t nSubsTotal = 0; - nSubsTotal += this->subscribeToMultipleTopics( - m_source_topics_2dscan, m_subs_2dlaser, - &LocalObstaclesNode::onNewSensor_Laser2D); - - ROS_INFO( - "Total number of sensor subscriptions: %u\n", - static_cast(nSubsTotal)); - ROS_ASSERT_MSG( - nSubsTotal > 0, - "*Error* It is mandatory to set at least one source topic for " - "sensory information!"); - - // Local map params: - m_localmap_pts.insertionOptions.minDistBetweenLaserPoints = 0; - m_localmap_pts.insertionOptions.also_interpolate = false; - - // Init timers: - m_timer_publish = m_nh.createTimer( - ros::Duration(m_publish_period), &LocalObstaclesNode::onDoPublish, - this); - - } // end ctor -}; // end class - -int main(int argc, char** argv) + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; + } + + // Insert into the observation history: + TInfoPerTimeStep ipt; + ipt.observation = obsPts; + ipt.robot_pose = robotPose; + + m_hist_obs_mtx.lock(); + m_hist_obs.insert( + m_hist_obs.end(), TListObservations::value_type(timestamp, ipt)); + m_hist_obs_mtx.unlock(); +} //end on_new_sensor_pointcloud + +// read params from parameter server +void LocalObstaclesNode::read_parameters() +{ + this->declare_parameter("show_gui", false); + this->get_parameter("show_gui", m_show_gui); + RCLCPP_INFO(this->get_logger(), "show_gui: %b", m_show_gui); + + this->declare_parameter("frameid_reference", "odom"); + this->get_parameter("frameid_reference", m_frameid_reference); + RCLCPP_INFO(this->get_logger(), "frameid_reference: %s", m_frameid_reference.c_str()); + + this->declare_parameter("frameid_robot", "base_link"); + this->get_parameter("frameid_robot", m_frameid_robot); + RCLCPP_INFO(this->get_logger(), "frameid_robot: %s", m_frameid_robot.c_str()); + + this->declare_parameter("time_window", 0.20); + this->get_parameter("time_window", m_time_window); + RCLCPP_INFO(this->get_logger(), "time_window: %f", m_time_window); + + this->declare_parameter("publish_period", 0.05); + this->get_parameter("publish_period", m_publish_period); + RCLCPP_INFO(this->get_logger(), "publish_period: %f", m_publish_period); + + this->declare_parameter("topic_local_map_pointcloud", "local_map_pointcloud"); + this->get_parameter("topic_local_map_pointcloud", m_topic_local_map_pointcloud); + RCLCPP_INFO(this->get_logger(), "topic_local_map_pointcloud: %s",m_topic_local_map_pointcloud.c_str()); + + this->declare_parameter("source_topics_2dscan", "scan, laser1"); + this->get_parameter("source_topics_2dscan", m_topics_source_2dscan); + RCLCPP_INFO(this->get_logger(), "source_topics_2dscan: %s", m_topics_source_2dscan.c_str()); + + this->declare_parameter("source_topics_pointclouds", ""); + this->get_parameter("source_topics_pointclouds", m_topics_source_pointclouds); + RCLCPP_INFO(this->get_logger(), "source_topics_pointclouds: %s", m_topics_source_pointclouds.c_str()); + + this->declare_parameter("filter_yaml_file", ""); + this->get_parameter("filter_yaml_file", m_filter_yaml_file); + RCLCPP_INFO(this->get_logger(), "filter_yaml_file: %s", m_filter_yaml_file.c_str()); + if(!m_filter_yaml_file.empty()) + { + ASSERT_FILE_EXISTS_(m_filter_yaml_file); + mrpt::containers::yaml cfg; + cfg.loadFromFile(m_filter_yaml_file); + std::stringstream ss; + cfg.printAsYAML(ss); + RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str()); + m_filter_pipeline = mp2p_icp_filters::filter_pipeline_from_yaml(cfg); + } + + this->declare_parameter("filter_output_layer_name", "decimated"); + this->get_parameter("filter_output_layer_name", m_filter_output_layer_name); + RCLCPP_INFO(this->get_logger(), "filter_output_layer_name: %s", m_filter_output_layer_name.c_str()); + +} + +int main(int argc, char ** argv) { - LocalObstaclesNode the_node(argc, argv); - ros::spin(); - return 0; + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; } diff --git a/mrpt_local_obstacles/submodules/mola-common b/mrpt_local_obstacles/submodules/mola-common new file mode 160000 index 00000000..5b2001fb --- /dev/null +++ b/mrpt_local_obstacles/submodules/mola-common @@ -0,0 +1 @@ +Subproject commit 5b2001fb9b0503a3e38508cf0c5839c17047dfdb diff --git a/mrpt_local_obstacles/submodules/mp2p_icp b/mrpt_local_obstacles/submodules/mp2p_icp new file mode 160000 index 00000000..db2ac512 --- /dev/null +++ b/mrpt_local_obstacles/submodules/mp2p_icp @@ -0,0 +1 @@ +Subproject commit db2ac5127b8baee2a0e4b53bda1b2e8a5cdeb036 diff --git a/mrpt_msgs_bridge/CMakeLists.txt b/mrpt_msgs_bridge/CMakeLists.txt index 8d71a322..a885c4a3 100644 --- a/mrpt_msgs_bridge/CMakeLists.txt +++ b/mrpt_msgs_bridge/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(mrpt_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(marker_msgs REQUIRED) find_package(mrpt-ros2bridge REQUIRED) find_package(mrpt-obs REQUIRED) @@ -58,15 +59,13 @@ target_link_libraries(${PROJECT_NAME} # Mark executables and/or libraries for installation install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) # Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include - FILES_MATCHING PATTERN "*.h" + FILES_MATCHING PATTERN "*.hpp" ) ############# @@ -75,6 +74,7 @@ ament_target_dependencies(${PROJECT_NAME} visualization_msgs tf2 mrpt_msgs + marker_msgs ) #ament_copyright() diff --git a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.h b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.hpp similarity index 73% rename from mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.h rename to mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.hpp index 485f187e..670d0a63 100644 --- a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.h +++ b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.hpp @@ -2,24 +2,23 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #pragma once - -#include #include #include -#include +#include +#include #include #include namespace mrpt_msgs_bridge { -/** @name ObservationRangeBeacon: ROS <-> MRPT +/** @name ObservationRangeBeacon: ROS2 <-> MRPT * @{ */ /** ROS->MRPT: Takes a mrpt_msgs::ObservationRangeBeacon and the relative pose @@ -28,26 +27,26 @@ namespace mrpt_msgs_bridge * \sa mrpt2ros */ bool fromROS( - const mrpt_msgs::ObservationRangeBeacon& _msg, + const mrpt_msgs::msg::ObservationRangeBeacon _msg, const mrpt::poses::CPose3D& _pose, mrpt::obs::CObservationBeaconRanges& _obj); -/** MRPT->ROS: Takes a CObservationBeaconRanges and outputs range data in +/** MRPT->ROS2: Takes a CObservationBeaconRanges and outputs range data in * mrpt_msgs::ObservationRangeBeacon \return true on sucessful conversion, false * on any error. \sa ros2mrpt */ bool toROS( const mrpt::obs::CObservationBeaconRanges& _obj, - mrpt_msgs::ObservationRangeBeacon& _msg); + mrpt_msgs::msg::ObservationRangeBeacon& _msg); -/** MRPT->ROS: Takes a CObservationBeaconRanges and outputs range data in +/** MRPT->ROS2: Takes a CObservationBeaconRanges and outputs range data in * mrpt_msgs::ObservationRangeBeacon + the relative pose of the range sensor wrt * base_link \return true on sucessful conversion, false on any error. \sa * ros2mrpt */ bool toROS( const mrpt::obs::CObservationBeaconRanges& _obj, - mrpt_msgs::ObservationRangeBeacon& _msg, geometry_msgs::Pose& _pose); + mrpt_msgs::msg::ObservationRangeBeacon& _msg, geometry_msgs::msg::Pose& _pose); /** @} */ diff --git a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.h b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.hpp similarity index 69% rename from mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.h rename to mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.hpp index 1cfcadc5..aad702a9 100644 --- a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.h +++ b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.hpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -17,35 +17,35 @@ #include #include -#include -#include +#include +#include #include namespace mrpt_msgs_bridge { -/** @name LaserScan: ROS <-> MRPT +/** @name LaserScan: ROS2 <-> MRPT * @{ */ -/** ROS->MRPT: Takes a mrpt_msgs::CObservationBearingRange and the relative pose +/** ROS2->MRPT: Takes a mrpt_msgs::CObservationBearingRange and the relative pose * of the sensor wrt base_link and builds a ObservationRangeBearing * \return true on sucessful conversion, false on any error. * \sa mrpt2ros */ bool fromROS( - const mrpt_msgs::ObservationRangeBearing& _msg, + const mrpt_msgs::msg::ObservationRangeBearing& _msg, const mrpt::poses::CPose3D& _pose, mrpt::obs::CObservationBearingRange& _obj ); -/** MRPT->ROS: Takes a CObservationBearingRange and outputs range data in +/** MRPT->ROS2: Takes a CObservationBearingRange and outputs range data in * mrpt_msgs::ObservationRangeBearing * \return true on sucessful conversion, false on any error. * \sa ros2mrpt */ bool toROS( const mrpt::obs::CObservationBearingRange& _obj, - mrpt_msgs::ObservationRangeBearing& _msg); + mrpt_msgs::msg::ObservationRangeBearing& _msg); -/** MRPT->ROS: Takes a CObservationBearingRange and outputs range data in +/** MRPT->ROS2: Takes a CObservationBearingRange and outputs range data in * mrpt_msgs::ObservationRangeBearing + the relative pose of the range sensor * wrt base_link * \return true on sucessful conversion, false on any error. @@ -53,8 +53,8 @@ bool toROS( */ bool toROS( const mrpt::obs::CObservationBearingRange& _obj, - mrpt_msgs::ObservationRangeBearing& _msg, geometry_msgs::Pose& sensorPose); + mrpt_msgs::msg::ObservationRangeBearing& _msg, geometry_msgs::msg::Pose& sensorPose); /** @} */ -} // namespace mrpt_bridge +} // namespace mrpt_bridge \ No newline at end of file diff --git a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.h b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.hpp similarity index 79% rename from mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.h rename to mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.hpp index 65e1df3b..89ded9b5 100644 --- a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.h +++ b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.hpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | - | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -16,8 +16,9 @@ #pragma once -#include -#include +#include +#include + #include #include #include @@ -26,18 +27,18 @@ namespace mrpt_msgs_bridge { -// NOTE: These converters are here instead of mrpt::ros1bridge since +// NOTE: These converters are here instead of mrpt::ros2bridge since // the builds dep. MarkerDetection is not available in Debian/Ubuntu // official repos, so we need to build it here within ROS. bool fromROS( - const marker_msgs::MarkerDetection& _msg, + const marker_msgs::msg::MarkerDetection& _msg, const mrpt::poses::CPose3D& sensorPoseOnRobot, mrpt::obs::CObservationBearingRange& _obj); bool fromROS( - const marker_msgs::MarkerDetection& _msg, + const marker_msgs::msg::MarkerDetection& _msg, const mrpt::poses::CPose3D& sensorPoseOnRobot, mrpt::obs::CObservationBeaconRanges& _obj); -} // namespace mrpt_msgs_bridge +} // namespace mrpt_msgs_bridge \ No newline at end of file diff --git a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.h b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.hpp similarity index 65% rename from mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.h rename to mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.hpp index 1ff1c1ba..3de9b82a 100644 --- a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.h +++ b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.hpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -14,12 +14,12 @@ #pragma once #include -#include +#include namespace mrpt_msgs_bridge { -/**\name MRPT \rightarrow ROS conversions */ -/**\brief Convert [MRPT] CNetworkOfPoses*D \rightarrow [ROS] NetworkOfPoses. +/**\name MRPT \rightarrow ROS2 conversions */ +/**\brief Convert [MRPT] CNetworkOfPoses*D \rightarrow [ROS2] NetworkOfPoses. * \param[in] mrpt_graph MRPT graph representation * \param[out] ros_graph ROS graph representation */ @@ -28,62 +28,62 @@ namespace mrpt_msgs_bridge // TODO - convert these methods into a common polymorphic method void toROS( const mrpt::graphs::CNetworkOfPoses2D& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses3D& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); /**\} */ ///////////////////////////////////////////////////////////////////////// -/**\name ROS \rightarrow MRPT conversions */ -/**\brief Convert [ROS] NetworkOfPoses \rightarrow [MRPT] CNetworkOfPoses*DInf. - * \param[in] mrpt_graph ROS graph representation +/**\name ROS2 \rightarrow MRPT conversions */ +/**\brief Convert [ROS2] NetworkOfPoses \rightarrow [MRPT] CNetworkOfPoses*DInf. + * \param[in] mrpt_graph ROS2 graph representation * \param[out] ros_graph MRPT graph representation */ /**\{ */ void fromROS( const mrpt::graphs::CNetworkOfPoses2D& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void fromROS( const mrpt::graphs::CNetworkOfPoses3D& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph); void fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph); void fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph); void fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph); /**\} */ -} // namespace mrpt_msgs_bridge +} // namespace mrpt_msgs_bridge \ No newline at end of file diff --git a/mrpt_msgs_bridge/package.xml b/mrpt_msgs_bridge/package.xml index d0f0b222..edf47e54 100644 --- a/mrpt_msgs_bridge/package.xml +++ b/mrpt_msgs_bridge/package.xml @@ -6,23 +6,28 @@ https://www.mrpt.org/ José Luis Blanco-Claraco + Shravan S Rai + José Luis Blanco-Claraco + Shravan S Rai + BSD https://wiki.ros.org/mrpt_msgs_bridge ros_environment - + ament_cmake catkin mrpt2 geometry_msgs visualization_msgs + marker_msgs mrpt_msgs tf2 - catkin + ament_cmake diff --git a/mrpt_msgs_bridge/src/beacon.cpp b/mrpt_msgs_bridge/src/beacon.cpp index 61045b8b..e5c8ae93 100644 --- a/mrpt_msgs_bridge/src/beacon.cpp +++ b/mrpt_msgs_bridge/src/beacon.cpp @@ -2,17 +2,14 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ +#include +#include +#include -#include -#include -#include -#include -#include -#include #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/transform_datatypes.h" @@ -20,10 +17,10 @@ using namespace mrpt::obs; bool mrpt_msgs_bridge::fromROS( - const mrpt_msgs::ObservationRangeBeacon& _msg, + const mrpt_msgs::msg::ObservationRangeBeacon _msg, const mrpt::poses::CPose3D& _pose, CObservationBeaconRanges& _obj) { - _obj.timestamp = mrpt::ros1bridge::fromROS(_msg.header.stamp); + _obj.timestamp = mrpt::ros2bridge::fromROS(_msg.header.stamp); mrpt::poses::CPose3D cpose_obj; _obj.stdError = _msg.sensor_std_range; @@ -33,7 +30,7 @@ bool mrpt_msgs_bridge::fromROS( if (_pose.empty()) { - cpose_obj = mrpt::ros1bridge::fromROS(_msg.sensor_pose_on_robot); + cpose_obj = mrpt::ros2bridge::fromROS(_msg.sensor_pose_on_robot); _obj.setSensorPose(cpose_obj); } else @@ -56,13 +53,13 @@ bool mrpt_msgs_bridge::fromROS( bool mrpt_msgs_bridge::toROS( const CObservationBeaconRanges& _obj, - mrpt_msgs::ObservationRangeBeacon& _msg) + mrpt_msgs::msg::ObservationRangeBeacon& _msg) { mrpt::poses::CPose3D cpose_obj; - _msg.header.stamp = mrpt::ros1bridge::toROS(_obj.timestamp); + _msg.header.stamp = mrpt::ros2bridge::toROS(_obj.timestamp); _obj.getSensorPose(cpose_obj); - _msg.sensor_pose_on_robot = mrpt::ros1bridge::toROS_Pose(cpose_obj); + _msg.sensor_pose_on_robot = mrpt::ros2bridge::toROS_Pose(cpose_obj); _msg.sensor_std_range = _obj.stdError; _msg.header.frame_id = _obj.sensorLabel; @@ -84,11 +81,11 @@ bool mrpt_msgs_bridge::toROS( bool mrpt_msgs_bridge::toROS( const CObservationBeaconRanges& _obj, - mrpt_msgs::ObservationRangeBeacon& _msg, geometry_msgs::Pose& _pose) + mrpt_msgs::msg::ObservationRangeBeacon& _msg, geometry_msgs::msg::Pose& _pose) { toROS(_obj, _msg); mrpt::poses::CPose3D pose; _obj.getSensorPose(pose); - _pose = mrpt::ros1bridge::toROS_Pose(pose); + _pose = mrpt::ros2bridge::toROS_Pose(pose); return true; -} +} \ No newline at end of file diff --git a/mrpt_msgs_bridge/src/landmark.cpp b/mrpt_msgs_bridge/src/landmark.cpp index 000d9ac4..246c54f4 100644 --- a/mrpt_msgs_bridge/src/landmark.cpp +++ b/mrpt_msgs_bridge/src/landmark.cpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -13,20 +13,18 @@ * */ -#include -#include -#include -#include -#include -#include +#include +#include +#include + bool mrpt_msgs_bridge::fromROS( - const mrpt_msgs::ObservationRangeBearing& _msg, + const mrpt_msgs::msg::ObservationRangeBearing& _msg, const mrpt::poses::CPose3D& _pose, mrpt::obs::CObservationBearingRange& _obj) { - _obj.timestamp = mrpt::ros1bridge::fromROS(_msg.header.stamp); + _obj.timestamp = mrpt::ros2bridge::fromROS(_msg.header.stamp); mrpt::poses::CPose3D cpose_obj; @@ -41,7 +39,7 @@ bool mrpt_msgs_bridge::fromROS( if (_pose.empty()) { _obj.setSensorPose( - mrpt::ros1bridge::fromROS(_msg.sensor_pose_on_robot)); + mrpt::ros2bridge::fromROS(_msg.sensor_pose_on_robot)); } else { @@ -65,11 +63,11 @@ bool mrpt_msgs_bridge::fromROS( bool mrpt_msgs_bridge::toROS( const mrpt::obs::CObservationBearingRange& _obj, - mrpt_msgs::ObservationRangeBearing& _msg) + mrpt_msgs::msg::ObservationRangeBearing& _msg) { - _msg.header.stamp = mrpt::ros1bridge::toROS(_obj.timestamp); + _msg.header.stamp = mrpt::ros2bridge::toROS(_obj.timestamp); - _msg.sensor_pose_on_robot = mrpt::ros1bridge::toROS_Pose(_obj.sensorPose()); + _msg.sensor_pose_on_robot = mrpt::ros2bridge::toROS_Pose(_obj.sensorPose()); _msg.max_sensor_distance = _obj.maxSensorDistance; _msg.min_sensor_distance = _obj.minSensorDistance; @@ -94,9 +92,9 @@ bool mrpt_msgs_bridge::toROS( bool mrpt_msgs_bridge::toROS( const mrpt::obs::CObservationBearingRange& _obj, - mrpt_msgs::ObservationRangeBearing& _msg, geometry_msgs::Pose& sensorPose) + mrpt_msgs::msg::ObservationRangeBearing& _msg, geometry_msgs::msg::Pose& sensorPose) { toROS(_obj, _msg); - sensorPose = mrpt::ros1bridge::toROS_Pose(_obj.sensorPose()); + sensorPose = mrpt::ros2bridge::toROS_Pose(_obj.sensorPose()); return true; -} +} \ No newline at end of file diff --git a/mrpt_msgs_bridge/src/marker_msgs.cpp b/mrpt_msgs_bridge/src/marker_msgs.cpp index 4dcbf43a..864f9732 100644 --- a/mrpt_msgs_bridge/src/marker_msgs.cpp +++ b/mrpt_msgs_bridge/src/marker_msgs.cpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | - | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -14,17 +14,15 @@ * @brief Funtions to convert marker_msgs to mrpt msgs **/ -#include -#include -#include -#include +#include +#include bool mrpt_msgs_bridge::fromROS( - const marker_msgs::MarkerDetection& src, + const marker_msgs::msg::MarkerDetection& src, const mrpt::poses::CPose3D& sensorPoseOnRobot, mrpt::obs::CObservationBearingRange& des) { - des.timestamp = mrpt::ros1bridge::fromROS(src.header.stamp); + des.timestamp = mrpt::ros2bridge::fromROS(src.header.stamp); des.setSensorPose(sensorPoseOnRobot); des.minSensorDistance = src.distance_min; des.maxSensorDistance = src.distance_max; @@ -32,7 +30,7 @@ bool mrpt_msgs_bridge::fromROS( des.sensedData.resize(src.markers.size()); for (size_t i = 0; i < src.markers.size(); i++) { - const marker_msgs::Marker& marker = src.markers[i]; + const marker_msgs::msg::Marker& marker = src.markers[i]; // mrpt::obs::CObservationBearingRange::TMeasurement auto& m = des.sensedData[i]; m.range = sqrt( @@ -53,11 +51,11 @@ bool mrpt_msgs_bridge::fromROS( } bool mrpt_msgs_bridge::fromROS( - const marker_msgs::MarkerDetection& src, + const marker_msgs::msg::MarkerDetection& src, const mrpt::poses::CPose3D& sensorPoseOnRobot, mrpt::obs::CObservationBeaconRanges& des) { - des.timestamp = mrpt::ros1bridge::fromROS(src.header.stamp); + des.timestamp = mrpt::ros2bridge::fromROS(src.header.stamp); des.setSensorPose(sensorPoseOnRobot); des.minSensorDistance = src.distance_min; @@ -66,7 +64,7 @@ bool mrpt_msgs_bridge::fromROS( des.sensedData.resize(src.markers.size()); for (size_t i = 0; i < src.markers.size(); i++) { - const marker_msgs::Marker& marker = src.markers[i]; + const marker_msgs::msg::Marker& marker = src.markers[i]; // mrpt::obs::CObservationBeaconRanges::TMeasurement auto& m = des.sensedData[i]; m.sensedDistance = sqrt( @@ -83,4 +81,4 @@ bool mrpt_msgs_bridge::fromROS( } } return true; -} +} \ No newline at end of file diff --git a/mrpt_msgs_bridge/src/network_of_poses.cpp b/mrpt_msgs_bridge/src/network_of_poses.cpp index ccb359ad..9f2f546d 100644 --- a/mrpt_msgs_bridge/src/network_of_poses.cpp +++ b/mrpt_msgs_bridge/src/network_of_poses.cpp @@ -2,32 +2,31 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ -#include -#include +#include +#include #include -#include -#include -#include +#include +#include #include // for debugging reasons using mrpt::graphs::TNodeID; /////////////////////////////////////////////////////////////////////////////////////////// -// MRPT => ROS +// MRPT => ROS2 /////////////////////////////////////////////////////////////////////////////////////////// void mrpt_msgs_bridge::toROS( const mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph) + mrpt_msgs::msg::NetworkOfPoses& ros_graph) { MRPT_START - using namespace geometry_msgs; + using namespace geometry_msgs::msg; using namespace mrpt::math; using namespace mrpt::graphs; using namespace mrpt::poses; @@ -46,12 +45,12 @@ void mrpt_msgs_bridge::toROS( for (poses_cit_t poses_cit = mrpt_graph.nodes.begin(); poses_cit != mrpt_graph.nodes.end(); ++poses_cit) { - mrpt_msgs::NodeIDWithPose ros_node; + mrpt_msgs::msg::NodeIDWithPose ros_node; // nodeID ros_node.node_id = poses_cit->first; // pose - ros_node.pose = mrpt::ros1bridge::toROS_Pose(poses_cit->second); + ros_node.pose = mrpt::ros2bridge::toROS_Pose(poses_cit->second); // zero the optional fields ros_node.str_id.data = ""; @@ -63,7 +62,7 @@ void mrpt_msgs_bridge::toROS( // fill the constraints for (const auto& edge : constraints) { - mrpt_msgs::GraphConstraint ros_constr; + mrpt_msgs::msg::GraphConstraint ros_constr; // constraint ends ros_constr.node_id_from = edge.first.first; @@ -74,11 +73,11 @@ void mrpt_msgs_bridge::toROS( { CPosePDFGaussianInf constr_inv; edge.second.inverse(constr_inv); - ros_constr.constraint = mrpt::ros1bridge::toROS(constr_inv); + ros_constr.constraint = mrpt::ros2bridge::toROS(constr_inv); } else { - ros_constr.constraint = mrpt::ros1bridge::toROS(edge.second); + ros_constr.constraint = mrpt::ros2bridge::toROS(edge.second); } ros_graph.constraints.push_back(ros_constr); @@ -89,7 +88,7 @@ void mrpt_msgs_bridge::toROS( void mrpt_msgs_bridge::toROS( [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph, - [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph) + [[maybe_unused]] mrpt_msgs::msg::NetworkOfPoses& ros_graph) { THROW_EXCEPTION("Conversion not implemented yet"); // TODO: Implement CNetworkOfPoses3DInf => mrpt_msgs::NetworkOfPoses @@ -98,11 +97,11 @@ void mrpt_msgs_bridge::toROS( void mrpt_msgs_bridge::toROS( const mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph) + mrpt_msgs::msg::NetworkOfPoses& ros_graph) { MRPT_START - using namespace geometry_msgs; + using namespace geometry_msgs::msg; using namespace mrpt::math; using namespace mrpt::graphs; using namespace mrpt::poses; @@ -128,12 +127,12 @@ void mrpt_msgs_bridge::toROS( for (poses_cit_t poses_cit = mrpt_graph.nodes.begin(); poses_cit != mrpt_graph.nodes.end(); ++poses_cit) { - mrpt_msgs::NodeIDWithPose ros_node; + mrpt_msgs::msg::NodeIDWithPose ros_node; // nodeID ros_node.node_id = poses_cit->first; // pose - ros_node.pose = mrpt::ros1bridge::toROS_Pose(poses_cit->second); + ros_node.pose = mrpt::ros2bridge::toROS_Pose(poses_cit->second); // optional fields for the MR-SLAM case ros_node.str_id.data = poses_cit->second.agent_ID_str; @@ -146,7 +145,7 @@ void mrpt_msgs_bridge::toROS( // CNetworkOfPoses2DInf for (const auto& edge : constraints) { - mrpt_msgs::GraphConstraint ros_constr; + mrpt_msgs::msg::GraphConstraint ros_constr; // constraint ends ros_constr.node_id_from = edge.first.first; @@ -157,11 +156,11 @@ void mrpt_msgs_bridge::toROS( { CPosePDFGaussianInf constr_inv; edge.second.inverse(constr_inv); - ros_constr.constraint = mrpt::ros1bridge::toROS(constr_inv); + ros_constr.constraint = mrpt::ros2bridge::toROS(constr_inv); } else { - ros_constr.constraint = mrpt::ros1bridge::toROS(edge.second); + ros_constr.constraint = mrpt::ros2bridge::toROS(edge.second); } ros_graph.constraints.push_back(ros_constr); @@ -172,22 +171,22 @@ void mrpt_msgs_bridge::toROS( void mrpt_msgs_bridge::toROS( [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph, - [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph) + [[maybe_unused]] mrpt_msgs::msg::NetworkOfPoses& ros_graph) { THROW_EXCEPTION("Conversion not implemented yet"); } /////////////////////////////////////////////////////////////////////////////////////////// -// ROS => MRPT +// ROS2 => MRPT /////////////////////////////////////////////////////////////////////////////////////////// void mrpt_msgs_bridge::fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph) { MRPT_START using namespace mrpt::poses; - using namespace mrpt_msgs; + using namespace mrpt_msgs::msg; using namespace std; typedef NetworkOfPoses::_nodes_type::_vec_type::const_iterator nodes_cit_t; @@ -201,7 +200,7 @@ void mrpt_msgs_bridge::fromROS( nodes_cit != ros_graph.nodes.vec.end(); ++nodes_cit) { // get the pose - CPose2D mrpt_pose = CPose2D(mrpt::ros1bridge::fromROS(nodes_cit->pose)); + CPose2D mrpt_pose = CPose2D(mrpt::ros2bridge::fromROS(nodes_cit->pose)); mrpt_graph.nodes.insert( make_pair(static_cast(nodes_cit->node_id), mrpt_pose)); @@ -218,7 +217,7 @@ void mrpt_msgs_bridge::fromROS( // constraint value const auto mrpt_constr = mrpt::poses::CPosePDFGaussianInf( - mrpt::ros1bridge::fromROS(constr_cit->constraint)); + mrpt::ros2bridge::fromROS(constr_cit->constraint)); mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr)); } @@ -229,7 +228,7 @@ void mrpt_msgs_bridge::fromROS( } void mrpt_msgs_bridge::fromROS( - [[maybe_unused]] const mrpt_msgs::NetworkOfPoses& ros_graph, + [[maybe_unused]] const mrpt_msgs::msg::NetworkOfPoses& ros_graph, [[maybe_unused]] mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph) { THROW_EXCEPTION("Conversion not implemented yet"); @@ -238,12 +237,12 @@ void mrpt_msgs_bridge::fromROS( } void mrpt_msgs_bridge::fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph) { MRPT_START using namespace mrpt::poses; - using namespace mrpt_msgs; + using namespace mrpt_msgs::msg; using namespace std; using nodes_cit_t = NetworkOfPoses::_nodes_type::_vec_type::const_iterator; @@ -261,7 +260,7 @@ void mrpt_msgs_bridge::fromROS( { // set the nodeID/pose mrpt_graph_pose_t mrpt_node = - mrpt::ros1bridge::fromROS(nodes_cit->pose); + mrpt::ros2bridge::fromROS(nodes_cit->pose); // set the MR-SLAM fields mrpt_node.agent_ID_str = nodes_cit->str_id.data; @@ -282,7 +281,7 @@ void mrpt_msgs_bridge::fromROS( // constraint value const auto mrpt_constr = mrpt::poses::CPosePDFGaussianInf( - mrpt::ros1bridge::fromROS(constr_cit->constraint)); + mrpt::ros2bridge::fromROS(constr_cit->constraint)); mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr)); } @@ -293,8 +292,8 @@ void mrpt_msgs_bridge::fromROS( } void convert( - [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph, + [[maybe_unused]] mrpt_msgs::msg::NetworkOfPoses& ros_graph, [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph) { THROW_EXCEPTION("Conversion not implemented yet"); -} +} \ No newline at end of file diff --git a/mrpt_navigation/CMakeLists.txt b/mrpt_navigation/CMakeLists.txt index eb6284f8..aa2c0b25 100644 --- a/mrpt_navigation/CMakeLists.txt +++ b/mrpt_navigation/CMakeLists.txt @@ -1,4 +1,10 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) + project(mrpt_navigation) -find_package(catkin REQUIRED) -catkin_metapackage() + +# Find dependencies +find_package(ament_cmake REQUIRED) + + +# The following is needed for ROS 2's ament to generate the setup hooks +ament_package() \ No newline at end of file diff --git a/mrpt_navigation/package.xml b/mrpt_navigation/package.xml index d19561f3..621deb73 100644 --- a/mrpt_navigation/package.xml +++ b/mrpt_navigation/package.xml @@ -13,7 +13,7 @@ BSD https://wiki.ros.org/mrpt_navigation - catkin + ament_cmake mrpt_local_obstacles mrpt_localization @@ -23,6 +23,6 @@ mrpt_tutorials - + ament_cmake diff --git a/mrpt_reactivenav2d/CATKIN_IGNORE b/mrpt_reactivenav2d/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/mrpt_reactivenav2d/CMakeLists.txt b/mrpt_reactivenav2d/CMakeLists.txt index 0f18089f..c7c5399e 100644 --- a/mrpt_reactivenav2d/CMakeLists.txt +++ b/mrpt_reactivenav2d/CMakeLists.txt @@ -1,27 +1,33 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.5) project(mrpt_reactivenav2d) -## 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 - actionlib - actionlib_msgs - dynamic_reconfigure - geometry_msgs - roscpp - tf2 - tf2_ros - tf2_geometry_msgs - visualization_msgs -) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs) +find_package(sensor_msgs REQUIRED) +find_package(mrpt_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros) +find_package(tf2_geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + ## System dependencies are found with CMake's conventions -find_package(mrpt-ros1bridge REQUIRED) +find_package(mrpt-ros2bridge REQUIRED) find_package(mrpt-nav REQUIRED) find_package(mrpt-kinematics REQUIRED) message(STATUS "MRPT_VERSION: ${MRPT_VERSION}") +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. @@ -37,60 +43,41 @@ 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( -# INCLUDE_DIRS include -# LIBRARIES mrpt_reactivenav2d - CATKIN_DEPENDS - actionlib - actionlib_msgs - dynamic_reconfigure - geometry_msgs - roscpp - tf2 - tf2_ros - tf2_geometry_msgs - visualization_msgs -# DEPENDS mrpt -) - ########### ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(mrpt_reactivenav2d -# src/${PROJECT_NAME}/mrpt_reactivenav2d.cpp -# ) - ## Declare a cpp executable -add_executable(mrpt_reactivenav2d_node src/mrpt_reactivenav2d_node.cpp) +add_executable(${PROJECT_NAME}_node + src/mrpt_reactivenav2d_node.cpp + include/${PROJECT_NAME}/mrpt_reactivenav2d_node.hpp +) -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(mrpt_reactivenav2d_node mrpt_reactivenav2d_generate_messages_cpp) +target_include_directories(${PROJECT_NAME}_node + PUBLIC + $ + $ +) ## Specify libraries to link a library or executable target against -target_link_libraries(mrpt_reactivenav2d_node - ${catkin_LIBRARIES} +target_link_libraries(${PROJECT_NAME}_node mrpt::nav mrpt::kinematics - mrpt::ros1bridge + mrpt::ros2bridge +) + +ament_target_dependencies( + ${PROJECT_NAME}_node + "rclcpp" + "rclcpp_components" + "nav_msgs" + "sensor_msgs" + "geometry_msgs" + "mrpt_msgs" + "tf2" + "tf2_ros" + "tf2_geometry_msgs" + "visualization_msgs" ) ############# @@ -102,19 +89,29 @@ target_link_libraries(mrpt_reactivenav2d_node ## Mark executables and/or libraries for installation -install(TARGETS mrpt_reactivenav2d_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} ) ## Mark other files for installation (e.g. launch and bag files, etc.) install(DIRECTORY launch tutorial - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + 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() \ No newline at end of file diff --git a/mrpt_reactivenav2d/COLCON_IGNORE b/mrpt_reactivenav2d/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp new file mode 100644 index 00000000..9db9fdf8 --- /dev/null +++ b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp @@ -0,0 +1,277 @@ +/*********************************************************************************** + * Revised BSD License * + * Copyright (c) 2014-2023, Jose-Luis Blanco * + * 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. * + ***********************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace mrpt::nav; +using mrpt::maps::CSimplePointsMap; +using namespace mrpt::system; +using namespace mrpt::config; + +// The ROS node +class ReactiveNav2DNode: public rclcpp::Node +{ + public: + /* Ctor*/ + explicit ReactiveNav2DNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + /* Dtor*/ + ~ReactiveNav2DNode(){} + + private: + // methods + void read_parameters(); + void navigate_to(const mrpt::math::TPose2D& target); + void on_do_navigation(); + void on_goal_received(const geometry_msgs::msg::PoseStamped::SharedPtr& trg_ptr); + void on_local_obstacles(const sensor_msgs::msg::PointCloud2::SharedPtr& obs); + void on_set_robot_shape(const geometry_msgs::msg::Polygon::SharedPtr& newShape); + void on_odometry_received(const nav_msgs::msg::Odometry::SharedPtr& odom); + void update_waypoint_sequence(const mrpt_msgs::msg::WaypointSequence::SharedPtr& wp); + void on_waypoint_seq_received(const mrpt_msgs::msg::WaypointSequence::SharedPtr& wps); + + + private: + /** @name ROS pubs/subs + * @{ */ + rclcpp::Subscription::SharedPtr m_sub_odometry; + rclcpp::Subscription::SharedPtr m_sub_wp_seq; + rclcpp::Subscription::SharedPtr m_sub_nav_goal; + rclcpp::Subscription::SharedPtr m_sub_local_obs; + rclcpp::Subscription::SharedPtr m_sub_robot_shape; + rclcpp::Publisher::SharedPtr m_pub_cmd_vel; + + std::shared_ptr m_tf_buffer; + std::shared_ptr m_tf_listener; + /** @} */ + + CTimeLogger m_profiler; + bool m_1st_time_init; //!< Reactive initialization done? + double m_target_allowed_distance; + double m_nav_period; + + std::string m_sub_topic_reactive_nav_goal = "reactive_nav_goal"; + std::string m_sub_topic_local_obstacles = "local_map_pointcloud"; + std::string m_sub_topic_robot_shape{}; + std::string m_sub_topic_wp_seq = "reactive_nav_waypoints"; + std::string m_sub_topic_odometry = "odom"; + + std::string m_pub_topic_cmd_vel = "cmd_vel"; + + std::string m_frameid_reference = "map"; + std::string m_frameid_robot = "base_link"; + + std::string m_plugin_file; + std::string m_cfg_file_reactive; + + bool m_save_nav_log; + + rclcpp::TimerBase::SharedPtr m_timer_run_nav; + + mrpt::obs::CObservationOdometry m_odometry; + CSimplePointsMap m_last_obstacles; + std::mutex m_last_obstacles_cs; + std::mutex m_odometry_cs; + + struct MyReactiveInterface : public mrpt::nav::CRobot2NavInterface + { + ReactiveNav2DNode& m_parent; + + MyReactiveInterface(ReactiveNav2DNode& parent) : m_parent(parent) {} + + /** Get the current pose and speeds of the robot. + * \param curPose Current robot pose. + * \param curV Current linear speed, in meters per second. + * \param curW Current angular speed, in radians per second. + * \return false on any error. + */ + bool getCurrentPoseAndSpeeds( + mrpt::math::TPose2D& curPose, mrpt::math::TTwist2D& curVel, + mrpt::system::TTimeStamp& timestamp, + mrpt::math::TPose2D& curOdometry, std::string& frame_id) override + { + double curV, curW; + + CTimeLoggerEntry tle( + m_parent.m_profiler, "getCurrentPoseAndSpeeds"); + + //rclcpp::Duration timeout(0.1); + rclcpp::Duration timeout(std::chrono::milliseconds(100)); + + geometry_msgs::msg::TransformStamped tfGeom; + try + { + CTimeLoggerEntry tle2( + m_parent.m_profiler, + "getCurrentPoseAndSpeeds.lookupTransform_sensor"); + + tfGeom = m_parent.m_tf_buffer->lookupTransform( + m_parent.m_frameid_reference, m_parent.m_frameid_robot, + tf2::TimePointZero, tf2::durationFromSec(timeout.seconds())); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR(m_parent.get_logger(), "%s", ex.what()); + return false; + } + + tf2::Transform txRobotPose; + tf2::fromMsg(tfGeom.transform, txRobotPose); + + const mrpt::poses::CPose3D curRobotPose = + mrpt::ros2bridge::fromROS(txRobotPose); + + timestamp = mrpt::ros2bridge::fromROS(tfGeom.header.stamp); + + // Explicit 3d->2d to confirm we know we're losing information + curPose = mrpt::poses::CPose2D(curRobotPose).asTPose(); + curOdometry = curPose; + + curV = curW = 0; + MRPT_TODO("Retrieve current speeds from /odom topic?"); + RCLCPP_DEBUG(m_parent.get_logger(), + "[getCurrentPoseAndSpeeds] Latest pose: %s", + curPose.asString().c_str()); + + // From local to global: + curVel = mrpt::math::TTwist2D(curV, .0, curW).rotated(curPose.phi); + + return true; + } + + /** Change the instantaneous speeds of robot. + * \param v Linear speed, in meters per second. + * \param w Angular speed, in radians per second. + * \return false on any error. + */ + bool changeSpeeds( + const mrpt::kinematics::CVehicleVelCmd& vel_cmd) override + { + using namespace mrpt::kinematics; + const CVehicleVelCmd_DiffDriven* vel_cmd_diff_driven = + dynamic_cast(&vel_cmd); + ASSERT_(vel_cmd_diff_driven); + + const double v = vel_cmd_diff_driven->lin_vel; + const double w = vel_cmd_diff_driven->ang_vel; + RCLCPP_DEBUG(m_parent.get_logger(), + "changeSpeeds: v=%7.4f m/s w=%8.3f deg/s", v, + w * 180.0f / M_PI); + geometry_msgs::msg::Twist cmd; + cmd.linear.x = v; + cmd.angular.z = w; + m_parent.m_pub_cmd_vel->publish(cmd); + return true; + } + + bool stop(bool isEmergency) override + { + mrpt::kinematics::CVehicleVelCmd_DiffDriven vel_cmd; + vel_cmd.lin_vel = 0; + vel_cmd.ang_vel = 0; + return changeSpeeds(vel_cmd); + } + + /** Start the watchdog timer of the robot platform, if any. + * \param T_ms Period, in ms. + * \return false on any error. */ + virtual bool startWatchdog(float T_ms) override { return true; } + /** Stop the watchdog timer. + * \return false on any error. */ + virtual bool stopWatchdog() override { return true; } + /** Return the current set of obstacle points. + * \return false on any error. */ + bool senseObstacles( + CSimplePointsMap& obstacles, + mrpt::system::TTimeStamp& timestamp) override + { + timestamp = mrpt::system::now(); + std::lock_guard csl(m_parent.m_last_obstacles_cs); + obstacles = m_parent.m_last_obstacles; + + MRPT_TODO("TODO: Check age of obstacles!"); + return true; + } + + mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd() override + { + return getStopCmd(); + } + mrpt::kinematics::CVehicleVelCmd::Ptr getStopCmd() override + { + mrpt::kinematics::CVehicleVelCmd::Ptr ret = + mrpt::kinematics::CVehicleVelCmd::Ptr( + new mrpt::kinematics::CVehicleVelCmd_DiffDriven); + ret->setToStop(); + return ret; + } + + void sendNavigationStartEvent() override {} + void sendNavigationEndEvent() override {} + void sendNavigationEndDueToErrorEvent() override {} + void sendWaySeemsBlockedEvent() override {} + }; + + MyReactiveInterface m_reactive_if; + + CReactiveNavigationSystem m_reactive_nav_engine; + std::mutex m_reactive_nav_engine_cs; +}; \ No newline at end of file diff --git a/mrpt_reactivenav2d/package.xml b/mrpt_reactivenav2d/package.xml index 4731ad18..2c01eab9 100644 --- a/mrpt_reactivenav2d/package.xml +++ b/mrpt_reactivenav2d/package.xml @@ -1,33 +1,34 @@ - + mrpt_reactivenav2d 1.0.3 Reactive navigation for 2D robots using MRPT navigation algorithms (TP-Space) Jose-Luis Blanco-Claraco + Shravan S Rai + + Jose-Luis Blanco-Claraco BSD https://wiki.ros.org/mrpt_reactivenav2d - Jose-Luis Blanco-Claraco - - - catkin + ament_cmake mrpt2 - actionlib - actionlib_msgs - dynamic_reconfigure + rclcpp + rclcpp_components + nav_msgs + sensor_msgs geometry_msgs - roscpp + mrpt_msgs tf2 tf2_ros tf2_geometry_msgs visualization_msgs - + ament_cmake diff --git a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp index 424cc106..2c7bc7db 100644 --- a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp +++ b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp @@ -1,479 +1,412 @@ -/*********************************************************************************** - * Revised BSD License * - * Copyright (c) 2014-2015, Jose-Luis Blanco * - * 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. * - ***********************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include +#include +#include "mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp" using namespace mrpt::nav; using mrpt::maps::CSimplePointsMap; using namespace mrpt::system; using namespace mrpt::config; -// The ROS node -class ReactiveNav2DNode +/** Constructor: Inits ROS system */ +ReactiveNav2DNode::ReactiveNav2DNode(const rclcpp::NodeOptions& options) +: Node("mrpt_reactivenav2d", options) +, m_1st_time_init(false) +, m_target_allowed_distance(0.40f) +, m_nav_period(0.100) +, m_save_nav_log(false) +, m_reactive_if(*this) +, m_reactive_nav_engine(m_reactive_if) { - private: - struct TAuxInitializer - { - TAuxInitializer(int argc, char** argv) - { - ros::init(argc, argv, "mrpt_reactivenav2d"); - } - }; - - CTimeLogger m_profiler; - TAuxInitializer m_auxinit; //!< Just to make sure ROS is init first - ros::NodeHandle m_nh; //!< The node handle - ros::NodeHandle m_localn; //!< "~" - - /** @name ROS pubs/subs - * @{ */ - ros::Subscriber m_sub_nav_goal; - ros::Subscriber m_sub_local_obs; - ros::Subscriber m_sub_robot_shape; - ros::Publisher m_pub_cmd_vel; - - tf2_ros::Buffer m_tf_buffer; - tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; - /** @} */ - - bool m_1st_time_init; //!< Reactive initialization done? - double m_target_allowed_distance; - double m_nav_period; - - std::string m_pub_topic_reactive_nav_goal; - std::string m_sub_topic_local_obstacles; - std::string m_sub_topic_robot_shape; + // Load params + read_parameters(); - std::string m_frameid_reference; - std::string m_frameid_robot; + assert(m_nav_period > 0); - bool m_save_nav_log; + if (m_cfg_file_reactive.empty()) + { + RCLCPP_ERROR(this->get_logger(), + "Mandatory param 'cfg_file_reactive' is missing!"); + throw; + } - ros::Timer m_timer_run_nav; + if (!mrpt::system::fileExists(m_cfg_file_reactive)) + { + RCLCPP_ERROR(this->get_logger(), + "Config file not found: %s", m_cfg_file_reactive.c_str()); + throw; + } - CSimplePointsMap m_last_obstacles; - std::mutex m_last_obstacles_cs; + m_reactive_nav_engine.enableLogFile(m_save_nav_log); - struct MyReactiveInterface : public mrpt::nav::CRobot2NavInterface + // Load reactive config: + // ---------------------------------------------------- + if(!m_cfg_file_reactive.empty()) { - ReactiveNav2DNode& m_parent; - - MyReactiveInterface(ReactiveNav2DNode& parent) : m_parent(parent) {} - - /** Get the current pose and speeds of the robot. - * \param curPose Current robot pose. - * \param curV Current linear speed, in meters per second. - * \param curW Current angular speed, in radians per second. - * \return false on any error. - */ - bool getCurrentPoseAndSpeeds( - mrpt::math::TPose2D& curPose, mrpt::math::TTwist2D& curVel, - mrpt::system::TTimeStamp& timestamp, - mrpt::math::TPose2D& curOdometry, std::string& frame_id) override + try { - double curV, curW; - - CTimeLoggerEntry tle( - m_parent.m_profiler, "getCurrentPoseAndSpeeds"); - - ros::Duration timeout(0.1); - - geometry_msgs::TransformStamped tfGeom; - try - { - CTimeLoggerEntry tle2( - m_parent.m_profiler, - "getCurrentPoseAndSpeeds.lookupTransform_sensor"); + CConfigFile cfgFil(m_cfg_file_reactive); + m_reactive_nav_engine.loadConfigFile(cfgFil); + } + catch (std::exception& e) + { + RCLCPP_ERROR(this->get_logger(), + "Exception initializing reactive navigation engine:\n%s", + e.what()); + throw; + } + } + // load robot shape: (1) default, (2) via params, (3) via topic + // ---------------------------------------------------------------- + // m_reactive_nav_engine.changeRobotShape(); - tfGeom = m_parent.m_tf_buffer.lookupTransform( - m_parent.m_frameid_reference, m_parent.m_frameid_robot, - ros::Time(0), timeout); - } - catch (const tf2::TransformException& ex) + // Init this subscriber first so we know asap the desired robot shape, + // if provided via a topic: + if (!m_sub_topic_robot_shape.empty()) + { + m_sub_robot_shape = this->create_subscription( + m_sub_topic_robot_shape, 1, + [this](const geometry_msgs::msg::Polygon::SharedPtr poly) + {this->on_set_robot_shape(poly);}); + + RCLCPP_INFO(this->get_logger(), + "Params say robot shape will arrive via topic '%s'... waiting 3 seconds for it.", + m_sub_topic_robot_shape.c_str()); + + // Use rate object to implement sleep + rclcpp::Rate rate(1); // 1 Hz + for (int i = 0; i < 3; i++) + { + rclcpp::spin_some(this->get_node_base_interface()); + rate.sleep(); + } + RCLCPP_INFO(this->get_logger(), "Wait done."); + } + else + { + // Load robot shape: 1/2 polygon + // --------------------------------------------- + CConfigFile c(m_cfg_file_reactive); + std::string s = "CReactiveNavigationSystem"; + + std::vector xs, ys; + c.read_vector( + s, "RobotModel_shape2D_xs", std::vector(), xs, false); + c.read_vector( + s, "RobotModel_shape2D_ys", std::vector(), ys, false); + ASSERTMSG_( + xs.size() == ys.size(), + "Config parameters `RobotModel_shape2D_xs` and " + "`RobotModel_shape2D_ys` " + "must have the same length!"); + if (!xs.empty()) + { + mrpt::math::CPolygon poly; + poly.resize(xs.size()); + for (size_t i = 0; i < xs.size(); i++) { - ROS_ERROR("%s", ex.what()); - return false; + poly[i].x = xs[i]; + poly[i].y = ys[i]; } - tf2::Transform txRobotPose; - tf2::fromMsg(tfGeom.transform, txRobotPose); + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.changeRobotShape(poly); + } - const mrpt::poses::CPose3D curRobotPose = - mrpt::ros1bridge::fromROS(txRobotPose); + // Load robot shape: 2/2 circle + // --------------------------------------------- + if (const double robot_radius = c.read_double( + s, "RobotModel_circular_shape_radius", -1.0, false); + robot_radius > 0) + { + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.changeRobotCircularShapeRadius( + robot_radius); + } + } - timestamp = mrpt::ros1bridge::fromROS(tfGeom.header.stamp); + // Init ROS publishers: + // ----------------------- + m_pub_cmd_vel = this->create_publisher(m_pub_topic_cmd_vel, 1); + + // Init ROS subs: + // ----------------------- + m_sub_odometry = this->create_subscription( + m_sub_topic_odometry, 1, + [this](const nav_msgs::msg::Odometry::SharedPtr odom) + {this->on_odometry_received(odom);}); + + m_sub_wp_seq = this->create_subscription( + m_sub_topic_wp_seq, 1, + [this](const mrpt_msgs::msg::WaypointSequence::SharedPtr msg) + {this->on_waypoint_seq_received(msg);}); + + m_sub_nav_goal = this->create_subscription( + m_sub_topic_reactive_nav_goal, 1, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { this->on_goal_received(msg); }); + + m_sub_local_obs = this->create_subscription( + m_sub_topic_local_obstacles, 1, + [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { this->on_local_obstacles(msg); }); + + // Init tf buffers + // ---------------------------------------------------- + m_tf_buffer = std::make_shared(this->get_clock()); + m_tf_listener = std::make_shared(*m_tf_buffer); + + // Init timer: + // ---------------------------------------------------- + m_timer_run_nav = this->create_wall_timer( + std::chrono::duration(m_nav_period), + [this]() { this->on_do_navigation(); }); + + +} // end ctor + +void ReactiveNav2DNode::read_parameters() +{ + this->declare_parameter("cfg_file_reactive", "reactive2d_config.ini"); + this->get_parameter("cfg_file_reactive", m_cfg_file_reactive); + RCLCPP_INFO(this->get_logger(), "cfg_file_reactive %s", m_cfg_file_reactive.c_str()); - // Explicit 3d->2d to confirm we know we're losing information - curPose = mrpt::poses::CPose2D(curRobotPose).asTPose(); - curOdometry = curPose; + this->declare_parameter("target_allowed_distance", 0.40); + this->get_parameter("target_allowed_distance", m_target_allowed_distance); + RCLCPP_INFO(this->get_logger(), "target_allowed_distance: %f", m_target_allowed_distance); - curV = curW = 0; - MRPT_TODO("Retrieve current speeds from /odom topic?"); - ROS_DEBUG( - "[getCurrentPoseAndSpeeds] Latest pose: %s", - curPose.asString().c_str()); + this->declare_parameter("nav_period", 0.10); + this->get_parameter("nav_period", m_nav_period); + RCLCPP_INFO(this->get_logger(), "nav_period: %f", m_nav_period); - // From local to global: - curVel = mrpt::math::TTwist2D(curV, .0, curW).rotated(curPose.phi); + this->declare_parameter("frameid_reference", "odom"); + this->get_parameter("frameid_reference", m_frameid_reference); + RCLCPP_INFO(this->get_logger(), "frameid_reference: %s", m_frameid_reference.c_str()); - return true; - } + this->declare_parameter("frameid_robot", "base_link"); + this->get_parameter("frameid_robot", m_frameid_robot); + RCLCPP_INFO(this->get_logger(), "frameid_robot: %s", m_frameid_robot.c_str()); - /** Change the instantaneous speeds of robot. - * \param v Linear speed, in meters per second. - * \param w Angular speed, in radians per second. - * \return false on any error. - */ - bool changeSpeeds( - const mrpt::kinematics::CVehicleVelCmd& vel_cmd) override - { - using namespace mrpt::kinematics; - const CVehicleVelCmd_DiffDriven* vel_cmd_diff_driven = - dynamic_cast(&vel_cmd); - ASSERT_(vel_cmd_diff_driven); - - const double v = vel_cmd_diff_driven->lin_vel; - const double w = vel_cmd_diff_driven->ang_vel; - ROS_DEBUG( - "changeSpeeds: v=%7.4f m/s w=%8.3f deg/s", v, - w * 180.0f / M_PI); - geometry_msgs::Twist cmd; - cmd.linear.x = v; - cmd.angular.z = w; - m_parent.m_pub_cmd_vel.publish(cmd); - return true; - } + this->declare_parameter("topic_wp_seq", "waypointsequence"); + this->get_parameter("topic_wp_seq", m_sub_topic_wp_seq); + RCLCPP_INFO(this->get_logger(), "topic_wp_seq: %s", m_sub_topic_wp_seq.c_str()); - bool stop(bool isEmergency) override - { - mrpt::kinematics::CVehicleVelCmd_DiffDriven vel_cmd; - vel_cmd.lin_vel = 0; - vel_cmd.ang_vel = 0; - return changeSpeeds(vel_cmd); - } + this->declare_parameter("topic_odometry", "/odometry"); + this->get_parameter("topic_odometry", m_sub_topic_odometry); + RCLCPP_INFO(this->get_logger(), "topic_odometry: %s", m_sub_topic_odometry.c_str()); - /** Start the watchdog timer of the robot platform, if any. - * \param T_ms Period, in ms. - * \return false on any error. */ - virtual bool startWatchdog(float T_ms) override { return true; } - /** Stop the watchdog timer. - * \return false on any error. */ - virtual bool stopWatchdog() override { return true; } - /** Return the current set of obstacle points. - * \return false on any error. */ - bool senseObstacles( - CSimplePointsMap& obstacles, - mrpt::system::TTimeStamp& timestamp) override - { - timestamp = mrpt::system::now(); - std::lock_guard csl(m_parent.m_last_obstacles_cs); - obstacles = m_parent.m_last_obstacles; + this->declare_parameter("topic_cmd_vel", "/cmd_vel"); + this->get_parameter("topic_cmd_vel", m_pub_topic_cmd_vel); + RCLCPP_INFO(this->get_logger(), "topic_cmd_vel: %s", m_pub_topic_cmd_vel.c_str()); - MRPT_TODO("TODO: Check age of obstacles!"); - return true; - } + this->declare_parameter("topic_obstacles", "/pointcloud"); + this->get_parameter("topic_obstacles", m_sub_topic_local_obstacles); + RCLCPP_INFO(this->get_logger(), "topic_obstacles: %s", m_sub_topic_local_obstacles.c_str()); - mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd() override - { - return getStopCmd(); - } - mrpt::kinematics::CVehicleVelCmd::Ptr getStopCmd() override - { - mrpt::kinematics::CVehicleVelCmd::Ptr ret = - mrpt::kinematics::CVehicleVelCmd::Ptr( - new mrpt::kinematics::CVehicleVelCmd_DiffDriven); - ret->setToStop(); - return ret; - } + this->declare_parameter("save_nav_log", false); + this->get_parameter("save_nav_log", m_save_nav_log); + RCLCPP_INFO(this->get_logger(), "save_nav_log: %b", m_save_nav_log); - void sendNavigationStartEvent() override {} - void sendNavigationEndEvent() override {} - void sendNavigationEndDueToErrorEvent() override {} - void sendWaySeemsBlockedEvent() override {} - }; - - MyReactiveInterface m_reactive_if; - - CReactiveNavigationSystem m_reactive_nav_engine; - std::mutex m_reactive_nav_engine_cs; - - public: - /** Constructor: Inits ROS system */ - ReactiveNav2DNode(int argc, char** argv) - : m_auxinit(argc, argv), - m_nh(), - m_localn("~"), - m_1st_time_init(false), - m_target_allowed_distance(0.40f), - m_nav_period(0.100), - m_pub_topic_reactive_nav_goal("reactive_nav_goal"), - m_sub_topic_local_obstacles("local_map_pointcloud"), - m_sub_topic_robot_shape(""), - m_frameid_reference("map"), - m_frameid_robot("base_link"), - m_save_nav_log(false), - m_reactive_if(*this), - m_reactive_nav_engine(m_reactive_if) - { - // Load params: - std::string cfg_file_reactive; - m_localn.param( - "cfg_file_reactive", cfg_file_reactive, cfg_file_reactive); - m_localn.param( - "target_allowed_distance", m_target_allowed_distance, - m_target_allowed_distance); - m_localn.param("nav_period", m_nav_period, m_nav_period); - m_localn.param( - "frameid_reference", m_frameid_reference, m_frameid_reference); - m_localn.param("frameid_robot", m_frameid_robot, m_frameid_robot); - m_localn.param( - "topic_robot_shape", m_sub_topic_robot_shape, - m_sub_topic_robot_shape); - m_localn.param("save_nav_log", m_save_nav_log, m_save_nav_log); - - ROS_ASSERT(m_nav_period > 0); - ROS_ASSERT_MSG( - !cfg_file_reactive.empty(), - "Mandatory param 'cfg_file_reactive' is missing!"); - ROS_ASSERT_MSG( - mrpt::system::fileExists(cfg_file_reactive), - "Config file not found: '%s'", cfg_file_reactive.c_str()); - - m_reactive_nav_engine.enableLogFile(m_save_nav_log); - - // Load reactive config: - // ---------------------------------------------------- - try - { - CConfigFile cfgFil(cfg_file_reactive); - m_reactive_nav_engine.loadConfigFile(cfgFil); - } - catch (std::exception& e) - { - ROS_ERROR( - "Exception initializing reactive navigation engine:\n%s", - e.what()); - throw; - } + this->declare_parameter("ptg_plugin_files", ""); + this->get_parameter("ptg_plugin_files", m_plugin_file); + RCLCPP_INFO(this->get_logger(), "ptg_plugin_files: %s", m_plugin_file.c_str()); - // load robot shape: (1) default, (2) via params, (3) via topic - // ---------------------------------------------------------------- - // m_reactive_nav_engine.changeRobotShape(); - - // Init this subscriber first so we know asap the desired robot shape, - // if provided via a topic: - if (!m_sub_topic_robot_shape.empty()) + if (!m_plugin_file.empty()) + { + RCLCPP_INFO_STREAM( this->get_logger(), + "About to load plugins: " << m_plugin_file); + std::string errorMsgs; + if (!mrpt::system::loadPluginModules(m_plugin_file, errorMsgs)) { - m_sub_robot_shape = m_nh.subscribe( - m_sub_topic_robot_shape, 1, - &ReactiveNav2DNode::onRosSetRobotShape, this); - ROS_INFO( - "Params say robot shape will arrive via topic '%s'... waiting " - "3 seconds for it.", - m_sub_topic_robot_shape.c_str()); - ros::Duration(3.0).sleep(); - for (size_t i = 0; i < 100; i++) ros::spinOnce(); - ROS_INFO("Wait done."); + RCLCPP_ERROR_STREAM(this->get_logger(), "Error loading rnav plugins: " << errorMsgs); } + RCLCPP_INFO_STREAM(this->get_logger(), "Pluginns loaded OK."); + } +} - // Init ROS publishers: - // ----------------------- - m_pub_cmd_vel = m_nh.advertise("cmd_vel", 1); - - // Init ROS subs: - // ----------------------- - // "/reactive_nav_goal", "/move_base_simple/goal" ( - // geometry_msgs/PoseStamped ) - m_sub_nav_goal = m_nh.subscribe( - m_pub_topic_reactive_nav_goal, 1, - &ReactiveNav2DNode::onRosGoalReceived, this); - m_sub_local_obs = m_nh.subscribe( - m_sub_topic_local_obstacles, 1, - &ReactiveNav2DNode::onRosLocalObstacles, this); - - // Init timers: - // ---------------------------------------------------- - m_timer_run_nav = m_nh.createTimer( - ros::Duration(m_nav_period), &ReactiveNav2DNode::onDoNavigation, - this); - - } // end ctor - - /** - * @brief Issue a navigation command - * @param target The target location - */ - void navigateTo(const mrpt::math::TPose2D& target) - { - ROS_INFO( - "[navigateTo] Starting navigation to %s", - target.asString().c_str()); - - CAbstractPTGBasedReactive::TNavigationParamsPTG navParams; +/** + * @brief Issue a navigation command + * @param target The target location + */ +void ReactiveNav2DNode::navigate_to(const mrpt::math::TPose2D& target) +{ + RCLCPP_INFO(this->get_logger(), + "[navigateTo] Starting navigation to %s", + target.asString().c_str()); - CAbstractNavigator::TargetInfo target_info; - target_info.target_coords.x = target.x; - target_info.target_coords.y = target.y; - target_info.targetAllowedDistance = m_target_allowed_distance; - target_info.targetIsRelative = false; + CAbstractPTGBasedReactive::TNavigationParamsPTG navParams; - // API for multiple waypoints: - //... - // navParams.multiple_targets.push_back(target_info); + CAbstractNavigator::TargetInfo target_info; + target_info.target_coords.x = target.x; + target_info.target_coords.y = target.y; + target_info.targetAllowedDistance = m_target_allowed_distance; + target_info.targetIsRelative = false; - // API for single targets: - navParams.target = target_info; + // API for single targets: + navParams.target = target_info; - // Optional: restrict the PTGs to use - // navParams.restrict_PTG_indices.push_back(1); + // Optional: restrict the PTGs to use + // navParams.restrict_PTG_indices.push_back(1); - { - std::lock_guard csl(m_reactive_nav_engine_cs); - m_reactive_nav_engine.navigate(&navParams); - } + { + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.navigate(&navParams); } +} - /** Callback: On run navigation */ - void onDoNavigation(const ros::TimerEvent&) +/** Callback: On run navigation */ +void ReactiveNav2DNode::on_do_navigation() +{ + // 1st time init: + // ---------------------------------------------------- + if (!m_1st_time_init) { - // 1st time init: - // ---------------------------------------------------- - if (!m_1st_time_init) + m_1st_time_init = true; + RCLCPP_INFO( this->get_logger(), + "[ReactiveNav2DNode] Initializing reactive navigation " + "engine..."); { - m_1st_time_init = true; - ROS_INFO( - "[ReactiveNav2DNode] Initializing reactive navigation " - "engine..."); - { - std::lock_guard csl(m_reactive_nav_engine_cs); - m_reactive_nav_engine.initialize(); - } - ROS_INFO( - "[ReactiveNav2DNode] Reactive navigation engine init done!"); + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.initialize(); } - - CTimeLoggerEntry tle(m_profiler, "onDoNavigation"); - - m_reactive_nav_engine.navigationStep(); + RCLCPP_INFO(this->get_logger(), + "[ReactiveNav2DNode] Reactive navigation engine init done!"); } - void onRosGoalReceived(const geometry_msgs::PoseStampedConstPtr& trg_ptr) - { - geometry_msgs::PoseStamped trg = *trg_ptr; + CTimeLoggerEntry tle(m_profiler, "on_do_navigation"); + // Main nav loop (in whatever state nav is: IDLE, NAVIGATING, etc.) + m_reactive_nav_engine.navigationStep(); +} - ROS_INFO( - "Nav target received via topic sub: (%.03f,%.03f, %.03fdeg) " - "[frame_id=%s]", - trg.pose.position.x, trg.pose.position.y, - trg.pose.orientation.z * 180.0 / M_PI, trg.header.frame_id.c_str()); +void ReactiveNav2DNode::on_odometry_received(const nav_msgs::msg::Odometry::SharedPtr& msg) +{ + std::lock_guard csl(m_odometry_cs); + tf2::Quaternion quat( + msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + m_odometry.odometry = mrpt::poses::CPose2D( + msg->pose.pose.position.x, msg->pose.pose.position.y, yaw); + + m_odometry.velocityLocal.vx = msg->twist.twist.linear.x; + m_odometry.velocityLocal.vy = msg->twist.twist.linear.y; + m_odometry.velocityLocal.omega = msg->twist.twist.angular.z; + m_odometry.hasVelocities = true; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Odometry updated"); +} - // Convert to the "m_frameid_reference" frame of coordinates: - if (trg.header.frame_id != m_frameid_reference) - { - ros::Duration timeout(0.2); - try - { - geometry_msgs::TransformStamped ref_to_trgFrame = - m_tf_buffer.lookupTransform( - trg.header.frame_id, m_frameid_reference, ros::Time(0), - timeout); +void ReactiveNav2DNode::on_waypoint_seq_received(const mrpt_msgs::msg::WaypointSequence::SharedPtr& wps) +{ + update_waypoint_sequence(std::move(wps)); +} - tf2::doTransform(trg, trg, ref_to_trgFrame); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR("%s", ex.what()); - return; - } - } +void ReactiveNav2DNode::update_waypoint_sequence(const mrpt_msgs::msg::WaypointSequence::SharedPtr& msg) +{ + mrpt::nav::TWaypointSequence wps; - this->navigateTo(mrpt::math::TPose2D( - trg.pose.position.x, trg.pose.position.y, trg.pose.orientation.z)); + for (const auto& wp : msg->waypoints) + { + tf2::Quaternion quat( + wp.target.orientation.x, wp.target.orientation.y, + wp.target.orientation.z, wp.target.orientation.w); + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + auto waypoint = mrpt::nav::TWaypoint( + wp.target.position.x, wp.target.position.y, wp.allowed_distance, + wp.allow_skip); + + if (yaw == yaw && !wp.ignore_heading) // regular number, not NAN + waypoint.target_heading = yaw; + + wps.waypoints.push_back(waypoint); } - void onRosLocalObstacles(const sensor_msgs::PointCloudConstPtr& obs) + RCLCPP_INFO_STREAM(this->get_logger(), "New navigateWaypoints() command"); { - std::lock_guard csl(m_last_obstacles_cs); - mrpt::ros1bridge::fromROS(*obs, m_last_obstacles); - // ROS_DEBUG("Local obstacles received: %u points", static_cast(m_last_obstacles.size()) ); + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.navigateWaypoints(wps); } +} - void onRosSetRobotShape(const geometry_msgs::PolygonConstPtr& newShape) - { - ROS_INFO_STREAM( - "[onRosSetRobotShape] Robot shape received via topic: " - << *newShape); +void ReactiveNav2DNode::on_goal_received(const geometry_msgs::msg::PoseStamped::SharedPtr& trg_ptr) +{ + geometry_msgs::msg::PoseStamped trg = *trg_ptr; + + RCLCPP_INFO(this->get_logger(), + "Nav target received via topic sub: (%.03f,%.03f, %.03fdeg) " + "[frame_id=%s]", + trg.pose.position.x, trg.pose.position.y, + trg.pose.orientation.z * 180.0 / M_PI, trg.header.frame_id.c_str()); - mrpt::math::CPolygon poly; - poly.resize(newShape->points.size()); - for (size_t i = 0; i < newShape->points.size(); i++) + // Convert to the "m_frameid_reference" frame of coordinates: + if (trg.header.frame_id != m_frameid_reference) + { + //rclcpp::Duration timeout(0.2); + rclcpp::Duration timeout(std::chrono::milliseconds(200)); + try { - poly[i].x = newShape->points[i].x; - poly[i].y = newShape->points[i].y; - } + geometry_msgs::msg::TransformStamped ref_to_trgFrame = + m_tf_buffer->lookupTransform( + trg.header.frame_id, m_frameid_reference, + tf2::TimePointZero, tf2::durationFromSec(timeout.seconds())); + tf2::doTransform(trg, trg, ref_to_trgFrame); + } + catch (const tf2::TransformException& ex) { - std::lock_guard csl(m_reactive_nav_engine_cs); - m_reactive_nav_engine.changeRobotShape(poly); + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; } } -}; // end class + this->navigate_to(mrpt::math::TPose2D( + trg.pose.position.x, trg.pose.position.y, trg.pose.orientation.z)); +} + +void ReactiveNav2DNode::on_local_obstacles(const sensor_msgs::msg::PointCloud2::SharedPtr& obs) +{ + std::lock_guard csl(m_last_obstacles_cs); + mrpt::ros2bridge::fromROS(*obs, m_last_obstacles); + RCLCPP_DEBUG(this->get_logger(), + "Local obstacles received: %u points", static_cast(m_last_obstacles.size())); +} -int main(int argc, char** argv) +void ReactiveNav2DNode::on_set_robot_shape(const geometry_msgs::msg::Polygon::SharedPtr& newShape) { - ReactiveNav2DNode the_node(argc, argv); - ros::spin(); - return 0; + RCLCPP_INFO_STREAM(this->get_logger(), "[onSetRobotShape] Robot shape received via topic:"); + for (const auto& point : newShape->points) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Point - x: " << point.x << ", y: " << point.y << ", z: " << point.z); + } + + mrpt::math::CPolygon poly; + poly.resize(newShape->points.size()); + for (size_t i = 0; i < newShape->points.size(); i++) + { + poly[i].x = newShape->points[i].x; + poly[i].y = newShape->points[i].y; + } + + { + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.changeRobotShape(poly); + } } + + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file