From a81d779614288afd29e9fbd9f9b2c1d08318abfa Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 29 Dec 2024 22:45:54 +0100 Subject: [PATCH] ROS-related code moved out to its own repository. See: https://github.com/MRPT/mrpt_ros_bridge --- CMakeLists.txt | 10 - apps/rosbag2rawlog/CMakeLists.txt | 47 - apps/rosbag2rawlog/rosbag2rawlog_main.cpp | 837 ------------------ cmakemodules/script_ros.cmake | 167 ---- cmakemodules/script_show_final_summary.cmake | 6 - libs/ros1bridge/CMakeLists.txt | 58 -- libs/ros1bridge/include/mrpt/ros1bridge/gps.h | 52 -- .../include/mrpt/ros1bridge/image.h | 36 - libs/ros1bridge/include/mrpt/ros1bridge/imu.h | 53 -- .../include/mrpt/ros1bridge/laser_scan.h | 54 -- .../include/mrpt/ros1bridge/logging.h | 100 --- libs/ros1bridge/include/mrpt/ros1bridge/map.h | 120 --- .../include/mrpt/ros1bridge/point_cloud.h | 49 - .../include/mrpt/ros1bridge/point_cloud2.h | 99 --- .../ros1bridge/include/mrpt/ros1bridge/pose.h | 60 -- .../include/mrpt/ros1bridge/range.h | 50 -- .../include/mrpt/ros1bridge/stereo_image.h | 36 - .../ros1bridge/include/mrpt/ros1bridge/time.h | 36 - libs/ros1bridge/src/gps.cpp | 149 ---- libs/ros1bridge/src/image.cpp | 69 -- libs/ros1bridge/src/imu.cpp | 108 --- libs/ros1bridge/src/laser_scan.cpp | 96 -- libs/ros1bridge/src/map.cpp | 219 ----- libs/ros1bridge/src/map_unittest.cpp | 99 --- libs/ros1bridge/src/point_cloud.cpp | 45 - libs/ros1bridge/src/point_cloud2.cpp | 602 ------------- libs/ros1bridge/src/pointcloud2_unittest.cpp | 103 --- libs/ros1bridge/src/pose.cpp | 262 ------ libs/ros1bridge/src/pose_unittest.cpp | 161 ---- libs/ros1bridge/src/range.cpp | 69 -- libs/ros1bridge/src/stereo_image.cpp | 85 -- libs/ros1bridge/src/time.cpp | 29 - libs/ros1bridge/src/time_unittest.cpp | 28 - libs/ros2bridge/CMakeLists.txt | 70 -- libs/ros2bridge/include/mrpt/ros2bridge/gps.h | 53 -- .../include/mrpt/ros2bridge/image.h | 36 - libs/ros2bridge/include/mrpt/ros2bridge/imu.h | 53 -- .../include/mrpt/ros2bridge/laser_scan.h | 53 -- libs/ros2bridge/include/mrpt/ros2bridge/map.h | 120 --- .../include/mrpt/ros2bridge/point_cloud.h | 50 -- .../include/mrpt/ros2bridge/point_cloud2.h | 105 --- .../ros2bridge/include/mrpt/ros2bridge/pose.h | 60 -- .../include/mrpt/ros2bridge/range.h | 51 -- .../include/mrpt/ros2bridge/stereo_image.h | 37 - .../ros2bridge/include/mrpt/ros2bridge/time.h | 37 - libs/ros2bridge/src/gps.cpp | 150 ---- libs/ros2bridge/src/image.cpp | 69 -- libs/ros2bridge/src/imu.cpp | 108 --- libs/ros2bridge/src/laser_scan.cpp | 97 -- libs/ros2bridge/src/map.cpp | 219 ----- libs/ros2bridge/src/map_unittest.cpp | 99 --- libs/ros2bridge/src/point_cloud.cpp | 46 - libs/ros2bridge/src/point_cloud2.cpp | 617 ------------- libs/ros2bridge/src/pointcloud2_unittest.cpp | 104 --- libs/ros2bridge/src/pose.cpp | 271 ------ libs/ros2bridge/src/pose_unittest.cpp | 161 ---- libs/ros2bridge/src/range.cpp | 108 --- libs/ros2bridge/src/stereo_image.cpp | 85 -- libs/ros2bridge/src/time.cpp | 30 - libs/ros2bridge/src/time_unittest.cpp | 28 - parse-files/config.h.in | 2 - 61 files changed, 6913 deletions(-) delete mode 100644 apps/rosbag2rawlog/CMakeLists.txt delete mode 100644 apps/rosbag2rawlog/rosbag2rawlog_main.cpp delete mode 100644 cmakemodules/script_ros.cmake delete mode 100644 libs/ros1bridge/CMakeLists.txt delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/gps.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/image.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/imu.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/laser_scan.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/logging.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/map.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/point_cloud.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/pose.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/range.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/stereo_image.h delete mode 100644 libs/ros1bridge/include/mrpt/ros1bridge/time.h delete mode 100644 libs/ros1bridge/src/gps.cpp delete mode 100644 libs/ros1bridge/src/image.cpp delete mode 100644 libs/ros1bridge/src/imu.cpp delete mode 100644 libs/ros1bridge/src/laser_scan.cpp delete mode 100644 libs/ros1bridge/src/map.cpp delete mode 100644 libs/ros1bridge/src/map_unittest.cpp delete mode 100644 libs/ros1bridge/src/point_cloud.cpp delete mode 100644 libs/ros1bridge/src/point_cloud2.cpp delete mode 100644 libs/ros1bridge/src/pointcloud2_unittest.cpp delete mode 100644 libs/ros1bridge/src/pose.cpp delete mode 100644 libs/ros1bridge/src/pose_unittest.cpp delete mode 100644 libs/ros1bridge/src/range.cpp delete mode 100644 libs/ros1bridge/src/stereo_image.cpp delete mode 100644 libs/ros1bridge/src/time.cpp delete mode 100644 libs/ros1bridge/src/time_unittest.cpp delete mode 100644 libs/ros2bridge/CMakeLists.txt delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/gps.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/image.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/imu.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/laser_scan.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/map.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/point_cloud.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/pose.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/range.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/stereo_image.h delete mode 100644 libs/ros2bridge/include/mrpt/ros2bridge/time.h delete mode 100644 libs/ros2bridge/src/gps.cpp delete mode 100644 libs/ros2bridge/src/image.cpp delete mode 100644 libs/ros2bridge/src/imu.cpp delete mode 100644 libs/ros2bridge/src/laser_scan.cpp delete mode 100644 libs/ros2bridge/src/map.cpp delete mode 100644 libs/ros2bridge/src/map_unittest.cpp delete mode 100644 libs/ros2bridge/src/point_cloud.cpp delete mode 100644 libs/ros2bridge/src/point_cloud2.cpp delete mode 100644 libs/ros2bridge/src/pointcloud2_unittest.cpp delete mode 100644 libs/ros2bridge/src/pose.cpp delete mode 100644 libs/ros2bridge/src/pose_unittest.cpp delete mode 100644 libs/ros2bridge/src/range.cpp delete mode 100644 libs/ros2bridge/src/stereo_image.cpp delete mode 100644 libs/ros2bridge/src/time.cpp delete mode 100644 libs/ros2bridge/src/time_unittest.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 44b88ddf66..f2dabdc0f0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -259,16 +259,6 @@ endif() include(cmakemodules/script_gl_glut.cmake REQUIRED) # Check for the GL,GLUT libraries include(cmakemodules/script_jsoncpp.cmake REQUIRED) # Check for jsoncpp -# ---------------------------------------------------------------------------- -# ROS -# ---------------------------------------------------------------------------- -include(cmakemodules/script_ros.cmake REQUIRED) # Check for ROS1 / ROS2 - -# Abort from ROS scripts? (special case for the build farm) -if (MRPT_ABORT_CMAKE_SCRIPT) - return() -endif() - # ---------------------------------------------------------------------------- # Other sub-scripts: # ---------------------------------------------------------------------------- diff --git a/apps/rosbag2rawlog/CMakeLists.txt b/apps/rosbag2rawlog/CMakeLists.txt deleted file mode 100644 index 1d31182576..0000000000 --- a/apps/rosbag2rawlog/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ - -if(NOT TARGET ros1bridge) - # Try to find it as an external project (needed for mrpt_ros package) - find_package(ros1bridge QUIET) - if(NOT TARGET ros1bridge) - return() - endif() -endif() - -project(rosbag2rawlog) - -# ================================================ -# TARGET: rosbag2rawlog -# ================================================ -# Define the executable target: -add_executable(${PROJECT_NAME} - # Main: - rosbag2rawlog_main.cpp - ${MRPT_VERSION_RC_FILE} - ) - -# Add the required libraries for linking: -target_link_libraries(${PROJECT_NAME} - mrpt::ros1bridge - ${rosbag_storage_LIBRARIES} - ${cv_bridge_LIBRARIES} - ${tf2_LIBRARIES} -) - -target_include_directories(${PROJECT_NAME} - SYSTEM - PRIVATE - ${roscpp_INCLUDE_DIRS} - ${rosbag_storage_INCLUDE_DIRS} - ${cv_bridge_INCLUDE_DIRS} - ${tf2_INCLUDE_DIRS} -) - -target_compile_definitions(${PROJECT_NAME} - PRIVATE - ${ROS_DEFINITIONS} -) - -DeclareAppForInstall(${PROJECT_NAME}) -# Dependencies on MRPT libraries: Just mention the top-level dependency, the rest will be detected automatically, -# and all the needed #include<> dirs added (see the script DeclareAppDependencies.cmake for further details) -DeclareAppDependencies(${PROJECT_NAME} mrpt::system mrpt::obs mrpt::io mrpt::tclap mrpt::serialization) diff --git a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp deleted file mode 100644 index dde641b4ee..0000000000 --- a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp +++ /dev/null @@ -1,837 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | http://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, 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 | - +------------------------------------------------------------------------+ */ - -// =========================================================================== -// Program: rosbag2rawlog -// Intention: Parse bag files, save -// as a RawLog file, easily readable by MRPT C++ programs. -// -// Started: Hunter Laux @ SEPT-2018. -// Maintained: JLBC @ 2018-2024 -// =========================================================================== - -#include // this header is obsolete in ros2-I but as long as this app is only built for ros1 we are ok -#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 // rosbag_storage C++ lib -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // needed by tf2::fromMsg() -#include - -#include - -using namespace mrpt; -using namespace mrpt::io; -using namespace mrpt::serialization; -using namespace mrpt::system; -using namespace std; - -// Declare the supported command line switches =========== -TCLAP::CmdLine cmd("rosbag2rawlog (ROS 1)", ' ', MRPT_getVersion().c_str()); - -TCLAP::UnlabeledMultiArg arg_input_files( - "bags", "Input bag files (required) (*.bag)", true, "log.bag", cmd); - -TCLAP::ValueArg arg_output_file( - "o", "output", "Output dataset (*.rawlog)", true, "", "dataset_out.rawlog", cmd); - -TCLAP::ValueArg arg_config_file( - "c", "config", "Config yaml file (*.yml)", true, "", "config.yml", cmd); - -TCLAP::SwitchArg arg_overwrite( - "w", "overwrite", "Force overwrite target file without prompting.", cmd, false); - -TCLAP::ValueArg arg_base_link_frame( - "b", - "base-link", - "Reference /tf frame for the robot frame (Default: 'base_link')", - false, - "base_link", - "base_link", - cmd); - -std::optional odom_from_tf_label; -std::string odom_frame_id = "odom"; - -using Obs = std::list; - -using CallbackFunction = std::function; - -template -class RosSynchronizer : public std::enable_shared_from_this> -{ - public: - using Tuple = std::tuple...>; - - using Callback = std::function&...)>; - - RosSynchronizer(std::shared_ptr tfBuffer, const Callback& callback) : - m_tfBuffer(std::move(tfBuffer)), m_callback(callback) - { - } - - template - Obs signal(std::index_sequence) - { - auto ptr = m_callback(std::get(m_cache)...); - m_cache = {}; - return ptr; - } - - Obs signal() { return {}; } - - template - bool check(std::index_sequence) - { - return (std::get(m_cache) && ...); - } - - Obs checkAndSignal() - { - if (check(std::make_index_sequence{})) - { - return signal(); - } - return {}; - } - - template - CallbackFunction bind() - { - std::shared_ptr ptr = this->shared_from_this(); - return [=](const rosbag::MessageInstance& rosmsg) - { - if (!std::get(ptr->m_cache)) - { - std::get(ptr->m_cache) = - rosmsg.instantiate::type::element_type>(); - return ptr->checkAndSignal(); - } - return Obs(); - }; - } - - CallbackFunction bindTfSync() - { - std::shared_ptr ptr = this->shared_from_this(); - return [=](const rosbag::MessageInstance& rosmsg) { return ptr->checkAndSignal(); }; - } - - private: - std::shared_ptr m_tfBuffer; - Tuple m_cache; - bool m_poseValid = false; - mrpt::poses::CPose3D m_lastPose; - Callback m_callback; -}; - -std::shared_ptr tfBuffer; - -std::set known_tf_frames; - -void removeTrailingSlash(std::string& s) -{ - ASSERT_(!s.empty()); - if (s.at(0) == '/') s = s.substr(1); -} - -void addTfFrameAsKnown(std::string s) -{ - removeTrailingSlash(s); - known_tf_frames.insert(s); -} - -bool findOutSensorPose( - mrpt::poses::CPose3D& des, - std::string referenceFrame, - std::string frame, - const std::optional& fixedSensorPose) -{ - if (fixedSensorPose) - { - des = fixedSensorPose.value(); - return true; - } - - // TF1 old frames started with "/foo", forbidden in TF2. - // Handle this since this program is for importing old ROS1 bags: - removeTrailingSlash(referenceFrame); - removeTrailingSlash(frame); - - try - { - ASSERT_(tfBuffer); - - geometry_msgs::TransformStamped ref_to_trgFrame = - tfBuffer->lookupTransform(frame, referenceFrame, {} /*latest value*/); - - tf2::Transform tf; - tf2::fromMsg(ref_to_trgFrame.transform, tf); - des = mrpt::ros1bridge::fromROS(tf); - -#if 0 - std::cout << mrpt::format( - "[findOutSensorPose] Found pose %s -> %s: %s\n", - referenceFrame.c_str(), frame.c_str(), des.asString().c_str()); -#endif - - return true; - } - catch (const tf2::TransformException& ex) - { - std::cerr << "findOutSensorPose: " << ex.what() << std::endl << "\nKnown TF frames: "; - for (const auto& f : known_tf_frames) std::cerr << "'" << f << "',"; - std::cerr << std::endl; - - return false; - } -} - -Obs toPointCloud2( - std::string_view msg, - const rosbag::MessageInstance& rosmsg, - const std::optional& fixedSensorPose) -{ - auto pts = rosmsg.instantiate(); - - auto ptsObs = mrpt::obs::CObservationPointCloud::Create(); - ptsObs->sensorLabel = msg; - ptsObs->timestamp = mrpt::ros1bridge::fromROS(pts->header.stamp); - - bool sensorPoseOK = findOutSensorPose( - ptsObs->sensorPose, pts->header.frame_id, arg_base_link_frame.getValue(), fixedSensorPose); - if (!sensorPoseOK) - { - std::cerr << "Warning: dropping one observation of type '" << msg - << "' due to missing /tf data.\n"; - return {}; - } - - // Convert points: - std::set fields = mrpt::ros1bridge::extractFields(*pts); - - // We need X Y Z: - if (!fields.count("x") || !fields.count("y") || !fields.count("z")) return {}; - - if (fields.count("ring") || fields.count("time")) - { - // XYZIRT - auto mrptPts = mrpt::maps::CPointsMapXYZIRT::Create(); - ptsObs->pointcloud = mrptPts; - - if (!mrpt::ros1bridge::fromROS(*pts, *mrptPts)) - { - THROW_EXCEPTION("Could not convert pointcloud from ROS to CPointsMapXYZIRT"); - } - else - { // converted ok: - return {ptsObs}; - } - } - - if (fields.count("intensity")) - { - // XYZI - auto mrptPts = mrpt::maps::CPointsMapXYZI::Create(); - ptsObs->pointcloud = mrptPts; - - if (!mrpt::ros1bridge::fromROS(*pts, *mrptPts)) - { - thread_local bool warn1st = false; - if (!warn1st) - { - warn1st = true; - std::cerr << "Could not convert pointcloud from ROS to " - "CPointsMapXYZI. Trying with XYZ.\n"; - } - } - else - { // converted ok: - return {ptsObs}; - } - } - - { - // XYZ - auto mrptPts = mrpt::maps::CSimplePointsMap::Create(); - ptsObs->pointcloud = mrptPts; - if (!mrpt::ros1bridge::fromROS(*pts, *mrptPts)) - THROW_EXCEPTION( - "Could not convert pointcloud from ROS to " - "CSimplePointsMap"); - } - - return {ptsObs}; -} - -Obs toLidar2D( - std::string_view msg, - const rosbag::MessageInstance& rosmsg, - const std::optional& fixedSensorPose) -{ - auto scan = rosmsg.instantiate(); - - auto scanObs = mrpt::obs::CObservation2DRangeScan::Create(); - - // Extract sensor pose from tf frames, if enabled: - mrpt::poses::CPose3D sensorPose; - mrpt::ros1bridge::fromROS(*scan, sensorPose, *scanObs); - - scanObs->sensorLabel = msg; - scanObs->timestamp = mrpt::ros1bridge::fromROS(scan->header.stamp); - - bool sensorPoseOK = findOutSensorPose( - scanObs->sensorPose, scan->header.frame_id, arg_base_link_frame.getValue(), fixedSensorPose); - if (!sensorPoseOK) - { - std::cerr << "Warning: dropping one observation of type '" << msg - << "' due to missing /tf data.\n"; - return {}; - } - - return {scanObs}; -} - -Obs toRotatingScan( - std::string_view msg, - const rosbag::MessageInstance& rosmsg, - const std::optional& fixedSensorPose) -{ - auto pts = rosmsg.instantiate(); - - // Convert points: - std::set fields = mrpt::ros1bridge::extractFields(*pts); - - // We need X Y Z: - if (!fields.count("x") || !fields.count("y") || !fields.count("z") || !fields.count("ring")) - return {}; - - // As a structured 2D range images, if we have ring numbers: - auto obsRotScan = mrpt::obs::CObservationRotatingScan::Create(); - const mrpt::poses::CPose3D sensorPose; - - if (!mrpt::ros1bridge::fromROS(*pts, *obsRotScan, sensorPose)) - { - THROW_EXCEPTION( - "Could not convert pointcloud from ROS to " - "CObservationRotatingScan. Trying another format."); - } - - obsRotScan->sensorLabel = msg; - obsRotScan->timestamp = mrpt::ros1bridge::fromROS(pts->header.stamp); - - bool sensorPoseOK = findOutSensorPose( - obsRotScan->sensorPose, pts->header.frame_id, arg_base_link_frame.getValue(), - fixedSensorPose); - if (!sensorPoseOK) - { - std::cerr << "Warning: dropping one observation of type '" << msg - << "' due to missing /tf data.\n"; - return {}; - } - - return {obsRotScan}; -} - -Obs toIMU( - std::string_view msg, - const rosbag::MessageInstance& rosmsg, - const std::optional& fixedSensorPose) -{ - auto imu = rosmsg.instantiate(); - - auto mrptObs = mrpt::obs::CObservationIMU::Create(); - - mrptObs->sensorLabel = msg; - mrptObs->timestamp = mrpt::ros1bridge::fromROS(imu->header.stamp); - - // Convert data: - mrpt::ros1bridge::fromROS(*imu, *mrptObs); - - bool sensorPoseOK = findOutSensorPose( - mrptObs->sensorPose, imu->header.frame_id, arg_base_link_frame.getValue(), fixedSensorPose); - if (!sensorPoseOK) - { - std::cerr << "Warning: dropping one observation of type '" << msg - << "' due to missing /tf data.\n"; - return {}; - } - - return {mrptObs}; -} - -Obs toGPS( - std::string_view msg, - const rosbag::MessageInstance& rosmsg, - const std::optional& fixedSensorPose) -{ - auto gps = rosmsg.instantiate(); - - auto mrptObs = mrpt::obs::CObservationGPS::Create(); - - mrptObs->sensorLabel = msg; - mrptObs->timestamp = mrpt::ros1bridge::fromROS(gps->header.stamp); - - // Convert data: - mrpt::ros1bridge::fromROS(*gps, *mrptObs); - - bool sensorPoseOK = findOutSensorPose( - mrptObs->sensorPose, gps->header.frame_id, arg_base_link_frame.getValue(), fixedSensorPose); - if (!sensorPoseOK) - { - std::cerr << "Warning: dropping one observation of type '" << msg - << "' due to missing /tf data.\n"; - return {}; - } - - return {mrptObs}; -} - -Obs toOdometry(std::string_view msg, const rosbag::MessageInstance& rosmsg) -{ - auto odo = rosmsg.instantiate(); - - auto mrptObs = mrpt::obs::CObservationOdometry::Create(); - - mrptObs->sensorLabel = msg; - mrptObs->timestamp = mrpt::ros1bridge::fromROS(odo->header.stamp); - - // Convert data: - const auto pose = mrpt::ros1bridge::fromROS(odo->pose); - mrptObs->odometry = {pose.mean.x(), pose.mean.y(), pose.mean.yaw()}; - - mrptObs->hasVelocities = true; - mrptObs->velocityLocal.vx = odo->twist.twist.linear.x; - mrptObs->velocityLocal.vy = odo->twist.twist.linear.y; - mrptObs->velocityLocal.omega = odo->twist.twist.angular.z; - - return {mrptObs}; -} - -Obs toImage( - std::string_view msg, - const rosbag::MessageInstance& rosmsg, - const std::optional& fixedSensorPose) -{ - cv_bridge::CvImagePtr cv_ptr; - mrpt::Clock::time_point timeStamp; - std::string frame_id; - - if (auto image = rosmsg.instantiate(); image) - { - cv_ptr = cv_bridge::toCvCopy(image); - timeStamp = mrpt::ros1bridge::fromROS(image->header.stamp); - frame_id = image->header.frame_id; - } - else if (auto imC = rosmsg.instantiate(); imC) - { - cv_ptr = cv_bridge::toCvCopy(imC); - timeStamp = mrpt::ros1bridge::fromROS(imC->header.stamp); - frame_id = imC->header.frame_id; - } - else - { - THROW_EXCEPTION_FMT( - "Unhandled ROS topic '%s' type: images are expected to be either " - "sensor_msgs::Image or sensor_msgs::CompressedImage", - std::string(msg).c_str()); - } - - auto imgObs = mrpt::obs::CObservationImage::Create(); - - imgObs->sensorLabel = msg; - imgObs->timestamp = timeStamp; - - imgObs->image = mrpt::img::CImage(cv_ptr->image, mrpt::img::SHALLOW_COPY); - - bool sensorPoseOK = findOutSensorPose( - imgObs->cameraPose, frame_id, arg_base_link_frame.getValue(), fixedSensorPose); - if (!sensorPoseOK) - { - std::cerr << "Warning: dropping one observation of type '" << msg - << "' due to missing /tf data.\n"; - return {}; - } - - return {imgObs}; -} - -Obs toRangeImage( - std::string_view msg, - const sensor_msgs::Image::Ptr& image, - const sensor_msgs::CameraInfo::Ptr& cameraInfo, - bool rangeIsDepth, - const std::optional& fixedSensorPose) -{ - auto cv_ptr = cv_bridge::toCvShare(image); - - // For now we are just assuming this is a range image - if (cv_ptr->encoding == "32FC1") - { - auto rangeScan = mrpt::obs::CObservation3DRangeScan::Create(); - - rangeScan->sensorLabel = msg; - rangeScan->timestamp = mrpt::ros1bridge::fromROS(image->header.stamp); - - rangeScan->hasRangeImage = true; - rangeScan->rangeImage_setSize(cv_ptr->image.rows, cv_ptr->image.cols); - - rangeScan->cameraParams.nrows = cv_ptr->image.rows; - rangeScan->cameraParams.ncols = cv_ptr->image.cols; - - std::copy(cameraInfo->D.begin(), cameraInfo->D.end(), rangeScan->cameraParams.dist.begin()); - - size_t rows = cv_ptr->image.rows; - size_t cols = cv_ptr->image.cols; - std::copy( - cameraInfo->K.begin(), cameraInfo->K.end(), - rangeScan->cameraParams.intrinsicParams.begin()); - - rangeScan->rangeUnits = 1e-3; - const float inv_unit = 1.0f / rangeScan->rangeUnits; - - for (size_t i = 0; i < rows; i++) - for (size_t j = 0; j < cols; j++) - rangeScan->rangeImage(i, j) = - static_cast(inv_unit * cv_ptr->image.at(i, j)); - - rangeScan->range_is_depth = rangeIsDepth; - - bool sensorPoseOK = findOutSensorPose( - rangeScan->sensorPose, image->header.frame_id, arg_base_link_frame.getValue(), - fixedSensorPose); - if (!sensorPoseOK) - { - std::cerr << "Warning: dropping one observation of type '" << msg - << "' due to missing /tf data.\n"; - return {}; - } - -#if 0 // JLBC: not needed anymore? - // MRPT assumes the image plane is parallel to the YZ plane, so the - // camera is pointed in the X direction ROS assumes the image plane - // is parallel to XY plane, so the camera is pointed in the Z - // direction Apply a rotation to convert between these conventions. - mrpt::math::CQuaternion rot{0.5, 0.5, -0.5, 0.5}; - mrpt::poses::CPose3DQuat poseQuat(0, 0, 0, rot); - mrpt::poses::CPose3D pose(poseQuat); - rangeScan->setSensorPose(pose); -#endif - - return {rangeScan}; - } - return {}; -} - -template -Obs toTf(tf2::BufferCore& tfBuf, const rosbag::MessageInstance& rosmsg) -{ - if (rosmsg.getDataType() != "tf2_msgs/TFMessage") return {}; - - Obs ret; - auto tfs = rosmsg.instantiate(); - for (auto& tf : tfs->transforms) - { - try - { - tfBuf.setTransform(tf, "bagfile", isStatic); - - addTfFrameAsKnown(tf.child_frame_id); - addTfFrameAsKnown(tf.header.frame_id); -#if 0 - std::cout << "tf: " << tf.child_frame_id - << " <= " << tf.header.frame_id << "\n"; -#endif - - // Process /tf -> odometry conversion, if enabled: - const auto baseLink = arg_base_link_frame.getValue(); - - if (odom_from_tf_label && - (tf.child_frame_id == odom_frame_id || tf.header.frame_id == odom_frame_id) && - (tf.child_frame_id == baseLink || tf.header.frame_id == baseLink)) - { - mrpt::poses::CPose3D p; - bool valid = - findOutSensorPose(p, odom_frame_id, arg_base_link_frame.getValue(), std::nullopt); - if (valid) - { - auto o = mrpt::obs::CObservationOdometry::Create(); - o->sensorLabel = odom_from_tf_label.value(); - o->timestamp = mrpt::ros1bridge::fromROS(tf.header.stamp); - - // Convert data: - o->odometry = {p.x(), p.y(), p.yaw()}; - o->hasVelocities = false; - ret.push_back(o); - } - } - } - catch (const tf2::TransformException& ex) - { - std::cerr << ex.what() << std::endl; - } - } - return ret; -} - -class Transcriber -{ - public: - Transcriber(const mrpt::containers::yaml& config) - { - tfBuffer = std::make_shared(); - - if (config.has("odom_from_tf")) - { - ASSERT_(config["odom_from_tf"].isMap()); - ASSERTMSG_( - config["odom_from_tf"].has("sensor_label"), - "odom_from_tf YAML map must contain a sensor_label entry."); - - const auto c = config["odom_from_tf"]; - odom_from_tf_label = c["sensor_label"].as(); - if (c.has("odom_frame_id")) odom_frame_id = c["odom_frame_id"].as(); - } - - m_lookup["/tf"].emplace_back([=](const rosbag::MessageInstance& rosmsg) - { return toTf(*tfBuffer, rosmsg); }); - m_lookup["/tf_static"].emplace_back([=](const rosbag::MessageInstance& rosmsg) - { return toTf(*tfBuffer, rosmsg); }); - - for (auto& sensorNode : config["sensors"].asMapRange()) - { - auto sensorName = sensorNode.first.as(); - auto& sensor = sensorNode.second.asMap(); - const auto sensorType = sensor.at("type").as(); - - // Optional: fixed sensorPose (then ignores/don't need "tf" data): - std::optional fixedSensorPose; - if (sensor.count("fixed_sensor_pose") != 0) - { - fixedSensorPose = mrpt::poses::CPose3D::FromString( - "["s + sensor.at("fixed_sensor_pose").as() + "]"s); - } - -#if 0 - if (sensorType == "CObservation3DRangeScan") - { - bool rangeIsDepth = sensor.count("rangeIsDepth") - ? sensor.at("rangeIsDepth").as() - : true; - auto callback = [=](const sensor_msgs::Image::Ptr& image, - const sensor_msgs::CameraInfo::Ptr& info) { - return toRangeImage( - sensorName, image, info, rangeIsDepth, fixedSensorPose); - }; - using Synchronizer = RosSynchronizer< - sensor_msgs::Image, sensor_msgs::CameraInfo>; - auto sync = std::make_shared(tfBuffer, callback); - m_lookup[sensor.at("depth").as()].emplace_back( - sync->bind<0>()); - m_lookup[sensor.at("cameraInfo").as()] - .emplace_back(sync->bind<1>()); - m_lookup["/tf"].emplace_back(sync->bindTfSync()); - } - else -#endif - if (sensorType == "CObservationImage") - { - auto callback = [=](const rosbag::MessageInstance& m) - { return toImage(sensorName, m, fixedSensorPose); }; - ASSERT_(sensor.count("image_topic") != 0); - m_lookup[sensor.at("image_topic").as()].emplace_back(callback); - } - else if (sensorType == "CObservationPointCloud") - { - auto callback = [=](const rosbag::MessageInstance& m) - { return toPointCloud2(sensorName, m, fixedSensorPose); }; - m_lookup[sensor.at("topic").as()].emplace_back(callback); - } - else if (sensorType == "CObservation2DRangeScan") - { - auto callback = [=](const rosbag::MessageInstance& m) - { return toLidar2D(sensorName, m, fixedSensorPose); }; - m_lookup[sensor.at("topic").as()].emplace_back(callback); - } - else if (sensorType == "CObservationRotatingScan") - { - auto callback = [=](const rosbag::MessageInstance& m) - { return toRotatingScan(sensorName, m, fixedSensorPose); }; - m_lookup[sensor.at("topic").as()].emplace_back(callback); - } - else if (sensorType == "CObservationIMU") - { - auto callback = [=](const rosbag::MessageInstance& m) - { return toIMU(sensorName, m, fixedSensorPose); }; - m_lookup[sensor.at("topic").as()].emplace_back(callback); - } - else if (sensorType == "CObservationGPS") - { - auto callback = [=](const rosbag::MessageInstance& m) - { return toGPS(sensorName, m, fixedSensorPose); }; - m_lookup[sensor.at("topic").as()].emplace_back(callback); - } - else if (sensorType == "CObservationOdometry") - { - auto callback = [=](const rosbag::MessageInstance& m) { return toOdometry(sensorName, m); }; - m_lookup[sensor.at("topic").as()].emplace_back(callback); - } - // TODO: Handle more cases? - } - } - - Obs toMrpt(const rosbag::MessageInstance& rosmsg) - { - Obs rets; - auto topic = rosmsg.getTopic(); - - if (auto search = m_lookup.find(topic); search != m_lookup.end()) - { - for (const auto& callback : search->second) - { - auto obs = callback(rosmsg); - rets.insert(rets.end(), obs.begin(), obs.end()); - } - } - else - { - if (m_unhandledTopics.count(topic) == 0) - { - m_unhandledTopics.insert(topic); - - std::cout << "Warning: unhandled topic '" << topic << "' [" << rosmsg.getDataType() << "]" - << std::endl; - } - } - return rets; - }; - - private: - std::map> m_lookup; - std::set m_unhandledTopics; -}; - -int main(int argc, char** argv) -{ - try - { - printf(" rosbag2rawlog (ROS 1) - Part of the MRPT\n"); - printf( - " MRPT C++ Library: %s - Sources timestamp: %s\n", MRPT_getVersion().c_str(), - MRPT_getCompilationDate().c_str()); - - // Parse arguments: - if (!cmd.parse(argc, argv)) throw std::runtime_error(""); // should exit. - - auto config = mrpt::containers::yaml::FromFile(arg_config_file.getValue()); - - auto input_bag_files = arg_input_files.getValue(); - string output_rawlog_file = arg_output_file.getValue(); - - // Open input ros bag: - std::vector> bags; - rosbag::View full_view; - for (auto& file : input_bag_files) - { - ASSERT_FILE_EXISTS_(file); - std::cout << "Opening: " << file << std::endl; - auto bag = make_shared(); - bag->open(file); - bags.push_back(bag); - full_view.addQuery(*bag); - } - - // Open output: - if (mrpt::system::fileExists(output_rawlog_file) && !arg_overwrite.isSet()) - { - cout << "Output file already exists: `" << output_rawlog_file - << "`, aborting. Use `-w` flag to overwrite.\n"; - return 1; - } - - CFileGZOutputStream fil_out; - cout << "Opening for writing: '" << output_rawlog_file << "'...\n"; - if (!fil_out.open(output_rawlog_file)) throw std::runtime_error("Error writing file!"); - - auto arch = archiveFrom(fil_out); - Transcriber t(config); - const auto nEntries = full_view.size(); - size_t curEntry = 0, showProgressCnt = 0; - for (const auto& m : full_view) - { - auto ptrs = t.toMrpt(m); - for (auto& ptr : ptrs) - { - arch << ptr; - } - - curEntry++; - - if (++showProgressCnt > 100) - { - const double pr = (1.0 * curEntry) / nEntries; - - printf( - "Progress: %u/%u %s %.03f%% \r", static_cast(curEntry), - static_cast(nEntries), mrpt::system::progress(pr, 50).c_str(), - 100.0 * pr); - fflush(stdout); - showProgressCnt = 0; - } - } - - printf("\n"); - - for (auto& bag : bags) - { - bag->close(); - } - - // successful end of program. - return 0; - } - catch (std::exception& e) - { - if (strlen(e.what())) std::cerr << e.what() << std::endl; - return 1; - } -} // end of main() diff --git a/cmakemodules/script_ros.cmake b/cmakemodules/script_ros.cmake deleted file mode 100644 index 26ec2502ca..0000000000 --- a/cmakemodules/script_ros.cmake +++ /dev/null @@ -1,167 +0,0 @@ -# Check for the ROS1 libraries -# ============================== - -option(DISABLE_ROS "Disable detection/usage of ROS libraries" "OFF") -mark_as_advanced(DISABLE_ROS) - -set(MRPT_ROS_VERSION ) # empty=none - -if (NOT DISABLE_ROS) - - # Save flag: - get_directory_property(FORMER_DEFS "COMPILE_DEFINITIONS") - if (NOT "${FORMER_DEFS}" STREQUAL "") - message(FATAL_ERROR "Expected: empty COMPILE_DEFINITIONS at this point") - endif() - - # ROS libs: - find_package(rclcpp QUIET) - find_package(ament_cmake QUIET) - if(ament_cmake_FOUND AND rclcpp_FOUND) - set(MRPT_ROS_VERSION 2) - else() - find_package(roscpp QUIET) - if (roscpp_FOUND) - set(MRPT_ROS_VERSION 1) - endif() - endif() - - # ====================================== - # Search for common deps in ROS1 & ROS2 - # ====================================== - find_package(cv_bridge QUIET) - find_package(rosbag_storage QUIET) - find_package(rosbag2 QUIET) - - # optional, for tests only: - #find_package(pcl_conversions QUIET) - #find_package(PCL QUIET COMPONENTS common) - - # ROS libs for msgs: - find_package(sensor_msgs QUIET) - find_package(std_msgs QUIET) - find_package(geometry_msgs QUIET) - find_package(stereo_msgs QUIET) - - if ("${MRPT_ROS_VERSION}" STREQUAL "1") - # ================================= - # Search for deps in ROS1 style - # ================================= - - # tf2: try to find w/o the need for catkin (so we can use it from - # Debian build servers w/o ROS installed under /opt/ etc.) - # (JLBC) For some reason calling find_package(tf2) leads to error - # in that case due to missing catkin stuff, while the lib & .h's are - # actually there and are usable. - find_library(tf2_LIBRARIES tf2) - if (tf2_LIBRARIES) - set(tf2_FOUND 1) - else() - set(tf2_FOUND 0) - # 2nd attempt via cmake: - unset(tf2_LIBRARIES CACHE) - unset(tf2_INCLUDE_DIRS CACHE) - find_package(tf2 QUIET) - endif() - # tf2_msgs: idem (header-only lib) - find_file(tf2_msgs name TFMessage.h - PATHS - /usr/include/tf2_msgs - $ENV{ROS_ROOT}/../../include/tf2_msgs - ) - if (tf2_msgs) - set(tf2_msgs_FOUND 1) - else() - set(tf2_msgs_FOUND 0) - # 2nd attempt via cmake: - unset(tf2_msgs_LIBRARIES CACHE) - unset(tf2_msgs_INCLUDE_DIRS CACHE) - find_package(tf2_msgs QUIET) - endif() - # nav_msgs: idem (header-only lib) - find_file(nav_msgs name OccupancyGrid.h - PATHS - /usr/include/nav_msgs - $ENV{ROS_ROOT}/../../include/nav_msgs - ) - if (nav_msgs) - set(nav_msgs_FOUND 1) - else() - set(nav_msgs_FOUND 0) - # 2nd attempt via cmake: - unset(nav_msgs_LIBRARIES CACHE) - unset(nav_msgs_INCLUDE_DIRS CACHE) - find_package(nav_msgs QUIET) - endif() - elseif("${MRPT_ROS_VERSION}" STREQUAL "2") - # ================================= - # Search for deps in ROS2 style - # ================================= - find_package(rclcpp QUIET) - find_package(tf2 QUIET) - find_package(tf2_msgs QUIET) - find_package(nav_msgs QUIET) - endif() - - # Compare flag: - get_directory_property(ROS_DEFINITIONS "COMPILE_DEFINITIONS") - set_directory_properties(PROPERTIES "COMPILE_DEFINITIONS" "") - - - if ("$ENV{VERBOSE}" OR ("$ENV{HOME}" STREQUAL "/home/buildfarm")) - message(STATUS "ROS dependencies:") - message(STATUS " roscpp_INCLUDE_DIRS :${roscpp_INCLUDE_DIRS}") - message(STATUS " roscpp_LIBRARIES :${roscpp_LIBRARIES}") - message(STATUS " ament_cmake_INCLUDE_DIRS :${ament_cmake_INCLUDE_DIRS}") - message(STATUS " ament_cmake_LIBRARIES :${ament_cmake_LIBRARIES}") - message(STATUS " rosbag_storage_INCLUDE_DIRS :${rosbag_storage_INCLUDE_DIRS}") - message(STATUS " rosbag_storage_LIBRARIES :${rosbag_storage_LIBRARIES}") - message(STATUS " cv_bridge_INCLUDE_DIRS :${cv_bridge_INCLUDE_DIRS}") - message(STATUS " cv_bridge_LIBRARIES :${cv_bridge_LIBRARIES}") - message(STATUS " tf2_INCLUDE_DIRS :${tf2_INCLUDE_DIRS}") - message(STATUS " tf2_LIBRARIES :${tf2_LIBRARIES}") - message(STATUS " ROS definitions :${ROS_DEFINITIONS}") - message(STATUS " rclcpp_FOUND: : ${rclcpp_FOUND}") - message(STATUS " cv_bridge_FOUND : ${cv_bridge_FOUND}") - message(STATUS " geometry_msgs_FOUND : ${geometry_msgs_FOUND}") - message(STATUS " nav_msgs_FOUND : ${nav_msgs_FOUND}") - message(STATUS " rosbag_storage_FOUND : ${rosbag_storage_FOUND}") - message(STATUS " rosbag2_FOUND : ${rosbag2_FOUND}") - message(STATUS " sensor_msgs_FOUND : ${sensor_msgs_FOUND}") - message(STATUS " std_msgs_FOUND : ${std_msgs_FOUND}") - message(STATUS " stereo_msgs_FOUND : ${stereo_msgs_FOUND}") - message(STATUS " tf2_FOUND : ${tf2_FOUND}") - endif() - - # To ease debugging in build farms, etc. - if ("$ENV{HOME}" STREQUAL "/home/buildfarm") - message(STATUS "MRPT build 'env' ----------------------------------------------") - execute_process(COMMAND env) - message(STATUS "------------ end of 'env' -------------------------------------") - message(STATUS "MRPT build 'peek /opt/ros' ----------------------------------------------") - execute_process(COMMAND find /opt/ros -maxdepth 2) - message(STATUS "------------ end of 'peek /opt/ros' -------------------------------------") - endif() - - # Convert package versions to hex so they can be used in preprocessor for wider - # versions compatibility of "one-source for all": - mrpt_version_to_hex(cv_bridge_VERSION cv_bridge_VERSION_HEX) - - - # Kinda hack to prevent build farm to time out for "dev" jobs: - # The server scripts always run: (1) build+install, then (2) build+test. - # We will catch the (2) situation and don't build a thing, but return quickly. - if(0) # disabled: re-enable unit tests after package partitioning - if (("${BUILD_TESTING}" STREQUAL "1") AND ("$ENV{HOME}" STREQUAL "/home/buildfarm")) - message(STATUS "====== ROS build farm 'build+test' detected. Aborting tests in this build") - # "make test" shall not fail: - enable_testing() - # "make install" shall not fail: - install(DIRECTORY "${MRPT_SOURCE_DIR}/share/applications" DESTINATION share) - - set(MRPT_ABORT_CMAKE_SCRIPT 1) - return() - endif() - endif() - -endif() # NOT DISABLE_ROS diff --git a/cmakemodules/script_show_final_summary.cmake b/cmakemodules/script_show_final_summary.cmake index 18c193acb8..1ef2b03c3b 100644 --- a/cmakemodules/script_show_final_summary.cmake +++ b/cmakemodules/script_show_final_summary.cmake @@ -167,12 +167,6 @@ if (("$ENV{VERBOSE}") OR ("$ENV{HOME}" STREQUAL "/home/buildfarm")) message(STATUS "PYTHON_INSTALL_DIR : ${PYTHON_INSTALL_DIR}") endif() -message(STATUS " ROS system detected: MRPT_ROS_VERSION=${MRPT_ROS_VERSION}") -message(STATUS " cv_bridge (${cv_bridge_FOUND} ${cv_bridge_VERSION_HEX}) geometry_msgs (${geometry_msgs_FOUND}) nav_msgs (${nav_msgs_FOUND})") -message(STATUS " rosbag_storage (${rosbag_storage_FOUND}) roscpp (${roscpp_FOUND}) rclcpp (${rclcpp_FOUND})") -message(STATUS " sensor_msgs (${sensor_msgs_FOUND}) std_msgs (${std_msgs_FOUND}) stereo_msgs (${stereo_msgs_FOUND}) ") -message(STATUS " tf2 (${tf2_FOUND}) tf2_msgs (${tf2_msgs_FOUND}) pcl_conversions (${pcl_conversions_FOUND})") - MESSAGE(STATUS "") message(STATUS " _____________________ HARDWARE & SENSORS _______________________") diff --git a/libs/ros1bridge/CMakeLists.txt b/libs/ros1bridge/CMakeLists.txt deleted file mode 100644 index 8deb2d5fad..0000000000 --- a/libs/ros1bridge/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ - -# Don't declare the library if dependencies are missing: -if(NOT MRPT_ROS_VERSION EQUAL 1) - return() -endif() -if( - NOT cv_bridge_FOUND OR - NOT geometry_msgs_FOUND OR - NOT nav_msgs_FOUND OR - NOT sensor_msgs_FOUND OR - NOT std_msgs_FOUND OR - NOT stereo_msgs_FOUND OR - NOT tf2_FOUND OR - NOT roscpp_FOUND -) - message(STATUS "mrpt-ros1bridge: Skipping due to missing dependencies") - return() -endif() - -# Additional dependencies for tests only: -if (pcl_conversions_FOUND AND PCL_FOUND) - set_property(GLOBAL PROPERTY mrpt_ros1bridge_UNIT_TEST_EXTRA_DEPS ${pcl_conversions_LIBRARIES} ${PCL_COMMON_LIBRARIES}) - set_property(GLOBAL PROPERTY mrpt_ros1bridge_UNIT_TEST_EXTRA_INCLUDE_DIRS ${pcl_conversions_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS}) - set_property(GLOBAL PROPERTY mrpt_ros1bridge_UNIT_TEST_EXTRA_DEFINITIONS "HAVE_PCL_CONVERSIONS") -endif() - -define_mrpt_lib( - ros1bridge # Lib name - # Dependencies: - mrpt-maps - mrpt-obs - ) - -if(BUILD_mrpt-ros1bridge) -# Add the required libraries for linking: - target_link_libraries(ros1bridge - PUBLIC - ${roscpp_LIBRARIES} - ${cv_bridge_LIBRARIES} - ) - if (tf2_LIBRARIES) - target_link_libraries(ros1bridge PUBLIC ${tf2_LIBRARIES}) - endif() - if (TARGET tf2) - target_link_libraries(ros1bridge PUBLIC tf2) - endif() - - target_include_directories(ros1bridge - SYSTEM - PUBLIC - ${tf2_INCLUDE_DIRS} - PRIVATE - ${cv_bridge_INCLUDE_DIRS} - ) - target_compile_definitions(ros1bridge PRIVATE ${ROS_DEFINITIONS}) - target_compile_definitions(ros1bridge PRIVATE IS_MRPT_ROS1BRIDGE) - target_compile_definitions(ros1bridge PRIVATE CV_BRIDGE_VERSION=${cv_bridge_VERSION_HEX}) -endif() diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/gps.h b/libs/ros1bridge/include/mrpt/ros1bridge/gps.h deleted file mode 100644 index cb55c5c518..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/gps.h +++ /dev/null @@ -1,52 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: GPS.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#pragma once - -#include -#include - -/// ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html -/// MRPT message: -/// https://github.com/MRPT/mrpt/blob/master/libs/obs/include/mrpt/obs/CObservationGPS.h - -/** Conversion functions between ROS 1 <-> MRPT types. - * \ingroup mrpt_ros1bridge_grp - */ -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS - * \return true on sucessful conversion, false on any error. - */ -bool fromROS(const sensor_msgs::NavSatFix& msg, mrpt::obs::CObservationGPS& obj); - -/** Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * \return true on sucessful conversion, only if the input observation has a GGA - * message. - */ -bool toROS( - const mrpt::obs::CObservationGPS& obj, - const std_msgs::Header& msg_header, - sensor_msgs::NavSatFix& msg); - -/** @} */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/image.h b/libs/ros1bridge/include/mrpt/ros1bridge/image.h deleted file mode 100644 index d1e4b908c8..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/image.h +++ /dev/null @@ -1,36 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: image.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#pragma once - -#include -#include -#include - -#include // size_t - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** Makes a deep copy of the image data */ -mrpt::img::CImage fromROS(const sensor_msgs::Image& i); - -/** Makes a deep copy of the image data */ -sensor_msgs::Image toROS(const mrpt::img::CImage& i, const std_msgs::Header& msg_header); -/** @} */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/imu.h b/libs/ros1bridge/include/mrpt/ros1bridge/imu.h deleted file mode 100644 index 71e47cc428..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/imu.h +++ /dev/null @@ -1,53 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: imu.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ -#pragma once - -#include -#include -#include -#include - -#include // size_t - -/// ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html -/// MRPT message: -/// https://github.com/MRPT/mrpt/blob/master/libs/obs/include/mrpt/obs/CObservationIMU.h - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** Convert sensor_msgs/Imu -> mrpt::obs::CObservationIMU - * // STILL NEED TO WRITE CODE FOR COVARIANCE - * \return true on sucessful conversion, false on any error. - */ -bool fromROS(const sensor_msgs::Imu& msg, mrpt::obs::CObservationIMU& obj); - -/** Convert mrpt::obs::CObservationIMU -> sensor_msgs/Imu - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * Since COnservationIMU does not contain covariance terms NEED TO fix those. - * \return true on sucessful conversion, false on any error. - */ -bool toROS( - const mrpt::obs::CObservationIMU& obj, - const std_msgs::Header& msg_header, - sensor_msgs::Imu& msg); - -/** @} */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/laser_scan.h b/libs/ros1bridge/include/mrpt/ros1bridge/laser_scan.h deleted file mode 100644 index 181e8e4a0a..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/laser_scan.h +++ /dev/null @@ -1,54 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include -#include - -#include -#include - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** ROS->MRPT: Takes a sensor_msgs::LaserScan and the relative pose of the laser - * wrt base_link and builds a CObservation2DRangeScan - * \return true on sucessful conversion, false on any error. - * \sa toROS - */ -bool fromROS( - const sensor_msgs::LaserScan& msg, - const mrpt::poses::CPose3D& pose, - mrpt::obs::CObservation2DRangeScan& obj); - -/** MRPT->ROS: Takes a CObservation2DRangeScan and outputs range data in - * sensor_msgs::LaserScan - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS(const mrpt::obs::CObservation2DRangeScan& obj, sensor_msgs::LaserScan& msg); - -/** MRPT->ROS: Takes a CObservation2DRangeScan and outputs range data in - * sensor_msgs::LaserScan + the relative pose of the laser wrt base_link - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::obs::CObservation2DRangeScan& obj, - sensor_msgs::LaserScan& msg, - geometry_msgs::Pose& pose); - -/** @} */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/logging.h b/libs/ros1bridge/include/mrpt/ros1bridge/logging.h deleted file mode 100644 index 52253a82ec..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/logging.h +++ /dev/null @@ -1,100 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | http://www.mrpt.org/ | - | | - | Copyright (c) 2005-2018, 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 | - +------------------------------------------------------------------------+ */ - -/* - * File: logging.h - * (Was: utils.h in mrpt_bridge package, now obsolete since 2022) - * Author: Vladislav Tananaev - */ - -#pragma once - -#include -#include -#include -#include - -namespace mrpt::ros1bridge -{ -/** - *@brief function that converts ROS verbosity level log4cxx::Level to MRPT - * equivalent MRPT's VerbosityLevel - */ -static inline mrpt::system::VerbosityLevel rosLoggerLvlToMRPTLoggerLvl(log4cxx::LevelPtr lvl) -{ - using namespace log4cxx; - - // determine on the corresponding VerbosityLevel - mrpt::system::VerbosityLevel mrpt_lvl; - - if (lvl == Level::getFatal() || lvl == Level::getError()) - { - mrpt_lvl = mrpt::system::LVL_ERROR; - } - else if (lvl == Level::getWarn()) - { - mrpt_lvl = mrpt::system::LVL_WARN; - } - else if (lvl == Level::getInfo()) - { - mrpt_lvl = mrpt::system::LVL_INFO; - } - else if (lvl == Level::getDebug() || lvl == Level::getTrace()) - { - mrpt_lvl = mrpt::system::LVL_DEBUG; - } - else - { - mrpt_lvl = mrpt::system::LVL_INFO; - ROS_ERROR("Unknown log4cxx::Level is given."); - } - - return mrpt_lvl; - -} // end of rosLoggerLvlToMRPTLoggerLvl - -/** - *@brief callback that is called by MRPT mrpt::system::COuputLogger to redirect - * log messages to ROS logger. - * This function has to be inline, otherwise option - * log4j.logger.ros.package_name will be taken from mrpt_bridge - * instead of the package from which macro is actually called. - */ -static inline void mrptToROSLoggerCallback( - const std::string& msg, - const mrpt::system::VerbosityLevel level, - [[maybe_unused]] const std::string& loggerName, - [[maybe_unused]] const mrpt::system::TTimeStamp timestamp) -{ - // Remove trailing \n if present - std::string tmsg = msg; - if (!tmsg.empty() && tmsg.compare(tmsg.length() - 1, tmsg.length(), "\n") == 0) - { - tmsg.erase(tmsg.end() - 1); - } - - switch (level) - { - case mrpt::system::LVL_DEBUG: - ROS_DEBUG_STREAM(msg); - break; - case mrpt::system::LVL_WARN: - ROS_WARN_STREAM(tmsg); - break; - case mrpt::system::LVL_ERROR: - ROS_ERROR_STREAM(tmsg); - break; - default: - case mrpt::system::LVL_INFO: - ROS_INFO_STREAM(tmsg); - break; - } -} - -} // namespace mrpt::ros1bridge \ No newline at end of file diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/map.h b/libs/ros1bridge/include/mrpt/ros1bridge/map.h deleted file mode 100644 index d46e5b5e4a..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/map.h +++ /dev/null @@ -1,120 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include - -#include -#include - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** @name Maps, Occupancy Grid Maps: ROS <-> MRPT - * @{ */ - -/** Methods to convert between ROS msgs and MRPT objects for map datatypes. - * @brief the map class is implemented as singeleton use map::instance - * ()->fromROS ... - */ -class MapHdl -{ - private: -#ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS - int lut_cellmrpt2ros[0x100]; // lookup table for entry convertion -#else - int lut_cellmrpt2ros[0xFFFF]; // lookup table for entry convertion -#endif - int lut_cellros2mrpt[101]; // lookup table for entry convertion - - MapHdl(); - MapHdl(const MapHdl&); - ~MapHdl() = default; - - public: - /** - * @return returns singeleton instance - * @brief it creates a instance with some look up table to speed up the - * conversions - */ - static MapHdl* instance(); - -#ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS - int8_t cellMrpt2Ros(int8_t i) { return lut_cellmrpt2ros[static_cast(i) - INT8_MIN]; } -#else - int16_t cellMrpt2Ros(int16_t i) { return lut_cellmrpt2ros[static_cast(i) - INT16_MIN]; } -#endif - int8_t cellRos2Mrpt(int8_t i) - { - if (i < 0) - { - // unobserved cells: no log-odds information - return 0; - } - ASSERT_LE_(i, 100); - return lut_cellros2mrpt[i]; - } - - /** - * loads a mprt map - * @return true on sucess. - * @param _metric_map - * @param _config_file - * @param _map_file default: map.simplemap - * @param _section_name default: metricMap - * @param _debug default: false - */ - static bool loadMap( - mrpt::maps::CMultiMetricMap& _metric_map, - const mrpt::config::CConfigFileBase& _config_file, - const std::string& _map_file = "map.simplemap", - const std::string& _section_name = "metricMap", - bool _debug = false); -}; - -/** - * converts ros msg to mrpt object - * @return true on sucessful conversion, false on any error. - * @param src - * @param des - */ -bool fromROS(const nav_msgs::OccupancyGrid& src, mrpt::maps::COccupancyGridMap2D& des); - -/** - * converts mrpt object to ros msg and updates the msg header - * @return true on sucessful conversion, false on any error. - * @param src - * @param header - * @param as_costmap If set to true, gridmap cell values will be copied without changes - * (interpreted as int8_t instead of Log-odds) - */ -bool toROS( - const mrpt::maps::COccupancyGridMap2D& src, - nav_msgs::OccupancyGrid& msg, - const std_msgs::Header& header, - bool as_costmap = false); -/** - * converts mrpt object to ros msg - * @return true on sucessful conversion, false on any error. - */ -bool toROS( - const mrpt::maps::COccupancyGridMap2D& src, - nav_msgs::OccupancyGrid& msg, - bool as_costmap = false); - -/** @} - * @} - */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud.h b/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud.h deleted file mode 100644 index 987f81254e..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud.h +++ /dev/null @@ -1,49 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** @name Point clouds: ROS <-> MRPT - * @{ */ - -/** Convert sensor_msgs/PointCloud -> mrpt::maps::CSimplePointsMap - * CSimplePointsMap only contains (x,y,z) data, so - * sensor_msgs::PointCloud::channels are ignored. - * \return true on sucessful conversion, false on any error. - * \sa toROS - */ -bool fromROS(const sensor_msgs::PointCloud& msg, mrpt::maps::CSimplePointsMap& obj); - -/** Convert mrpt::maps::CSimplePointsMap -> sensor_msgs/PointCloud - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * Since CSimplePointsMap only contains (x,y,z) data, - * sensor_msgs::PointCloud::channels will be empty. - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::maps::CSimplePointsMap& obj, - const std_msgs::Header& msg_header, - sensor_msgs::PointCloud& msg); - -/** @} @} - */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h b/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h deleted file mode 100644 index 36dad300f4..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h +++ /dev/null @@ -1,99 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** @name sensor_msgs::PointCloud2: ROS <-> MRPT - * @{ */ - -/** Convert sensor_msgs/PointCloud2 -> mrpt::slam::CSimplePointsMap - * Only (x,y,z) data is converted. To use the intensity channel, see - * the alternative signature for CPointsMapXYZI. - * Requires point cloud fields: x,y,z. - * \return true on sucessful conversion, false on any error. - * \sa toROS - */ -bool fromROS(const sensor_msgs::PointCloud2& msg, mrpt::maps::CSimplePointsMap& obj); - -/** \overload For (x,y,z,intensity) channels. - * Requires point cloud fields: x,y,z,intensity - */ -bool fromROS(const sensor_msgs::PointCloud2& msg, mrpt::maps::CPointsMapXYZI& obj); - -/** \overload For (x,y,z,intensity,ring,time) channels. - * Requires point cloud fields: x,y,z,intensity,ring,time - */ -bool fromROS(const sensor_msgs::PointCloud2& msg, mrpt::maps::CPointsMapXYZIRT& obj); - -/** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan. - * Requires point cloud fields: x,y,z,intensity,ring - */ -bool fromROS( - const sensor_msgs::PointCloud2& m, - mrpt::obs::CObservationRotatingScan& o, - const mrpt::poses::CPose3D& sensorPoseOnRobot, - unsigned int num_azimuth_divisions = 360, - float max_intensity = 1000.0f); - -/** Extract a list of fields found in the point cloud. - * Typically: {"x","y","z","intensity"} - */ -std::set extractFields(const sensor_msgs::PointCloud2& msg); - -/** Convert mrpt::slam::CSimplePointsMap -> sensor_msgs/PointCloud2 - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * Generated sensor_msgs::PointCloud2::channels: `x`, `y`, `z`. - * - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::maps::CSimplePointsMap& obj, - const std_msgs::Header& msg_header, - sensor_msgs::PointCloud2& msg); - -/** \overload With these fields: `x`, `y`, `z`, `intensity` - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::maps::CPointsMapXYZI& obj, - const std_msgs::Header& msg_header, - sensor_msgs::PointCloud2& msg); - -/** \overload With these fields: `x`, `y`, `z`, `intensity`, `ring`, `timestamp` - * \return true on successful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::maps::CPointsMapXYZIRT& obj, - const std_msgs::Header& msg_header, - sensor_msgs::PointCloud2& msg); - -/** @} */ -/** @} */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/pose.h b/libs/ros1bridge/include/mrpt/ros1bridge/pose.h deleted file mode 100644 index ce7a8df28a..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/pose.h +++ /dev/null @@ -1,60 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include // size_t - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -tf2::Matrix3x3 toROS(const mrpt::math::CMatrixDouble33& src); - -tf2::Transform toROS_tfTransform(const mrpt::poses::CPose2D& src); -geometry_msgs::Pose toROS_Pose(const mrpt::poses::CPose2D& src); - -tf2::Transform toROS_tfTransform(const mrpt::math::TPose2D& src); -geometry_msgs::Pose toROS_Pose(const mrpt::math::TPose2D& src); - -tf2::Transform toROS_tfTransform(const mrpt::poses::CPose3D& src); -geometry_msgs::Pose toROS_Pose(const mrpt::poses::CPose3D& src); - -tf2::Transform toROS_tfTransform(const mrpt::math::TPose3D& src); -geometry_msgs::Pose toROS_Pose(const mrpt::math::TPose3D& src); - -geometry_msgs::PoseWithCovariance toROS_Pose(const mrpt::poses::CPose3DPDFGaussian& src); - -geometry_msgs::PoseWithCovariance toROS(const mrpt::poses::CPose3DPDFGaussianInf& src); - -geometry_msgs::PoseWithCovariance toROS(const mrpt::poses::CPosePDFGaussian& src); - -geometry_msgs::PoseWithCovariance toROS(const mrpt::poses::CPosePDFGaussianInf& src); - -geometry_msgs::Quaternion toROS(const mrpt::math::CQuaternionDouble& src); - -mrpt::poses::CPose3D fromROS(const tf2::Transform& src); -mrpt::math::CMatrixDouble33 fromROS(const tf2::Matrix3x3& src); -mrpt::poses::CPose3D fromROS(const geometry_msgs::Pose& src); -mrpt::poses::CPose3DPDFGaussian fromROS(const geometry_msgs::PoseWithCovariance& src); -mrpt::math::CQuaternionDouble fromROS(const geometry_msgs::Quaternion& src); - -/** @} */ -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/range.h b/libs/ros1bridge/include/mrpt/ros1bridge/range.h deleted file mode 100644 index 74b18e990e..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/range.h +++ /dev/null @@ -1,50 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: range.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#pragma once - -#include -#include - -/// ROS message : http://docs.ros.org/api/sensor_msgs/html/msg/Range.html -/// MRPT message: -/// https://github.com/MRPT/mrpt/blob/master/libs/obs/include/mrpt/obs/CObservationRange.h - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** Convert sensor_msgs/Range -> mrpt::obs::CObservationRange - * \return true on sucessful conversion, false on any error. - */ -bool fromROS(const sensor_msgs::Range& msg, mrpt::obs::CObservationRange& obj); - -/** Convert mrpt::obs::CObservationRange -> sensor_msgs/Range - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * Since COnservation does not contain "radiation_type", - * sensor_msgs::Range::radiation_type will be empty. \return true on sucessful - * conversion, false on any error. - */ -bool toROS( - const mrpt::obs::CObservationRange& obj, - const std_msgs::Header& msg_header, - sensor_msgs::Range* msg); - -/** @} */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/stereo_image.h b/libs/ros1bridge/include/mrpt/ros1bridge/stereo_image.h deleted file mode 100644 index b293e5ec65..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/stereo_image.h +++ /dev/null @@ -1,36 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: stereo_image.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#pragma once - -#include -#include -#include - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -bool toROS( - const mrpt::obs::CObservationStereoImages& obj, - const std_msgs::Header& msg_header, - sensor_msgs::Image& left, - sensor_msgs::Image& right, - stereo_msgs::DisparityImage& disparity); - -/** @} */ - -} // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/time.h b/libs/ros1bridge/include/mrpt/ros1bridge/time.h deleted file mode 100644 index 1cec3e4daf..0000000000 --- a/libs/ros1bridge/include/mrpt/ros1bridge/time.h +++ /dev/null @@ -1,36 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include - -namespace mrpt::ros1bridge -{ -/** \addtogroup mrpt_ros1bridge_grp - * @{ */ - -/** - * converts ros time to mrpt time - * @param src ros time - * @param des mrpt time - */ -mrpt::system::TTimeStamp fromROS(const ros::Time& src); - -/** - * converts mrpt time to ros time - * @param src ros time - * @param des mrpt time - */ -ros::Time toROS(const mrpt::system::TTimeStamp& src); - -/** @} */ - -}; // namespace mrpt::ros1bridge diff --git a/libs/ros1bridge/src/gps.cpp b/libs/ros1bridge/src/gps.cpp deleted file mode 100644 index 4a1a7666e6..0000000000 --- a/libs/ros1bridge/src/gps.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - LIBRARY: mrpt_ros bridge - FILE: gps.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#include -#include - -bool mrpt::ros1bridge::fromROS(const sensor_msgs::NavSatFix& msg, mrpt::obs::CObservationGPS& obj) -{ - mrpt::obs::gnss::Message_NMEA_GGA gga; - gga.fields.altitude_meters = msg.altitude; - gga.fields.latitude_degrees = msg.latitude; - gga.fields.longitude_degrees = msg.longitude; - - switch (msg.status.status) - { - case -1: - gga.fields.fix_quality = 0; - break; - case 0: - gga.fields.fix_quality = 1; - break; - case 2: - gga.fields.fix_quality = 2; - break; - case 1: - gga.fields.fix_quality = 3; - break; - default: - gga.fields.fix_quality = 0; // never going to execute default - } - obj.setMsg(gga); - - obj.timestamp = mrpt::ros1bridge::fromROS(msg.header.stamp); - - if (msg.position_covariance_type != sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN) - { - auto& cov = obj.covariance_enu.emplace(); - for (int r = 0, i = 0; r < 3; r++) - for (int c = 0; c < 3; c++) cov(r, c) = msg.position_covariance.at(i++); - } - - return true; -} - -bool mrpt::ros1bridge::toROS( - const mrpt::obs::CObservationGPS& obj, - const std_msgs::Header& msg_header, - sensor_msgs::NavSatFix& msg) -{ - bool valid = false; - - // 1) sensor_msgs::NavSatFix:: header - msg.header = msg_header; - - // 2) other NavSatFix Parameters, the following 3 could be wrong too - - if (obj.hasMsgClass()) - { - const mrpt::obs::gnss::Message_NMEA_GGA& gga = - obj.getMsgByClass(); - msg.altitude = gga.fields.altitude_meters; - msg.latitude = gga.fields.latitude_degrees; - msg.longitude = gga.fields.longitude_degrees; - - /// following parameter assigned as per - /// http://mrpt.ual.es/reference/devel/structmrpt_1_1obs_1_1gnss_1_1_message___n_m_e_a___g_g_a_1_1content__t.html#a33415dc947663d43015605c41b0f66cb - /// http://mrpt.ual.es/reference/devel/gnss__messages__ascii__nmea_8h_source.html - switch (gga.fields.fix_quality) - { - case 0: - msg.status.status = -1; - break; - case 1: - msg.status.status = 0; - break; - case 2: - msg.status.status = 2; - break; - case 3: - msg.status.status = 1; - break; - default: - // this is based on literature available on GPS as the number of - // types in ROS and MRPT are not same - msg.status.status = 0; - } - // this might be incorrect as there is not matching field in mrpt - // message type - msg.status.service = 1; - - valid = true; - } - - // cov: - if (obj.covariance_enu.has_value()) - { - msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN; - - for (int r = 0, i = 0; r < 3; r++) - for (int c = 0; c < 3; c++) msg.position_covariance.at(i++) = (*obj.covariance_enu)(r, c); - } - else - { - msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; - } - - return valid; -} - -/// NavSatFix ROS message -/* -uint8 COVARIANCE_TYPE_UNKNOWN=0 -uint8 COVARIANCE_TYPE_APPROXIMATED=1 -uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2 -uint8 COVARIANCE_TYPE_KNOWN=3 -std_msgs/Header header -sensor_msgs/NavSatStatus status -float64 latitude -float64 longitude -float64 altitude -float64[9] position_covariance -uint8 position_covariance_type -*/ - -/// NavSatStatus ROS message -/* -int8 STATUS_NO_FIX=-1 -int8 STATUS_FIX=0 -int8 STATUS_SBAS_FIX=1 -int8 STATUS_GBAS_FIX=2 -uint16 SERVICE_GPS=1 -uint16 SERVICE_GLONASS=2 -uint16 SERVICE_COMPASS=4 -uint16 SERVICE_GALILEO=8 -int8 status -uint16 service - */ diff --git a/libs/ros1bridge/src/image.cpp b/libs/ros1bridge/src/image.cpp deleted file mode 100644 index 8386580fff..0000000000 --- a/libs/ros1bridge/src/image.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: image.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#if CV_BRIDGE_VERSION < 0x030400 -#include -#else -#include -#endif - -#include -#include -#include - -using namespace mrpt::img; -using namespace ros; -using namespace sensor_msgs; -using namespace cv; -using namespace cv_bridge; - -mrpt::img::CImage mrpt::ros1bridge::fromROS(const sensor_msgs::Image& i) -{ - return mrpt::img::CImage(cv_bridge::toCvCopy(i, "bgr8").get()->image, mrpt::img::DEEP_COPY); -} - -sensor_msgs::Image mrpt::ros1bridge::toROS( - const mrpt::img::CImage& i, const std_msgs::Header& msg_header) -{ - const Mat& cvImg = i.asCvMatRef(); - - cv_bridge::CvImage img_bridge; - - sensor_msgs::Image msg; - img_bridge = CvImage( - msg.header, - i.isColor() ? sensor_msgs::image_encodings::BGR8 : sensor_msgs::image_encodings::MONO8, - cvImg); - - img_bridge.toImageMsg(msg); - - msg.encoding = i.isColor() ? "bgr8" : "mono8"; - msg.header = msg_header; - msg.height = i.getHeight(); - msg.width = i.getWidth(); - - return msg; -} - -// -/* -std_msgs/Header header -uint32 height -uint32 width -string encoding -uint8 is_bigendian -uint32 step -uint8[] data - */ diff --git a/libs/ros1bridge/src/imu.cpp b/libs/ros1bridge/src/imu.cpp deleted file mode 100644 index c87867f524..0000000000 --- a/libs/ros1bridge/src/imu.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: imu.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#include - -bool mrpt::ros1bridge::fromROS(const sensor_msgs::Imu& msg, mrpt::obs::CObservationIMU& obj) -{ - using namespace mrpt::obs; - - if (msg.orientation_covariance.at(0) >= 0) - { - obj.set(IMU_ORI_QUAT_X, msg.orientation.x); - obj.set(IMU_ORI_QUAT_Y, msg.orientation.y); - obj.set(IMU_ORI_QUAT_Z, msg.orientation.z); - obj.set(IMU_ORI_QUAT_W, msg.orientation.w); - } - - if (msg.linear_acceleration_covariance.at(0) >= 0) - { - obj.set(IMU_X_ACC, msg.linear_acceleration.x); - obj.set(IMU_Y_ACC, msg.linear_acceleration.y); - obj.set(IMU_Z_ACC, msg.linear_acceleration.z); - } - - if (msg.angular_velocity_covariance.at(0) >= 0) - { - obj.set(IMU_WX, msg.angular_velocity.x); - obj.set(IMU_WY, msg.angular_velocity.y); - obj.set(IMU_WZ, msg.angular_velocity.z); - } - - return true; -} - -bool mrpt::ros1bridge::toROS( - const mrpt::obs::CObservationIMU& obj, - const std_msgs::Header& msg_header, - sensor_msgs::Imu& msg) -{ - using namespace mrpt::obs; - - msg.header = msg_header; - - const auto& m = obj.rawMeasurements; - if (obj.has(IMU_ORI_QUAT_X)) - { - msg.orientation.x = m.at(IMU_ORI_QUAT_X); - msg.orientation.y = m.at(IMU_ORI_QUAT_Y); - msg.orientation.z = m.at(IMU_ORI_QUAT_Z); - msg.orientation.w = m.at(IMU_ORI_QUAT_W); - - msg.orientation_covariance.fill(0.01); - } - else - { - msg.orientation_covariance.fill(-1); - } - - if (obj.has(IMU_X_ACC)) - { - msg.linear_acceleration.x = m.at(IMU_X_ACC); - msg.linear_acceleration.y = m.at(IMU_Y_ACC); - msg.linear_acceleration.z = m.at(IMU_Z_ACC); - - msg.linear_acceleration_covariance.fill(0.01); - } - else - { - msg.linear_acceleration_covariance.fill(-1); - } - - if (obj.has(IMU_WX)) - { - msg.angular_velocity.x = m.at(IMU_WX); - msg.angular_velocity.y = m.at(IMU_WY); - msg.angular_velocity.z = m.at(IMU_WZ); - - msg.angular_velocity_covariance.fill(0.01); - } - else - { - msg.angular_velocity_covariance.fill(-1); - } - - return true; -} - -/* -std_msgs/Header header -geometry_msgs/Quaternion orientation -float64[9] orientation_covariance -geometry_msgs/Vector3 angular_velocity -float64[9] angular_velocity_covariance -geometry_msgs/Vector3 linear_acceleration -float64[9] linear_acceleration_covariance - */ diff --git a/libs/ros1bridge/src/laser_scan.cpp b/libs/ros1bridge/src/laser_scan.cpp deleted file mode 100644 index 283cfc2060..0000000000 --- a/libs/ros1bridge/src/laser_scan.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include -#include -#include -#include -#include -#include - -bool mrpt::ros1bridge::fromROS( - const sensor_msgs::LaserScan& msg, - const mrpt::poses::CPose3D& pose, - mrpt::obs::CObservation2DRangeScan& obj) -{ - obj.timestamp = fromROS(msg.header.stamp); - obj.rightToLeft = true; - obj.sensorLabel = msg.header.frame_id; - obj.aperture = msg.angle_max - msg.angle_min; - obj.maxRange = msg.range_max; - obj.sensorPose = pose; - - ASSERT_(msg.ranges.size() > 1); - - const size_t N = msg.ranges.size(); - const double ang_step = obj.aperture / (N - 1); - const double fov05 = 0.5 * obj.aperture; - const double inv_ang_step = (N - 1) / obj.aperture; - - obj.resizeScan(N); - for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++) - { - // ROS indices go from msg.angle_min to msg.angle_max, while - // in MRPT they go from -FOV/2 to +FOV/2. - int i_ros = inv_ang_step * (-fov05 - msg.angle_min + ang_step * i_mrpt); - if (i_ros < 0) - i_ros += N; - else if (i_ros >= (int)N) - i_ros -= N; // wrap around 2PI... - - // set the scan - const float r = msg.ranges[i_ros]; - obj.setScanRange(i_mrpt, r); - - // set the validity of the scan - const auto ri = obj.getScanRange(i_mrpt); - const bool r_valid = ((ri < (msg.range_max * 0.99)) && (ri > msg.range_min)); - obj.setScanRangeValidity(i_mrpt, r_valid); - } - - return true; -} - -bool mrpt::ros1bridge::toROS( - const mrpt::obs::CObservation2DRangeScan& obj, sensor_msgs::LaserScan& msg) -{ - const size_t nRays = obj.getScanSize(); - if (!nRays) return false; - - msg.angle_min = -0.5f * obj.aperture; - msg.angle_max = 0.5f * obj.aperture; - msg.angle_increment = obj.aperture / (obj.getScanSize() - 1); - - // setting the following values to zero solves a rviz visualization problem - msg.time_increment = 0.0; // 1./30.; // Anything better? - msg.scan_time = 0.0; // msg.time_increment; // idem? - - msg.range_min = 0.02f; - msg.range_max = obj.maxRange; - - msg.ranges.resize(nRays); - for (size_t i = 0; i < nRays; i++) msg.ranges[i] = obj.getScanRange(i); - - // Set header data: - msg.header.stamp = toROS(obj.timestamp); - msg.header.frame_id = obj.sensorLabel; - - return true; -} - -bool mrpt::ros1bridge::toROS( - const mrpt::obs::CObservation2DRangeScan& obj, - sensor_msgs::LaserScan& msg, - geometry_msgs::Pose& pose) -{ - toROS(obj, msg); - pose = toROS_Pose(obj.sensorPose); - return true; -} diff --git a/libs/ros1bridge/src/map.cpp b/libs/ros1bridge/src/map.cpp deleted file mode 100644 index 2153ae904f..0000000000 --- a/libs/ros1bridge/src/map.cpp +++ /dev/null @@ -1,219 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include // for fileExists() -#include // for lowerCase() -#include -#include -#include - -using namespace mrpt::config; -using namespace mrpt::io; -using mrpt::maps::CLogOddsGridMapLUT; -using mrpt::maps::CMultiMetricMap; -using mrpt::maps::COccupancyGridMap2D; -using mrpt::maps::CSimpleMap; - -#ifndef INT8_MAX // avoid duplicated #define's -#define INT8_MAX 0x7f -#define INT8_MIN (-INT8_MAX - 1) -#define INT16_MAX 0x7fff -#define INT16_MIN (-INT16_MAX - 1) -#endif // INT8_MAX - -using namespace mrpt::ros1bridge; - -MapHdl::MapHdl() -{ - // MRPT -> ROS LUT: - CLogOddsGridMapLUT table; - -#ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS - const int i_min = INT8_MIN, i_max = INT8_MAX; -#else - const int i_min = INT16_MIN, i_max = INT16_MAX; -#endif - - for (int i = i_min; i <= i_max; i++) - { - int8_t ros_val; - if (i == 0) - { - // Unknown cell (no evidence data): - ros_val = -1; - } - else - { - float p = 1.0 - table.l2p(i); - ros_val = round(p * 100.); - // printf("- cell -> ros = %4i -> %4i, p=%4.3f\n", i, ros_val, p); - } - - lut_cellmrpt2ros[static_cast(i) - i_min] = ros_val; - } - - // ROS -> MRPT: [0,100] -> - for (int i = 0; i <= 100; i++) - { - const float p = 1.0 - (i / 100.0); - lut_cellros2mrpt[i] = table.p2l(p); - - // printf("- ros->cell=%4i->%4i p=%4.3f\n",i, lut_cellros2mrpt[i], p); - } -} -MapHdl* MapHdl::instance() -{ - static MapHdl m; // singeleton instance - return &m; -} - -bool mrpt::ros1bridge::fromROS(const nav_msgs::OccupancyGrid& src, COccupancyGridMap2D& des) -{ - MRPT_START - if ((src.info.origin.orientation.x != 0) || (src.info.origin.orientation.y != 0) || - (src.info.origin.orientation.z != 0) || (src.info.origin.orientation.w != 1)) - { - std::cerr << "[fromROS] Rotated maps are not supported!\n"; - return false; - } - float xmin = src.info.origin.position.x; - float ymin = src.info.origin.position.y; - float xmax = xmin + src.info.width * src.info.resolution; - float ymax = ymin + src.info.height * src.info.resolution; - - des.setSize(xmin, xmax, ymin, ymax, src.info.resolution); - auto inst = MapHdl::instance(); - for (unsigned int h = 0; h < src.info.height; h++) - { - COccupancyGridMap2D::cellType* pDes = des.getRow(h); - const int8_t* pSrc = &src.data[h * src.info.width]; - for (unsigned int w = 0; w < src.info.width; w++) *pDes++ = inst->cellRos2Mrpt(*pSrc++); - } - return true; - MRPT_END -} -bool mrpt::ros1bridge::toROS( - const COccupancyGridMap2D& src, - nav_msgs::OccupancyGrid& des, - const std_msgs::Header& header, - bool as_costmap) -{ - des.header = header; - return toROS(src, des, as_costmap); -} - -bool mrpt::ros1bridge::toROS( - const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des, bool as_costmap) -{ - des.info.width = src.getSizeX(); - des.info.height = src.getSizeY(); - des.info.resolution = src.getResolution(); - - des.info.origin.position.x = src.getXMin(); - des.info.origin.position.y = src.getYMin(); - des.info.origin.position.z = 0; - - des.info.origin.orientation.x = 0; - des.info.origin.orientation.y = 0; - des.info.origin.orientation.z = 0; - des.info.origin.orientation.w = 1; - - // I hope the data is always aligned - des.data.resize(des.info.width * des.info.height); - for (unsigned int h = 0; h < des.info.height; h++) - { - const COccupancyGridMap2D::cellType* pSrc = src.getRow(h); - ASSERT_(pSrc); - int8_t* pDes = &des.data[h * des.info.width]; - for (unsigned int w = 0; w < des.info.width; w++) - { - if (as_costmap) - *pDes++ = static_cast(*pSrc++); - else - *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++); - } - } - return true; -} - -bool MapHdl::loadMap( - CMultiMetricMap& _metric_map, - const CConfigFileBase& _config_file, - const std::string& _map_file, - const std::string& _section_name, - bool _debug) -{ - using namespace mrpt::maps; - - TSetOfMetricMapInitializers mapInitializers; - mapInitializers.loadFromConfigFile(_config_file, _section_name); - - CSimpleMap simpleMap; - - // Load the set of metric maps to consider in the experiments: - _metric_map.setListOfMaps(mapInitializers); - if (_debug) mapInitializers.dumpToConsole(); - - if (_debug) printf("%s, _map_file.size() = %zu\n", _map_file.c_str(), _map_file.size()); - // Load the map (if any): - if (_map_file.size() < 3) - { - if (_debug) printf("No mrpt map file!\n"); - return false; - } - else - { - ASSERT_(mrpt::system::fileExists(_map_file)); - - // Detect file extension: - std::string mapExt = mrpt::system::lowerCase( - mrpt::system::extractFileExtension(_map_file, true)); // Ignore possible .gz extensions - - if (!mapExt.compare("simplemap")) - { - // It's a ".simplemap": - if (_debug) printf("Loading '.simplemap' file..."); - CFileGZInputStream f(_map_file); - mrpt::serialization::archiveFrom(f) >> simpleMap; - - ASSERTMSG_(simpleMap.size() > 0, "Simplemap was aparently loaded OK, but it is empty!"); - - // Build metric map: - if (_debug) printf("Building metric map(s) from '.simplemap'..."); - _metric_map.loadFromSimpleMap(simpleMap); - if (_debug) printf("Ok\n"); - } - else if (!mapExt.compare("gridmap")) - { - // It's a ".gridmap": - if (_debug) printf("Loading gridmap from '.gridmap'..."); - ASSERTMSG_( - _metric_map.countMapsByClass() == 1, - "Error: Trying to load a gridmap into a multi-metric map " - "requires 1 gridmap member."); - CFileGZInputStream fm(_map_file); - mrpt::serialization::archiveFrom(fm) >> (*_metric_map.mapByClass()); - if (_debug) printf("Ok\n"); - } - else - { - THROW_EXCEPTION(mrpt::format("Map file has unknown extension: '%s'", mapExt.c_str())); - return false; - } - } - return true; -} diff --git a/libs/ros1bridge/src/map_unittest.cpp b/libs/ros1bridge/src/map_unittest.cpp deleted file mode 100644 index 02e436fc03..0000000000 --- a/libs/ros1bridge/src/map_unittest.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * test_map.cpp - * - * Created on: July 21, 2014 - * Author: Markus Bader - */ - -#include -#include -#include -#include -#include - -using mrpt::maps::COccupancyGridMap2D; - -void getEmptyRosMsg(nav_msgs::OccupancyGrid& msg) -{ - msg.info.width = 300; - msg.info.height = 500; - msg.info.resolution = 0.1; - msg.info.origin.position.x = rand() % 10 - 5; - msg.info.origin.position.y = rand() % 10 - 5; - msg.info.origin.position.z = 0; - - msg.info.origin.orientation.x = 0; - msg.info.origin.orientation.y = 0; - msg.info.origin.orientation.z = 0; - msg.info.origin.orientation.w = 1; - - msg.data.resize(msg.info.width * msg.info.height, -1); -} - -TEST(Map, basicTestHeader) -{ - nav_msgs::OccupancyGrid srcRos; - COccupancyGridMap2D desMrpt; - - getEmptyRosMsg(srcRos); - - srcRos.info.origin.orientation.x = 0; // fix rotation - EXPECT_TRUE(mrpt::ros1bridge::fromROS(srcRos, desMrpt)); - - EXPECT_EQ(srcRos.info.width, desMrpt.getSizeX()); - EXPECT_EQ(srcRos.info.height, desMrpt.getSizeY()); - EXPECT_EQ(srcRos.info.resolution, desMrpt.getResolution()); - for (uint32_t h = 0; h < srcRos.info.width; h++) - { - for (uint32_t w = 0; w < srcRos.info.width; w++) - { - EXPECT_EQ(desMrpt.getPos(w, h), 0.5); // all -1 entreis should map to 0.5 - } - } -} - -TEST(Map, check_ros2mrpt_and_back) -{ - nav_msgs::OccupancyGrid srcRos; - COccupancyGridMap2D desMrpt; - nav_msgs::OccupancyGrid desRos; - - // Test empty gridmap: - getEmptyRosMsg(srcRos); - - ASSERT_TRUE(mrpt::ros1bridge::fromROS(srcRos, desMrpt)); - ASSERT_TRUE(mrpt::ros1bridge::toROS(desMrpt, desRos, desRos.header)); - // all -1 entries should map back to -1 - for (uint32_t h = 0; h < srcRos.info.width; h++) - for (uint32_t w = 0; w < srcRos.info.width; w++) - EXPECT_EQ(desRos.data[h * srcRos.info.width + h], -1); - - // Test gridmap with values: 0 to 100 - for (int i = 0; i <= 100; i++) - { - // 50 is mid-gray -> unknown = -1 in ROS - srcRos.data[i] = (std::abs(i - 50) <= 2) ? -1 : i; - } - - EXPECT_TRUE(mrpt::ros1bridge::fromROS(srcRos, desMrpt)); - EXPECT_TRUE(mrpt::ros1bridge::toROS(desMrpt, desRos, desRos.header)); - - for (int i = 0; i <= 100; i++) - { - /*printf( - "%4i, %4.3f = %4.3f,%4i\n", srcRos.data[i], 1.0f - 0.01f * i, - desMrpt.getCell(i, 0), desRos.data[i]); */ - EXPECT_NEAR(1.0f - 0.01f * i, desMrpt.getCell(i, 0), 0.03f) << "ros to mprt" - << "i=" << i; - EXPECT_NEAR(srcRos.data[i], desRos.data[i], 1) << "ros to mprt to ros"; - } -} diff --git a/libs/ros1bridge/src/point_cloud.cpp b/libs/ros1bridge/src/point_cloud.cpp deleted file mode 100644 index 8e7dd7c009..0000000000 --- a/libs/ros1bridge/src/point_cloud.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include - -using namespace mrpt::maps; - -bool mrpt::ros1bridge::fromROS(const sensor_msgs::PointCloud& msg, CSimplePointsMap& obj) -{ - const size_t N = msg.points.size(); - - obj.clear(); - obj.reserve(N); - for (size_t i = 0; i < N; i++) obj.insertPoint(msg.points[i].x, msg.points[i].y, msg.points[i].z); - - return true; -} - -bool mrpt::ros1bridge::toROS( - const CSimplePointsMap& obj, const std_msgs::Header& msg_header, sensor_msgs::PointCloud& msg) -{ - // 1) sensor_msgs::PointCloud:: header - msg.header = msg_header; - - // 2) sensor_msgs::PointCloud:: points - const size_t N = obj.size(); - msg.points.resize(N); - for (size_t i = 0; i < N; i++) - { - geometry_msgs::Point32& pt = msg.points[i]; - obj.getPoint(i, pt.x, pt.y, pt.z); - } - - // 3) sensor_msgs::PointCloud:: channels - msg.channels.clear(); - - return true; -} diff --git a/libs/ros1bridge/src/point_cloud2.cpp b/libs/ros1bridge/src/point_cloud2.cpp deleted file mode 100644 index cc3de959dc..0000000000 --- a/libs/ros1bridge/src/point_cloud2.cpp +++ /dev/null @@ -1,602 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace mrpt::maps; - -namespace -{ -bool check_field( - const sensor_msgs::PointField& input_field, - std::string check_name, - const sensor_msgs::PointField** output) -{ - bool coherence_error = false; - if (input_field.name == check_name) - { - if (input_field.datatype != sensor_msgs::PointField::FLOAT32 && - input_field.datatype != sensor_msgs::PointField::FLOAT64 && - input_field.datatype != sensor_msgs::PointField::UINT16 && - input_field.datatype != sensor_msgs::PointField::UINT32 && - input_field.datatype != sensor_msgs::PointField::UINT8) - { - *output = nullptr; - coherence_error = true; - } - else - { - *output = &input_field; - } - } - return coherence_error; -} - -void get_float_from_field( - const sensor_msgs::PointField* field, const unsigned char* data, float& output) -{ - if (field != nullptr) - { - if (field->datatype == sensor_msgs::PointField::FLOAT32) - output = *(reinterpret_cast(&data[field->offset])); - else - output = (float)(*(reinterpret_cast(&data[field->offset]))); - } - else - output = 0.0; -} -void get_uint16_from_field( - const sensor_msgs::PointField* field, const unsigned char* data, uint16_t& output) -{ - if (field != nullptr) - { - if (field->datatype == sensor_msgs::PointField::UINT16) - output = *(reinterpret_cast(&data[field->offset])); - else if (field->datatype == sensor_msgs::PointField::UINT8) - output = *(reinterpret_cast(&data[field->offset])); - } - else - output = 0; -} -void get_uint32_from_field( - const sensor_msgs::PointField* field, const unsigned char* data, uint32_t& output) -{ - if (field != nullptr) - { - if (field->datatype == sensor_msgs::PointField::UINT32) - output = *(reinterpret_cast(&data[field->offset])); - } - else - output = 0; -} -} // namespace - -std::set mrpt::ros1bridge::extractFields(const sensor_msgs::PointCloud2& msg) -{ - std::set lst; - for (const auto& f : msg.fields) lst.insert(f.name); - return lst; -} - -/** Convert sensor_msgs/PointCloud2 -> mrpt::slam::CSimplePointsMap - * - * \return true on successful conversion, false on any error. - */ -bool mrpt::ros1bridge::fromROS(const sensor_msgs::PointCloud2& msg, CSimplePointsMap& obj) -{ - // Copy point data - unsigned int num_points = msg.width * msg.height; - obj.clear(); - obj.reserve(num_points); - - bool incompatible = false; - const sensor_msgs::PointField *x_field = nullptr, *y_field = nullptr, *z_field = nullptr; - - for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) - { - incompatible |= check_field(msg.fields[i], "x", &x_field); - incompatible |= check_field(msg.fields[i], "y", &y_field); - incompatible |= check_field(msg.fields[i], "z", &z_field); - } - - if (incompatible || (!x_field || !y_field || !z_field)) return false; - - // If not, memcpy each group of contiguous fields separately - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - - float x, y, z; - get_float_from_field(x_field, msg_data, x); - get_float_from_field(y_field, msg_data, y); - get_float_from_field(z_field, msg_data, z); - obj.insertPoint(x, y, z); - } - } - - return true; -} - -bool mrpt::ros1bridge::fromROS(const sensor_msgs::PointCloud2& msg, CPointsMapXYZI& obj) -{ - // Copy point data - unsigned int num_points = msg.width * msg.height; - obj.clear(); - obj.reserve(num_points); - - bool incompatible = false; - const sensor_msgs::PointField *x_field = nullptr, *y_field = nullptr, *z_field = nullptr, - *i_field = nullptr; - - for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) - { - incompatible |= check_field(msg.fields[i], "x", &x_field); - incompatible |= check_field(msg.fields[i], "y", &y_field); - incompatible |= check_field(msg.fields[i], "z", &z_field); - incompatible |= check_field(msg.fields[i], "intensity", &i_field); - } - - if (incompatible || (!x_field || !y_field || !z_field || !i_field)) return false; - - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - - float x, y, z, i; - get_float_from_field(x_field, msg_data, x); - get_float_from_field(y_field, msg_data, y); - get_float_from_field(z_field, msg_data, z); - get_float_from_field(i_field, msg_data, i); - obj.insertPoint(x, y, z); - obj.insertPointField_Intensity(i); - } - } - return true; -} - -bool mrpt::ros1bridge::fromROS(const sensor_msgs::PointCloud2& msg, CPointsMapXYZIRT& obj) -{ - // Copy point data - unsigned int num_points = msg.width * msg.height; - - bool incompatible = false; - const sensor_msgs::PointField *x_field = nullptr, *y_field = nullptr, *z_field = nullptr, - *i_field = nullptr, *r_field = nullptr, *t_field = nullptr; - - for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) - { - incompatible |= check_field(msg.fields[i], "x", &x_field); - incompatible |= check_field(msg.fields[i], "y", &y_field); - incompatible |= check_field(msg.fields[i], "z", &z_field); - incompatible |= check_field(msg.fields[i], "intensity", &i_field); - incompatible |= check_field(msg.fields[i], "ring", &r_field); - - incompatible |= check_field(msg.fields[i], "timestamp", &t_field); - incompatible |= check_field(msg.fields[i], "time", &t_field); - incompatible |= check_field(msg.fields[i], "t", &t_field); - } - - if (incompatible || (!x_field || !y_field || !z_field)) return false; - - obj.resize_XYZIRT(num_points, !!i_field, !!r_field, !!t_field); - - std::optional minTime, maxTime; - - unsigned int idx = 0; - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col, ++idx) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - - float x, y, z; - get_float_from_field(x_field, msg_data, x); - get_float_from_field(y_field, msg_data, y); - get_float_from_field(z_field, msg_data, z); - obj.setPointFast(idx, x, y, z); - - if (i_field) - { - float i; - get_float_from_field(i_field, msg_data, i); - obj.setPointIntensity(idx, i); - } - if (r_field) - { - uint16_t ring_id = 0; - get_uint16_from_field(r_field, msg_data, ring_id); - obj.setPointRing(idx, ring_id); - } - if (t_field) - { - if (t_field->datatype == sensor_msgs::PointField::FLOAT32) - { - float t = 0; - get_float_from_field(t_field, msg_data, t); - obj.setPointTime(idx, t); - } - else - { - uint32_t t = 0; - get_uint32_from_field(t_field, msg_data, t); - - // Convention: - // I only found one case (NTU Viral dataset) using uint32_t for time, - // and times ranged from 0 to ~99822766 = 100,000,000 = 1e8 - // so they seem to be nanoseconds: - obj.setPointTime(idx, t * 1e-9); - } - - const float t = obj.getPointTime(idx); - if (!minTime) - { - minTime = t; - maxTime = t; - } - else - { - mrpt::keep_min(*minTime, t); - mrpt::keep_max(*maxTime, t); - } - } - } - } - - // Force timestamps to be in the range [-T/2,T/2]: - if (minTime && *maxTime > *minTime) - { - const float At = (*maxTime - *minTime) * 0.5f; - for (size_t i = 0; i < obj.size(); i++) - { - obj.setPointTime(i, obj.getPointTime(i) - At); - } - } - - return true; -} - -bool mrpt::ros1bridge::toROS( - const CSimplePointsMap& obj, const std_msgs::Header& msg_header, sensor_msgs::PointCloud2& msg) -{ - msg.header = msg_header; - - // 2D structure of the point cloud. If the cloud is unordered, height is - // 1 and width is the length of the point cloud. - msg.height = 1; - msg.width = obj.size(); - - std::array names = {"x", "y", "z"}; - std::array offsets = {0, sizeof(float) * 1, sizeof(float) * 2}; - - msg.fields.resize(3); - for (size_t i = 0; i < 3; i++) - { - auto& f = msg.fields.at(i); - - f.count = 1; - f.offset = offsets[i]; - f.datatype = sensor_msgs::PointField::FLOAT32; - f.name = names[i]; - } - -#if MRPT_IS_BIG_ENDIAN - msg.is_bigendian = true; -#else - msg.is_bigendian = false; -#endif - - msg.point_step = sizeof(float) * 3; - msg.row_step = msg.width * msg.point_step; - - // data: - msg.data.resize(msg.row_step * msg.height); - - const auto& xs = obj.getPointsBufferRef_x(); - const auto& ys = obj.getPointsBufferRef_y(); - const auto& zs = obj.getPointsBufferRef_z(); - - float* pointDest = reinterpret_cast(msg.data.data()); - for (size_t i = 0; i < xs.size(); i++) - { - *pointDest++ = xs[i]; - *pointDest++ = ys[i]; - *pointDest++ = zs[i]; - } - - return true; -} - -bool mrpt::ros1bridge::toROS( - const CPointsMapXYZI& obj, const std_msgs::Header& msg_header, sensor_msgs::PointCloud2& msg) -{ - msg.header = msg_header; - - // 2D structure of the point cloud. If the cloud is unordered, height is - // 1 and width is the length of the point cloud. - msg.height = 1; - msg.width = obj.size(); - - std::array names = {"x", "y", "z", "intensity"}; - std::array offsets = {0, sizeof(float) * 1, sizeof(float) * 2, sizeof(float) * 3}; - - msg.fields.resize(4); - for (size_t i = 0; i < 4; i++) - { - auto& f = msg.fields.at(i); - - f.count = 1; - f.offset = offsets[i]; - f.datatype = sensor_msgs::PointField::FLOAT32; - f.name = names[i]; - } - -#if MRPT_IS_BIG_ENDIAN - msg.is_bigendian = true; -#else - msg.is_bigendian = false; -#endif - - msg.point_step = sizeof(float) * 4; - msg.row_step = msg.width * msg.point_step; - - // data: - msg.data.resize(msg.row_step * msg.height); - - const auto& xs = obj.getPointsBufferRef_x(); - const auto& ys = obj.getPointsBufferRef_y(); - const auto& zs = obj.getPointsBufferRef_z(); - const auto* Is = obj.getPointsBufferRef_intensity(); - - float* pointDest = reinterpret_cast(msg.data.data()); - for (size_t i = 0; i < xs.size(); i++) - { - *pointDest++ = xs[i]; - *pointDest++ = ys[i]; - *pointDest++ = zs[i]; - *pointDest++ = (*Is)[i]; - } - - return true; -} - -bool mrpt::ros1bridge::toROS( - const CPointsMapXYZIRT& obj, const std_msgs::Header& msg_header, sensor_msgs::PointCloud2& msg) -{ - msg.header = msg_header; - - // 2D structure of the point cloud. If the cloud is unordered, height is - // 1 and width is the length of the point cloud. - msg.height = 1; - msg.width = obj.size(); - - std::vector names = {"x", "y", "z"}; - std::vector offsets = {0, sizeof(float) * 1, sizeof(float) * 2}; - - msg.point_step = sizeof(float) * 3; - - if (obj.hasIntensityField()) - { - ASSERT_EQUAL_(obj.getPointsBufferRef_intensity()->size(), obj.size()); - names.push_back("intensity"); - offsets.push_back(msg.point_step); - msg.point_step += sizeof(float); - } - if (obj.hasTimeField()) - { - ASSERT_EQUAL_(obj.getPointsBufferRef_timestamp()->size(), obj.size()); - names.push_back("time"); - offsets.push_back(msg.point_step); - msg.point_step += sizeof(float); - } - if (obj.hasRingField()) - { - ASSERT_EQUAL_(obj.getPointsBufferRef_ring()->size(), obj.size()); - names.push_back("ring"); - offsets.push_back(msg.point_step); - msg.point_step += sizeof(uint16_t); - } - - msg.fields.resize(names.size()); - for (size_t i = 0; i < names.size(); i++) - { - auto& f = msg.fields.at(i); - - f.count = 1; - f.offset = offsets[i]; - f.datatype = - (names[i] == "ring") ? sensor_msgs::PointField::UINT16 : sensor_msgs::PointField::FLOAT32; - f.name = names[i]; - } - -#if MRPT_IS_BIG_ENDIAN - msg.is_bigendian = true; -#else - msg.is_bigendian = false; -#endif - - msg.row_step = msg.width * msg.point_step; - - // data: - msg.data.resize(msg.row_step * msg.height); - - const auto& xs = obj.getPointsBufferRef_x(); - const auto& ys = obj.getPointsBufferRef_y(); - const auto& zs = obj.getPointsBufferRef_z(); - const auto& Is = *obj.getPointsBufferRef_intensity(); - const auto& Rs = *obj.getPointsBufferRef_ring(); - const auto& Ts = *obj.getPointsBufferRef_timestamp(); - - uint8_t* pointDest = msg.data.data(); - for (size_t i = 0; i < xs.size(); i++) - { - int f = 0; - memcpy(pointDest + offsets[f++], &xs[i], sizeof(float)); - memcpy(pointDest + offsets[f++], &ys[i], sizeof(float)); - memcpy(pointDest + offsets[f++], &zs[i], sizeof(float)); - - if (obj.hasIntensityField()) memcpy(pointDest + offsets[f++], &Is[i], sizeof(float)); - - if (obj.hasTimeField()) memcpy(pointDest + offsets[f++], &Ts[i], sizeof(float)); - - if (obj.hasRingField()) memcpy(pointDest + offsets[f++], &Rs[i], sizeof(uint16_t)); - - pointDest += msg.point_step; - } - - return true; -} - -/** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan */ -bool mrpt::ros1bridge::fromROS( - const sensor_msgs::PointCloud2& msg, - mrpt::obs::CObservationRotatingScan& obj, - const mrpt::poses::CPose3D& sensorPoseOnRobot, - unsigned int num_azimuth_divisions, - float max_intensity) -{ - // Copy point data - obj.timestamp = mrpt::ros1bridge::fromROS(msg.header.stamp); - - bool incompatible = false; - const sensor_msgs::PointField *x_field = nullptr, *y_field = nullptr, *z_field = nullptr, - *i_field = nullptr, *ring_field = nullptr; - - for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) - { - incompatible |= check_field(msg.fields[i], "x", &x_field); - incompatible |= check_field(msg.fields[i], "y", &y_field); - incompatible |= check_field(msg.fields[i], "z", &z_field); - incompatible |= check_field(msg.fields[i], "ring", &ring_field); - check_field(msg.fields[i], "intensity", &i_field); - } - - if (incompatible || (!x_field || !y_field || !z_field || !ring_field)) return false; - - // 1st: go through the scan and find ring count: - uint16_t ring_min = 0, ring_max = 0; - - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - uint16_t ring_id = 0; - get_uint16_from_field(ring_field, msg_data, ring_id); - - mrpt::keep_min(ring_min, ring_id); - mrpt::keep_max(ring_max, ring_id); - } - } - ASSERT_NOT_EQUAL_(ring_min, ring_max); - - obj.rowCount = ring_max - ring_min + 1; - - const bool inputCloudIsOrganized = msg.height != 1; - - if (!num_azimuth_divisions) - { - if (inputCloudIsOrganized) - { - ASSERT_GT_(msg.width, 0U); - num_azimuth_divisions = msg.width; - } - else - { - THROW_EXCEPTION( - "An explicit value for num_azimuth_divisions must be given if " - "the input cloud is not 'organized'"); - } - } - - obj.columnCount = num_azimuth_divisions; - - obj.rangeImage.resize(obj.rowCount, obj.columnCount); - obj.rangeImage.fill(0); - - obj.sensorPose = sensorPoseOnRobot; - - // Default unit: 1cm - if (obj.rangeResolution == 0) obj.rangeResolution = 1e-2; - - if (i_field) - { - obj.intensityImage.resize(obj.rowCount, obj.columnCount); - obj.intensityImage.fill(0); - } - else - obj.intensityImage.resize(0, 0); - - if (inputCloudIsOrganized) - { - obj.organizedPoints.resize(obj.rowCount, obj.columnCount); - } - - // If not, memcpy each group of contiguous fields separately - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - - float x = 0, y = 0, z = 0; - uint16_t ring_id = 0; - get_float_from_field(x_field, msg_data, x); - get_float_from_field(y_field, msg_data, y); - get_float_from_field(z_field, msg_data, z); - get_uint16_from_field(ring_field, msg_data, ring_id); - - const mrpt::math::TPoint3D localPt = {x, y, z}; - - unsigned int az_idx; - if (inputCloudIsOrganized) - { - // "azimuth index" is just the "column": - az_idx = col; - } - else - { - // Recover "azimuth index" from trigonometry: - const double azimuth = std::atan2(localPt.y, localPt.x); - - az_idx = lround((num_azimuth_divisions - 1) * (azimuth + M_PI) / (2 * M_PI)); - ASSERT_LE_(az_idx, num_azimuth_divisions - 1); - } - - // Store in matrix form: - obj.rangeImage(ring_id, az_idx) = lround(localPt.norm() / obj.rangeResolution); - - if (i_field) - { - float intensity = 0; - get_float_from_field(i_field, msg_data, intensity); - obj.intensityImage(ring_id, az_idx) = lround(255 * intensity / max_intensity); - } - - if (inputCloudIsOrganized) obj.organizedPoints(ring_id, az_idx) = localPt; - } - } - - return true; -} diff --git a/libs/ros1bridge/src/pointcloud2_unittest.cpp b/libs/ros1bridge/src/pointcloud2_unittest.cpp deleted file mode 100644 index d0c9288c0d..0000000000 --- a/libs/ros1bridge/src/pointcloud2_unittest.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * test_pose_conversions.cpp - * - * Created on: Mar 15, 2012 - * Author: Pablo Iñigo Blasco - */ - -#include -#include -#include - -#if HAVE_PCL -#include -#include -#include - -TEST(PointCloud2, basicTest) -{ - pcl::PointCloud point_cloud; - - point_cloud.height = 10; - point_cloud.width = 10; - point_cloud.is_dense = true; - - int num_points = point_cloud.height * point_cloud.width; - point_cloud.points.resize(num_points); - - float i_f = 0; - for (int i = 0; i < num_points; i++) - { - pcl::PointXYZ& point = point_cloud.points[i]; - point.x = i_f; - point.y = -i_f; - point.z = -2 * i_f; - i_f += 1.0; - } - - sensor_msgs::PointCloud2 point_cloud2_msg; - pcl::toROSMsg(point_cloud, point_cloud2_msg); - - mrpt::maps::CSimplePointsMap mrpt_pc; - - mrpt::ros1bridge::fromROS(point_cloud2_msg, mrpt_pc); - - i_f = 0; - for (int i = 0; i < num_points; i++) - { - float mrpt_x, mrpt_y, mrpt_z; - mrpt_pc.getPoint(i, mrpt_x, mrpt_y, mrpt_z); - EXPECT_FLOAT_EQ(mrpt_x, i_f); - EXPECT_FLOAT_EQ(mrpt_y, -i_f); - EXPECT_FLOAT_EQ(mrpt_z, -2 * i_f); - - i_f += 1.0; - } -} -#endif // HAVE_PCL - -TEST(PointCloud2, toROS) -{ - mrpt::maps::CSimplePointsMap pc1; - - const size_t num_points = 1000; - pc1.resize(num_points); - - float i_f = 0; - for (size_t i = 0; i < num_points; i++) - { - pc1.setPoint(i, i_f, -i_f, -2 * i_f); - i_f += 1.0; - } - - sensor_msgs::PointCloud2 pc_msg; - std_msgs::Header hdr; - hdr.frame_id = "map"; - bool ok = mrpt::ros1bridge::toROS(pc1, hdr, pc_msg); - ASSERT_(ok); - - EXPECT_EQ(pc_msg.header.frame_id, hdr.frame_id); - - // - mrpt::maps::CSimplePointsMap pc2; - bool ok2 = mrpt::ros1bridge::fromROS(pc_msg, pc2); - ASSERT_(ok2); - - EXPECT_EQ(pc1.size(), pc2.size()); - for (size_t i = 0; i < pc1.size(); i++) - { - mrpt::math::TPoint3D pt1, pt2; - pc1.getPoint(i, pt1); - pc2.getPoint(i, pt2); - EXPECT_TRUE(pt1 == pt2); - } -} diff --git a/libs/ros1bridge/src/pose.cpp b/libs/ros1bridge/src/pose.cpp deleted file mode 100644 index d7345cdb55..0000000000 --- a/libs/ros1bridge/src/pose.cpp +++ /dev/null @@ -1,262 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * pose.cpp - * - * Created on: Mar 14, 2012 - * Author: Pablo Iñigo Blasco - * - * To understand better how this is implemented see the references: - * - http://www.mrpt.org/2D_3D_Geometry - * - * Aug 17, 2019: Refactored into mrpt::ros1bridge library for MRPT 2.0 (JLBC) - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// MRPT -> ROS functions: - -tf2::Matrix3x3 mrpt::ros1bridge::toROS(const mrpt::math::CMatrixDouble33& src) -{ - tf2::Matrix3x3 des; - for (int r = 0; r < 3; r++) - for (int c = 0; c < 3; c++) des[r][c] = src(r, c); - return des; -} - -tf2::Transform mrpt::ros1bridge::toROS_tfTransform(const mrpt::poses::CPose2D& src) -{ - return toROS_tfTransform(mrpt::poses::CPose3D(src)); -} - -geometry_msgs::Pose mrpt::ros1bridge::toROS_Pose(const mrpt::poses::CPose2D& src) -{ - return toROS_Pose(mrpt::poses::CPose3D(src)); -} - -tf2::Transform mrpt::ros1bridge::toROS_tfTransform(const mrpt::math::TPose2D& src) -{ - return toROS_tfTransform(mrpt::poses::CPose3D(mrpt::math::TPose3D(src))); -} - -geometry_msgs::Pose mrpt::ros1bridge::toROS_Pose(const mrpt::math::TPose2D& src) -{ - geometry_msgs::Pose des; - - des.position.x = src.x; - des.position.y = src.y; - des.position.z = 0; - - const double yaw = src.phi; - if (std::abs(yaw) < 1e-10) - { - des.orientation.x = 0.; - des.orientation.y = 0.; - des.orientation.z = .5 * yaw; - des.orientation.w = 1.; - } - else - { - const double s = ::sin(yaw * .5); - const double c = ::cos(yaw * .5); - des.orientation.x = 0.; - des.orientation.y = 0.; - des.orientation.z = s; - des.orientation.w = c; - } - - return des; -} - -tf2::Transform mrpt::ros1bridge::toROS_tfTransform(const mrpt::poses::CPose3D& src) -{ - tf2::Transform des; - des.setBasis(toROS(src.getRotationMatrix())); - des.setOrigin(tf2::Vector3(src.x(), src.y(), src.z())); - return des; -} - -geometry_msgs::Pose mrpt::ros1bridge::toROS_Pose(const mrpt::poses::CPose3D& src) -{ - geometry_msgs::Pose des; - des.position.x = src.x(); - des.position.y = src.y(); - des.position.z = src.z(); - - mrpt::math::CQuaternionDouble q; - src.getAsQuaternion(q); - - des.orientation.x = q.x(); - des.orientation.y = q.y(); - des.orientation.z = q.z(); - des.orientation.w = q.r(); - - return des; -} - -tf2::Transform mrpt::ros1bridge::toROS_tfTransform(const mrpt::math::TPose3D& src) -{ - return toROS_tfTransform(mrpt::poses::CPose3D(src)); -} - -geometry_msgs::Pose mrpt::ros1bridge::toROS_Pose(const mrpt::math::TPose3D& src) -{ - return toROS_Pose(mrpt::poses::CPose3D(src)); -} - -geometry_msgs::PoseWithCovariance mrpt::ros1bridge::toROS_Pose( - const mrpt::poses::CPose3DPDFGaussian& src) -{ - geometry_msgs::PoseWithCovariance des; - des.pose = toROS_Pose(src.mean); - - // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation - // # Row-major representation of the 6x6 covariance matrix - // # The orientation parameters use a fixed-axis representation. - // # In order, the parameters are: - // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about - // Z axis) - // float64[36] covariance - // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a - // transform Jacobian here!" - // JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations. - - // X,Y,Z,YAW,PITCH,ROLL - const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3}; - for (int i = 0; i < 6; i++) - for (int j = 0; j < 6; j++) des.covariance[indxs_map[i] * 6 + indxs_map[j]] = src.cov(i, j); - return des; -} - -geometry_msgs::PoseWithCovariance mrpt::ros1bridge::toROS( - const mrpt::poses::CPose3DPDFGaussianInf& src) -{ - mrpt::poses::CPose3DPDFGaussian src2; - src2.copyFrom(src); - return toROS_Pose(src2); -} - -geometry_msgs::PoseWithCovariance mrpt::ros1bridge::toROS(const mrpt::poses::CPosePDFGaussian& src) -{ - geometry_msgs::PoseWithCovariance des; - - des.pose = toROS_Pose(src.mean); - - // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation - // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a - // transform Jacobian here!" - // JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations. - - // geometry_msgs/PoseWithCovariance msg stores the covariance matrix in - // row-major representation - // Indexes are : - // [ 0 1 2 3 4 5 ] - // [ 6 7 8 9 10 11 ] - // [ 12 13 14 15 16 17 ] - // [ 18 19 20 21 22 23 ] - // [ 24 25 26 27 28 29 ] - // [ 30 31 32 33 34 35 ] - - des.covariance[0] = src.cov(0, 0); - des.covariance[1] = src.cov(0, 1); - des.covariance[5] = src.cov(0, 2); - des.covariance[6] = src.cov(1, 0); - des.covariance[7] = src.cov(1, 1); - des.covariance[11] = src.cov(1, 2); - des.covariance[30] = src.cov(2, 0); - des.covariance[31] = src.cov(2, 1); - des.covariance[35] = src.cov(2, 2); - - return des; -} - -geometry_msgs::PoseWithCovariance mrpt::ros1bridge::toROS( - const mrpt::poses::CPosePDFGaussianInf& src) -{ - mrpt::poses::CPosePDFGaussian src2; - src2.copyFrom(src); - - return toROS(src2); -} - -geometry_msgs::Quaternion mrpt::ros1bridge::toROS(const mrpt::math::CQuaternionDouble& src) -{ - geometry_msgs::Quaternion des; - des.x = src.x(); - des.y = src.y(); - des.z = src.z(); - des.w = src.r(); - return des; -} - -// ROS -> MRPT functions: -mrpt::poses::CPose3D mrpt::ros1bridge::fromROS(const tf2::Transform& src) -{ - mrpt::poses::CPose3D des; - const tf2::Vector3& t = src.getOrigin(); - des.x(t[0]); - des.y(t[1]); - des.z(t[2]); - des.setRotationMatrix(fromROS(src.getBasis())); - return des; -} -mrpt::math::CMatrixDouble33 mrpt::ros1bridge::fromROS(const tf2::Matrix3x3& src) -{ - mrpt::math::CMatrixDouble33 des; - for (int r = 0; r < 3; r++) - for (int c = 0; c < 3; c++) des(r, c) = src[r][c]; - return des; -} - -mrpt::poses::CPose3D mrpt::ros1bridge::fromROS(const geometry_msgs::Pose& src) -{ - const mrpt::math::CQuaternionDouble q( - src.orientation.w, src.orientation.x, src.orientation.y, src.orientation.z); - return mrpt::poses::CPose3D(q, src.position.x, src.position.y, src.position.z); -} - -mrpt::poses::CPose3DPDFGaussian mrpt::ros1bridge::fromROS( - const geometry_msgs::PoseWithCovariance& src) -{ - mrpt::poses::CPose3DPDFGaussian des; - - des.mean = fromROS(src.pose); - - const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3}; - - for (int i = 0; i < 6; i++) - for (int j = 0; j < 6; j++) des.cov(i, j) = src.covariance[indxs_map[i] * 6 + indxs_map[j]]; - - return des; -} - -mrpt::math::CQuaternionDouble mrpt::ros1bridge::fromROS(const geometry_msgs::Quaternion& src) -{ - mrpt::math::CQuaternionDouble des; - des.x(src.x); - des.y(src.y); - des.z(src.z); - des.r(src.w); - return des; -} diff --git a/libs/ros1bridge/src/pose_unittest.cpp b/libs/ros1bridge/src/pose_unittest.cpp deleted file mode 100644 index e6772adb3b..0000000000 --- a/libs/ros1bridge/src/pose_unittest.cpp +++ /dev/null @@ -1,161 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * test_pose.cpp - * - * Created on: Mar 15, 2012 - * Author: Pablo Iñigo Blasco - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; - -TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33) -{ - tf2::Matrix3x3 src(0, 1, 2, 3, 4, 5, 6, 7, 8); - const mrpt::math::CMatrixDouble33 des = mrpt::ros1bridge::fromROS(src); - - for (int r = 0; r < 3; r++) - for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des(r, c), src[r][c]); -} -TEST(PoseConversions, copyCMatrixDouble33ToMatrix3x3) -{ - mrpt::math::CMatrixDouble33 src; - for (int r = 0, i = 0; r < 3; r++) - for (int c = 0; c < 3; c++, i++) src(r, c) = i; - - const tf2::Matrix3x3 des = mrpt::ros1bridge::toROS(src); - - for (int r = 0; r < 3; r++) - for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des[r][c], src(r, c)); -} - -// Declare a test -TEST(PoseConversions, reference_frame_change_with_rotations) -{ - geometry_msgs::PoseWithCovariance ros_msg_original_pose; - - ros_msg_original_pose.pose.position.x = 1; - ros_msg_original_pose.pose.position.y = 0; - ros_msg_original_pose.pose.position.z = 0; - ros_msg_original_pose.pose.orientation.x = 0; - ros_msg_original_pose.pose.orientation.y = 0; - ros_msg_original_pose.pose.orientation.z = 0; - ros_msg_original_pose.pose.orientation.w = 1; - - // to mrpt - mrpt::poses::CPose3DPDFGaussian mrpt_original_pose = - mrpt::ros1bridge::fromROS(ros_msg_original_pose); - - EXPECT_EQ(ros_msg_original_pose.pose.position.x, mrpt_original_pose.mean[0]); - -#if 0 - // to tf - tf::Pose tf_original_pose; - tf::poseMsgToTF(ros_msg_original_pose.pose, tf_original_pose); - - // rotate yaw pi in MRPT - mrpt::poses::CPose3D rotation_mrpt; - double yaw = M_PI / 2.0; - rotation_mrpt.setFromValues(0, 0, 0, yaw, 0, 0); - mrpt::poses::CPose3D mrpt_result = rotation_mrpt + mrpt_original_pose.mean; - EXPECT_NEAR(mrpt_result[1], 1.0, 0.01); - - // rotate yaw pi in TF - tf::Quaternion rotation_tf; - rotation_tf.setRPY(0, 0, yaw); - tf::Pose rotation_pose_tf; - rotation_pose_tf.setIdentity(); - rotation_pose_tf.setRotation(rotation_tf); - tf::Pose tf_result = rotation_pose_tf * tf_original_pose; - EXPECT_NEAR(tf_result.getOrigin()[1], 1.0, 0.01); - - geometry_msgs::Pose mrpt_ros_result; - mrpt_bridge::convert(mrpt_result, mrpt_ros_result); - - EXPECT_NEAR(mrpt_ros_result.position.x, tf_result.getOrigin()[0], 0.01); - EXPECT_NEAR(mrpt_ros_result.position.y, tf_result.getOrigin()[1], 0.01); - EXPECT_NEAR(mrpt_ros_result.position.z, tf_result.getOrigin()[2], 0.01); -#endif -} - -void check_CPose3D_tofrom_ROS(double x, double y, double z, double yaw, double pitch, double roll) -{ - const mrpt::poses::CPose3D p3D(x, y, z, yaw, pitch, roll); - - // Convert MRPT->ROS - geometry_msgs::Pose ros_p3D = mrpt::ros1bridge::toROS_Pose(p3D); - - // Compare ROS quat vs. MRPT quat: - mrpt::math::CQuaternionDouble q; - p3D.getAsQuaternion(q); - - EXPECT_NEAR(ros_p3D.position.x, p3D.x(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.position.y, p3D.y(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.position.z, p3D.z(), 1e-4) << "p: " << p3D << endl; - - EXPECT_NEAR(ros_p3D.orientation.x, q.x(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.orientation.y, q.y(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.orientation.z, q.z(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.orientation.w, q.r(), 1e-4) << "p: " << p3D << endl; - - // Test the other path: ROS->MRPT - mrpt::poses::CPose3D p_bis = mrpt::ros1bridge::fromROS(ros_p3D); - - // p_bis==p3D? - EXPECT_NEAR((p_bis.asVectorVal() - p3D.asVectorVal()).array().abs().maxCoeff(), 0, 1e-4) - << "p_bis: " << p_bis << endl - << "p3D: " << p3D << endl; -} - -// Declare a test -TEST(PoseConversions, check_CPose3D_tofrom_ROS) -{ - using mrpt::DEG2RAD; - using mrpt::RAD2DEG; - using namespace mrpt; // for 0.0_deg - - check_CPose3D_tofrom_ROS(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg); - check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg); - - check_CPose3D_tofrom_ROS(1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg); - check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg); - check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg); - - check_CPose3D_tofrom_ROS(1, 2, 3, -5.0_deg, 15.0_deg, -30.0_deg); -} - -// Declare a test -TEST(PoseConversions, check_CPose2D_to_ROS) -{ - const mrpt::poses::CPose2D p2D(1, 2, 0.56); - - // Convert MRPT->ROS - const geometry_msgs::Pose ros_p2D = mrpt::ros1bridge::toROS_Pose(p2D); - - // Compare vs. 3D pose: - const mrpt::poses::CPose3D p3D = mrpt::poses::CPose3D(p2D); - const mrpt::poses::CPose3D p3D_ros = mrpt::ros1bridge::fromROS(ros_p2D); - - // p3D_ros should equal p3D - EXPECT_NEAR((p3D_ros.asVectorVal() - p3D.asVectorVal()).array().abs().maxCoeff(), 0, 1e-4) - << "p3D_ros: " << p3D_ros << endl - << "p3D: " << p3D << endl; -} diff --git a/libs/ros1bridge/src/range.cpp b/libs/ros1bridge/src/range.cpp deleted file mode 100644 index d9d108b1e5..0000000000 --- a/libs/ros1bridge/src/range.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: range.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#include - -bool mrpt::ros1bridge::fromROS(const sensor_msgs::Range& msg, mrpt::obs::CObservationRange& obj) -{ - obj.minSensorDistance = msg.min_range; - obj.maxSensorDistance = msg.max_range; - obj.sensorConeAperture = msg.field_of_view; - - obj.sensedData.resize(1); - obj.sensedData.at(0).sensedDistance = msg.range; - return true; -} - -bool mrpt::ros1bridge::toROS( - const mrpt::obs::CObservationRange& obj, - const std_msgs::Header& msg_header, - sensor_msgs::Range* msg) -{ - long num_range = obj.sensedData.size(); - - // 1) sensor_msgs::Range:: header - for (long i = 0; i < num_range; i++) msg[i].header = msg_header; - - // 2) sensor_msg::Range parameters - for (long i = 0; i < num_range; i++) - { - msg[i].max_range = obj.maxSensorDistance; - msg[i].min_range = obj.minSensorDistance; - msg[i].field_of_view = obj.sensorConeAperture; - } - - /// following part needs to be double checked, it looks incorrect - /// ROS has single number float for range, MRPT has a list of - /// sensedDistances - for (long i = 0; i < num_range; i++) msg[i].range = obj.sensedData.at(i).sensedDistance; - - /// currently the following are not available in MRPT for corresponding - /// range ROS message NO corresponding value for MRPT radiation_type at - /// http://mrpt.ual.es/reference/devel/_c_observation_range_8h_source.html - // msg.radiation_type - return true; -} - -/// Range ROS message -/* -uint8 ULTRASOUND=0 -uint8 INFRARED=1 -std_msgs/Header header -uint8 radiation_type -float32 field_of_view -float32 min_range -float32 max_range -float32 range -*/ diff --git a/libs/ros1bridge/src/stereo_image.cpp b/libs/ros1bridge/src/stereo_image.cpp deleted file mode 100644 index c2573f6cfa..0000000000 --- a/libs/ros1bridge/src/stereo_image.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: stereo_image.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#if CV_BRIDGE_VERSION <= 0x030400 -#include -#else -#include -#endif - -#include -#include -#include -#include - -using namespace ros; -using namespace sensor_msgs; -using namespace cv; -using namespace cv_bridge; - -bool mrpt::ros1bridge::toROS( - const mrpt::obs::CObservationStereoImages& obj, - const std_msgs::Header& msg_header, - sensor_msgs::Image& left, - sensor_msgs::Image& right, - stereo_msgs::DisparityImage& disparity) -{ - // left image - const Mat& cvImgL = obj.imageLeft.asCvMatRef(); - - cv_bridge::CvImage img_bridge; - img_bridge = CvImage(left.header, sensor_msgs::image_encodings::BGR8, cvImgL); - img_bridge.toImageMsg(left); - left.encoding = "bgr8"; - left.header = msg_header; - left.height = obj.imageLeft.getHeight(); - left.width = obj.imageLeft.getWidth(); - - // right image - const Mat& cvImgR = obj.imageLeft.asCvMatRef(); - - cv_bridge::CvImage img_bridge2; - img_bridge2 = CvImage(right.header, sensor_msgs::image_encodings::BGR8, cvImgR); - img_bridge2.toImageMsg(right); - right.encoding = "bgr8"; - right.header = msg_header; - right.height = obj.imageRight.getHeight(); - right.width = obj.imageRight.getWidth(); - - if (obj.hasImageDisparity) - { - const Mat& cvImgD = obj.imageDisparity.asCvMatRef(); - - cv_bridge::CvImage img_bridge3; - img_bridge3 = CvImage(disparity.header, sensor_msgs::image_encodings::BGR8, cvImgD); - img_bridge3.toImageMsg(disparity.image); - disparity.image.encoding = "bgr8"; - disparity.image.header = msg_header; - disparity.image.height = obj.imageDisparity.getHeight(); - disparity.image.width = obj.imageDisparity.getWidth(); - } - return true; -} - -// -/* -std_msgs/Header header -uint32 height -uint32 width -string encoding -uint8 is_bigendian -uint32 step -uint8[] data - */ diff --git a/libs/ros1bridge/src/time.cpp b/libs/ros1bridge/src/time.cpp deleted file mode 100644 index 7453c9a018..0000000000 --- a/libs/ros1bridge/src/time.cpp +++ /dev/null @@ -1,29 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include - -#include // std::fmod - -mrpt::system::TTimeStamp mrpt::ros1bridge::fromROS(const ros::Time& src) -{ - return mrpt::Clock::fromDouble(src.sec + src.nsec * 1e-9); -} - -ros::Time mrpt::ros1bridge::toROS(const mrpt::system::TTimeStamp& src) -{ - // Convert to "double-version of time_t", then extract integer and - // fractional parts: - const double t = mrpt::Clock::toDouble(src); - ros::Time des; - des.sec = static_cast(t); - des.nsec = static_cast(std::fmod(t, 1.0) * 1e9 + 0.5 /*round*/); - return des; -} diff --git a/libs/ros1bridge/src/time_unittest.cpp b/libs/ros1bridge/src/time_unittest.cpp deleted file mode 100644 index 55b89af9df..0000000000 --- a/libs/ros1bridge/src/time_unittest.cpp +++ /dev/null @@ -1,28 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * test_time.cpp - * - * Created on: July 15, 2014 - * Author: Markus Bader - */ - -#include -#include - -TEST(Time, basicTest) -{ - const auto org_time = mrpt::Clock::now(); - - ros::Time ros_tim = mrpt::ros1bridge::toROS(org_time); - mrpt::system::TTimeStamp mrpt_tim = mrpt::ros1bridge::fromROS(ros_tim); - - EXPECT_NEAR(mrpt::system::timeDifference(org_time, mrpt_tim), .0, 1e-6); -} diff --git a/libs/ros2bridge/CMakeLists.txt b/libs/ros2bridge/CMakeLists.txt deleted file mode 100644 index d64d4b9bc4..0000000000 --- a/libs/ros2bridge/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ - -# Don't declare the library if dependencies are missing: -if(NOT MRPT_ROS_VERSION EQUAL 2) - return() -endif() -if( - NOT cv_bridge_FOUND OR - NOT geometry_msgs_FOUND OR - NOT nav_msgs_FOUND OR - NOT sensor_msgs_FOUND OR - NOT std_msgs_FOUND OR - NOT stereo_msgs_FOUND OR - NOT tf2_FOUND OR - NOT rclcpp_FOUND -) - message(STATUS "mrpt-ros2bridge: Skipping due to missing dependencies") - return() -endif() - -# Additional dependencies for tests only: -if (pcl_conversions_FOUND AND PCL_FOUND) - set_property(GLOBAL PROPERTY mrpt_ros2bridge_UNIT_TEST_EXTRA_DEPS ${pcl_conversions_LIBRARIES} ${PCL_COMMON_LIBRARIES}) - set_property(GLOBAL PROPERTY mrpt_ros2bridge_UNIT_TEST_EXTRA_INCLUDE_DIRS ${pcl_conversions_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS}) - set_property(GLOBAL PROPERTY mrpt_ros2bridge_UNIT_TEST_EXTRA_DEFINITIONS "HAVE_PCL_CONVERSIONS") -endif() - -set(EXTRA_CONFIG_CMDS -"if (NOT TARGET imp_opencv)\n\ - add_library(imp_opencv INTERFACE IMPORTED)\n\ - set_target_properties(imp_opencv\n\ - PROPERTIES\n\ - INTERFACE_INCLUDE_DIRECTORIES \"${OpenCV_INCLUDE_DIRS}\"\n\ - INTERFACE_LINK_LIBRARIES \"${OpenCV_LIBRARIES}\"\n\ - )\n\ -endif()\n" -) - -define_mrpt_lib( - ros2bridge # Lib name - # Dependencies: - mrpt-maps - mrpt-obs - # Other imported targets, - # which will be mapped into find_package() commands - # in the xxx_config.cmake script. - rclcpp # find_package() lib names - nav_msgs - stereo_msgs - sensor_msgs - tf2 - cv_bridge - ) - -if(BUILD_mrpt-ros2bridge) - # Add the required libraries: - ament_target_dependencies(ros2bridge PUBLIC - rclcpp - tf2 - cv_bridge - sensor_msgs - std_msgs - geometry_msgs - stereo_msgs - nav_msgs - ) - - target_compile_definitions(ros2bridge PRIVATE ${ROS_DEFINITIONS}) - target_compile_definitions(ros2bridge PRIVATE IS_MRPT_ROS2BRIDGE) - target_compile_definitions(ros2bridge PRIVATE CV_BRIDGE_VERSION=${cv_bridge_VERSION_HEX}) -endif() diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/gps.h b/libs/ros2bridge/include/mrpt/ros2bridge/gps.h deleted file mode 100644 index 11132e2605..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/gps.h +++ /dev/null @@ -1,53 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: GPS.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#pragma once - -#include - -#include - -/// ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html -/// MRPT message: -/// https://github.com/MRPT/mrpt/blob/master/libs/obs/include/mrpt/obs/CObservationGPS.h - -/** Conversion functions between ROS 1 <-> MRPT types. - * \ingroup mrpt_ros2bridge_grp - */ -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS - * \return true on sucessful conversion, false on any error. - */ -bool fromROS(const sensor_msgs::msg::NavSatFix& msg, mrpt::obs::CObservationGPS& obj); - -/** Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * \return true on sucessful conversion, only if the input observation has a GGA - * message. - */ -bool toROS( - const mrpt::obs::CObservationGPS& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::NavSatFix& msg); - -/** @} */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/image.h b/libs/ros2bridge/include/mrpt/ros2bridge/image.h deleted file mode 100644 index 4838e1d840..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/image.h +++ /dev/null @@ -1,36 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: image.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#pragma once - -#include -#include - -#include // size_t -#include - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** Makes a deep copy of the image data */ -mrpt::img::CImage fromROS(const sensor_msgs::msg::Image& i); - -/** Makes a deep copy of the image data */ -sensor_msgs::msg::Image toROS(const mrpt::img::CImage& i, const std_msgs::msg::Header& msg_header); -/** @} */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/imu.h b/libs/ros2bridge/include/mrpt/ros2bridge/imu.h deleted file mode 100644 index 0d0456ad26..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/imu.h +++ /dev/null @@ -1,53 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: imu.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ -#pragma once - -#include - -#include // size_t -#include -#include -#include - -/// ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html -/// MRPT message: -/// https://github.com/MRPT/mrpt/blob/master/libs/obs/include/mrpt/obs/CObservationIMU.h - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** Convert sensor_msgs/Imu -> mrpt::obs::CObservationIMU - * // STILL NEED TO WRITE CODE FOR COVARIANCE - * \return true on sucessful conversion, false on any error. - */ -bool fromROS(const sensor_msgs::msg::Imu& msg, mrpt::obs::CObservationIMU& obj); - -/** Convert mrpt::obs::CObservationIMU -> sensor_msgs/Imu - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * Since COnservationIMU does not contain covariance terms NEED TO fix those. - * \return true on sucessful conversion, false on any error. - */ -bool toROS( - const mrpt::obs::CObservationIMU& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::Imu& msg); - -/** @} */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/laser_scan.h b/libs/ros2bridge/include/mrpt/ros2bridge/laser_scan.h deleted file mode 100644 index 73dd2ba323..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/laser_scan.h +++ /dev/null @@ -1,53 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include - -#include -#include -#include -#include - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** ROS->MRPT: Takes a sensor_msgs::msg::LaserScan and the relative pose of the - * laser wrt base_link and builds a CObservation2DRangeScan \return true on - * sucessful conversion, false on any error. \sa toROS - */ -bool fromROS( - const sensor_msgs::msg::LaserScan& msg, - const mrpt::poses::CPose3D& pose, - mrpt::obs::CObservation2DRangeScan& obj); - -/** MRPT->ROS: Takes a CObservation2DRangeScan and outputs range data in - * sensor_msgs::msg::LaserScan - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS(const mrpt::obs::CObservation2DRangeScan& obj, sensor_msgs::msg::LaserScan& msg); - -/** MRPT->ROS: Takes a CObservation2DRangeScan and outputs range data in - * sensor_msgs::msg::LaserScan + the relative pose of the laser wrt base_link - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::obs::CObservation2DRangeScan& obj, - sensor_msgs::msg::LaserScan& msg, - geometry_msgs::msg::Pose& pose); - -/** @} */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/map.h b/libs/ros2bridge/include/mrpt/ros2bridge/map.h deleted file mode 100644 index 78d9a7d17f..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/map.h +++ /dev/null @@ -1,120 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include - -#include -#include -#include - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** @name Maps, Occupancy Grid Maps: ROS <-> MRPT - * @{ */ - -/** Methods to convert between ROS msgs and MRPT objects for map datatypes. - * @brief the map class is implemented as singeleton use map::instance - * ()->fromROS ... - */ -class MapHdl -{ - private: -#ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS - int lut_cellmrpt2ros[0x100]; // lookup table for entry convertion -#else - int lut_cellmrpt2ros[0xFFFF]; // lookup table for entry convertion -#endif - int lut_cellros2mrpt[101]; // lookup table for entry convertion - - MapHdl(); - MapHdl(const MapHdl&); - ~MapHdl() = default; - - public: - /** - * @return returns singeleton instance - * @brief it creates a instance with some look up table to speed up the - * conversions - */ - static MapHdl* instance(); - -#ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS - int8_t cellMrpt2Ros(int8_t i) { return lut_cellmrpt2ros[static_cast(i) - INT8_MIN]; } -#else - int16_t cellMrpt2Ros(int16_t i) { return lut_cellmrpt2ros[static_cast(i) - INT16_MIN]; } -#endif - int8_t cellRos2Mrpt(int8_t i) - { - if (i < 0) - { - // unobserved cells: no log-odds information - return 0; - } - ASSERT_LE_(i, 100); - return lut_cellros2mrpt[i]; - } - - /** - * loads a mprt map - * @return true on sucess. - * @param _metric_map - * @param _config_file - * @param _map_file default: map.simplemap - * @param _section_name default: metricMap - * @param _debug default: false - */ - static bool loadMap( - mrpt::maps::CMultiMetricMap& _metric_map, - const mrpt::config::CConfigFileBase& _config_file, - const std::string& _map_file = "map.simplemap", - const std::string& _section_name = "metricMap", - bool _debug = false); -}; - -/** - * converts ros msg to mrpt object - * @return true on sucessful conversion, false on any error. - * @param src - * @param des - */ -bool fromROS(const nav_msgs::msg::OccupancyGrid& src, mrpt::maps::COccupancyGridMap2D& des); - -/** - * converts mrpt object to ros msg and updates the msg header - * @return true on sucessful conversion, false on any error. - * @param src - * @param header - * @param as_costmap If set to true, gridmap cell values will be copied without changes - * (interpreted as int8_t instead of Log-odds) - */ -bool toROS( - const mrpt::maps::COccupancyGridMap2D& src, - nav_msgs::msg::OccupancyGrid& msg, - const std_msgs::msg::Header& header, - bool as_costmap = false); -/** - * converts mrpt object to ros msg - * @return true on sucessful conversion, false on any error. - */ -bool toROS( - const mrpt::maps::COccupancyGridMap2D& src, - nav_msgs::msg::OccupancyGrid& msg, - bool as_costmap = false); - -/** @} - * @} - */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud.h b/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud.h deleted file mode 100644 index 15dede0ba2..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud.h +++ /dev/null @@ -1,50 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include - -#include - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** @name Point clouds: ROS <-> MRPT - * @{ */ - -/** Convert sensor_msgs/PointCloud -> mrpt::maps::CSimplePointsMap - * CSimplePointsMap only contains (x,y,z) data, so - * sensor_msgs::PointCloud::channels are ignored. - * \return true on sucessful conversion, false on any error. - * \sa toROS - */ -bool fromROS(const sensor_msgs::msg::PointCloud& msg, mrpt::maps::CSimplePointsMap& obj); - -/** Convert mrpt::maps::CSimplePointsMap -> sensor_msgs/PointCloud - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * Since CSimplePointsMap only contains (x,y,z) data, - * sensor_msgs::PointCloud::channels will be empty. - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::maps::CSimplePointsMap& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::PointCloud& msg); - -/** @} @} - */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h b/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h deleted file mode 100644 index 38afe6b865..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h +++ /dev/null @@ -1,105 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** @name sensor_msgs::msg::PointCloud2: ROS <-> MRPT - * @{ */ - -/** Convert sensor_msgs/PointCloud2 -> mrpt::slam::CSimplePointsMap - * Only (x,y,z) data is converted. To use the intensity channel, see - * the alternative signatures for CPointsMapXYZI or CPointsMapXYZIRT - * Requires point cloud fields: x,y,z. - * \return true on sucessful conversion, false on any error. - * \sa toROS - */ -bool fromROS(const sensor_msgs::msg::PointCloud2& msg, mrpt::maps::CSimplePointsMap& obj); - -/** \overload For (x,y,z,intensity) channels. - * Requires point cloud fields: x,y,z,intensity - */ -bool fromROS(const sensor_msgs::msg::PointCloud2& msg, mrpt::maps::CPointsMapXYZI& obj); - -/** \overload For (x,y,z,intensity,ring,time) channels. - * Requires point cloud fields: x,y,z,intensity,ring,time - */ -bool fromROS(const sensor_msgs::msg::PointCloud2& msg, mrpt::maps::CPointsMapXYZIRT& obj); - -/** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan. - * Requires point cloud fields: x,y,z,ring[,intensity][,time] - * - * If num_azimuth_divisions=0, it will be taken from the point cloud "width" - * field. - * - * Points are supposed to be given in the sensor frame of reference. - * - */ -bool fromROS( - const sensor_msgs::msg::PointCloud2& m, - mrpt::obs::CObservationRotatingScan& o, - const mrpt::poses::CPose3D& sensorPoseOnRobot, - unsigned int num_azimuth_divisions = 0, - float max_intensity = 1000.0f); - -/** Extract a list of fields found in the point cloud. - * Typically: {"x","y","z","intensity"} - */ -std::set extractFields(const sensor_msgs::msg::PointCloud2& msg); - -/** Convert mrpt::slam::CSimplePointsMap -> sensor_msgs/PointCloud2 - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * Generated sensor_msgs::PointCloud2::channels: `x`, `y`, `z`. - * - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::maps::CSimplePointsMap& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::PointCloud2& msg); - -/** \overload With these fields: `x`, `y`, `z`, `intensity` - * \return true on sucessful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::maps::CPointsMapXYZI& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::PointCloud2& msg); - -/** \overload With these fields: `x`, `y`, `z`, `intensity`, `ring`, `timestamp` - * \return true on successful conversion, false on any error. - * \sa fromROS - */ -bool toROS( - const mrpt::maps::CPointsMapXYZIRT& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::PointCloud2& msg); - -/** @} */ -/** @} */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/pose.h b/libs/ros2bridge/include/mrpt/ros2bridge/pose.h deleted file mode 100644 index fde12eea5f..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/pose.h +++ /dev/null @@ -1,60 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include // size_t -#include -#include - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -tf2::Matrix3x3 toROS(const mrpt::math::CMatrixDouble33& src); - -tf2::Transform toROS_tfTransform(const mrpt::poses::CPose2D& src); -geometry_msgs::msg::Pose toROS_Pose(const mrpt::poses::CPose2D& src); - -tf2::Transform toROS_tfTransform(const mrpt::math::TPose2D& src); -geometry_msgs::msg::Pose toROS_Pose(const mrpt::math::TPose2D& src); - -tf2::Transform toROS_tfTransform(const mrpt::poses::CPose3D& src); -geometry_msgs::msg::Pose toROS_Pose(const mrpt::poses::CPose3D& src); - -tf2::Transform toROS_tfTransform(const mrpt::math::TPose3D& src); -geometry_msgs::msg::Pose toROS_Pose(const mrpt::math::TPose3D& src); - -geometry_msgs::msg::PoseWithCovariance toROS_Pose(const mrpt::poses::CPose3DPDFGaussian& src); - -geometry_msgs::msg::PoseWithCovariance toROS(const mrpt::poses::CPose3DPDFGaussianInf& src); - -geometry_msgs::msg::PoseWithCovariance toROS(const mrpt::poses::CPosePDFGaussian& src); - -geometry_msgs::msg::PoseWithCovariance toROS(const mrpt::poses::CPosePDFGaussianInf& src); - -geometry_msgs::msg::Quaternion toROS(const mrpt::math::CQuaternionDouble& src); - -mrpt::poses::CPose3D fromROS(const tf2::Transform& src); -mrpt::math::CMatrixDouble33 fromROS(const tf2::Matrix3x3& src); -mrpt::poses::CPose3D fromROS(const geometry_msgs::msg::Pose& src); -mrpt::poses::CPose3DPDFGaussian fromROS(const geometry_msgs::msg::PoseWithCovariance& src); -mrpt::math::CQuaternionDouble fromROS(const geometry_msgs::msg::Quaternion& src); - -/** @} */ -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/range.h b/libs/ros2bridge/include/mrpt/ros2bridge/range.h deleted file mode 100644 index a732779146..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/range.h +++ /dev/null @@ -1,51 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: range.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#pragma once - -#include - -#include - -/// ROS message : http://docs.ros.org/api/sensor_msgs/html/msg/Range.html -/// MRPT message: -/// https://github.com/MRPT/mrpt/blob/master/libs/obs/include/mrpt/obs/CObservationRange.h - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** Convert sensor_msgs/Range -> mrpt::obs::CObservationRange - * \return true on sucessful conversion, false on any error. - */ -bool fromROS(const sensor_msgs::msg::Range& msg, mrpt::obs::CObservationRange& obj); - -/** Convert mrpt::obs::CObservationRange -> sensor_msgs/Range - * The user must supply the "msg_header" field to be copied into the output - * message object, since that part does not appear in MRPT classes. - * - * Since COnservation does not contain "radiation_type", - * sensor_msgs::msg::Range::radiation_type will be empty. \return true on - * sucessful conversion, false on any error. - */ -bool toROS( - const mrpt::obs::CObservationRange& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::Range* msg); - -/** @} */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/stereo_image.h b/libs/ros2bridge/include/mrpt/ros2bridge/stereo_image.h deleted file mode 100644 index 815135228e..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/stereo_image.h +++ /dev/null @@ -1,37 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: stereo_image.h - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#pragma once - -#include - -#include -#include - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -bool toROS( - const mrpt::obs::CObservationStereoImages& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::Image& left, - sensor_msgs::msg::Image& right, - stereo_msgs::msg::DisparityImage& disparity); - -/** @} */ - -} // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/time.h b/libs/ros2bridge/include/mrpt/ros2bridge/time.h deleted file mode 100644 index 1c81b1bb47..0000000000 --- a/libs/ros2bridge/include/mrpt/ros2bridge/time.h +++ /dev/null @@ -1,37 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include - -#include - -namespace mrpt::ros2bridge -{ -/** \addtogroup mrpt_ros2bridge_grp - * @{ */ - -/** - * converts ros time to mrpt time - * @param src ros time - * @param des mrpt time - */ -mrpt::system::TTimeStamp fromROS(const rclcpp::Time& src); - -/** - * converts mrpt time to ros time - * @param src ros time - * @param des mrpt time - */ -rclcpp::Time toROS(const mrpt::system::TTimeStamp& src); - -/** @} */ - -}; // namespace mrpt::ros2bridge diff --git a/libs/ros2bridge/src/gps.cpp b/libs/ros2bridge/src/gps.cpp deleted file mode 100644 index 4e68c16f39..0000000000 --- a/libs/ros2bridge/src/gps.cpp +++ /dev/null @@ -1,150 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: gps.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#include -#include - -bool mrpt::ros2bridge::fromROS( - const sensor_msgs::msg::NavSatFix& msg, mrpt::obs::CObservationGPS& obj) -{ - mrpt::obs::gnss::Message_NMEA_GGA gga; - gga.fields.altitude_meters = msg.altitude; - gga.fields.latitude_degrees = msg.latitude; - gga.fields.longitude_degrees = msg.longitude; - - switch (msg.status.status) - { - case -1: - gga.fields.fix_quality = 0; - break; - case 0: - gga.fields.fix_quality = 1; - break; - case 2: - gga.fields.fix_quality = 2; - break; - case 1: - gga.fields.fix_quality = 3; - break; - default: - gga.fields.fix_quality = 0; // never going to execute default - } - obj.setMsg(gga); - - obj.timestamp = mrpt::ros2bridge::fromROS(msg.header.stamp); - - if (msg.position_covariance_type != sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN) - { - auto& cov = obj.covariance_enu.emplace(); - for (int r = 0, i = 0; r < 3; r++) - for (int c = 0; c < 3; c++) cov(r, c) = msg.position_covariance.at(i++); - } - - return true; -} - -bool mrpt::ros2bridge::toROS( - const mrpt::obs::CObservationGPS& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::NavSatFix& msg) -{ - bool valid = false; - - // 1) sensor_msgs::NavSatFix:: header - msg.header = msg_header; - - // 2) other NavSatFix Parameters, the following 3 could be wrong too - - if (obj.hasMsgClass()) - { - const mrpt::obs::gnss::Message_NMEA_GGA& gga = - obj.getMsgByClass(); - msg.altitude = gga.fields.altitude_meters; - msg.latitude = gga.fields.latitude_degrees; - msg.longitude = gga.fields.longitude_degrees; - - /// following parameter assigned as per - /// http://mrpt.ual.es/reference/devel/structmrpt_1_1obs_1_1gnss_1_1_message___n_m_e_a___g_g_a_1_1content__t.html#a33415dc947663d43015605c41b0f66cb - /// http://mrpt.ual.es/reference/devel/gnss__messages__ascii__nmea_8h_source.html - switch (gga.fields.fix_quality) - { - case 0: - msg.status.status = -1; - break; - case 1: - msg.status.status = 0; - break; - case 2: - msg.status.status = 2; - break; - case 3: - msg.status.status = 1; - break; - default: - // this is based on literature available on GPS as the number of - // types in ROS and MRPT are not same - msg.status.status = 0; - } - // this might be incorrect as there is not matching field in mrpt - // message type - msg.status.service = 1; - - valid = true; - } - - // cov: - if (obj.covariance_enu.has_value()) - { - msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN; - - for (int r = 0, i = 0; r < 3; r++) - for (int c = 0; c < 3; c++) msg.position_covariance.at(i++) = (*obj.covariance_enu)(r, c); - } - else - { - msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; - } - - return valid; -} - -/// NavSatFix ROS message -/* -uint8 COVARIANCE_TYPE_UNKNOWN=0 -uint8 COVARIANCE_TYPE_APPROXIMATED=1 -uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2 -uint8 COVARIANCE_TYPE_KNOWN=3 -std_msgs/Header header -sensor_msgs/NavSatStatus status -float64 latitude -float64 longitude -float64 altitude -float64[9] position_covariance -uint8 position_covariance_type -*/ - -/// NavSatStatus ROS message -/* -int8 STATUS_NO_FIX=-1 -int8 STATUS_FIX=0 -int8 STATUS_SBAS_FIX=1 -int8 STATUS_GBAS_FIX=2 -uint16 SERVICE_GPS=1 -uint16 SERVICE_GLONASS=2 -uint16 SERVICE_COMPASS=4 -uint16 SERVICE_GALILEO=8 -int8 status -uint16 service - */ diff --git a/libs/ros2bridge/src/image.cpp b/libs/ros2bridge/src/image.cpp deleted file mode 100644 index f287b08741..0000000000 --- a/libs/ros2bridge/src/image.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: image.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#include - -#if CV_BRIDGE_VERSION < 0x030400 -#include -#else -#include -#endif - -#include -#include - -using namespace mrpt::img; -using namespace sensor_msgs; -using namespace cv; -using namespace cv_bridge; - -mrpt::img::CImage mrpt::ros2bridge::fromROS(const sensor_msgs::msg::Image& i) -{ - return mrpt::img::CImage(cv_bridge::toCvCopy(i, "bgr8").get()->image, mrpt::img::DEEP_COPY); -} - -sensor_msgs::msg::Image mrpt::ros2bridge::toROS( - const mrpt::img::CImage& i, const std_msgs::msg::Header& msg_header) -{ - const Mat& cvImg = i.asCvMatRef(); - - cv_bridge::CvImage img_bridge; - - sensor_msgs::msg::Image msg; - img_bridge = CvImage( - msg.header, - i.isColor() ? sensor_msgs::image_encodings::BGR8 : sensor_msgs::image_encodings::MONO8, - cvImg); - - img_bridge.toImageMsg(msg); - - msg.encoding = i.isColor() ? "bgr8" : "mono8"; - msg.header = msg_header; - msg.height = i.getHeight(); - msg.width = i.getWidth(); - - return msg; -} - -// -/* -std_msgs/Header header -uint32 height -uint32 width -string encoding -uint8 is_bigendian -uint32 step -uint8[] data - */ diff --git a/libs/ros2bridge/src/imu.cpp b/libs/ros2bridge/src/imu.cpp deleted file mode 100644 index 5555cd27b3..0000000000 --- a/libs/ros2bridge/src/imu.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: imu.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#include - -bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::Imu& msg, mrpt::obs::CObservationIMU& obj) -{ - using namespace mrpt::obs; - - if (msg.orientation_covariance.at(0) >= 0) - { - obj.set(IMU_ORI_QUAT_X, msg.orientation.x); - obj.set(IMU_ORI_QUAT_Y, msg.orientation.y); - obj.set(IMU_ORI_QUAT_Z, msg.orientation.z); - obj.set(IMU_ORI_QUAT_W, msg.orientation.w); - } - - if (msg.linear_acceleration_covariance.at(0) >= 0) - { - obj.set(IMU_X_ACC, msg.linear_acceleration.x); - obj.set(IMU_Y_ACC, msg.linear_acceleration.y); - obj.set(IMU_Z_ACC, msg.linear_acceleration.z); - } - - if (msg.angular_velocity_covariance.at(0) >= 0) - { - obj.set(IMU_WX, msg.angular_velocity.x); - obj.set(IMU_WY, msg.angular_velocity.y); - obj.set(IMU_WZ, msg.angular_velocity.z); - } - - return true; -} - -bool mrpt::ros2bridge::toROS( - const mrpt::obs::CObservationIMU& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::Imu& msg) -{ - using namespace mrpt::obs; - - msg.header = msg_header; - - const auto& m = obj.rawMeasurements; - if (obj.has(IMU_ORI_QUAT_X)) - { - msg.orientation.x = m.at(IMU_ORI_QUAT_X); - msg.orientation.y = m.at(IMU_ORI_QUAT_Y); - msg.orientation.z = m.at(IMU_ORI_QUAT_Z); - msg.orientation.w = m.at(IMU_ORI_QUAT_W); - - msg.orientation_covariance.fill(0.01); - } - else - { - msg.orientation_covariance.fill(-1); - } - - if (obj.has(IMU_X_ACC)) - { - msg.linear_acceleration.x = m.at(IMU_X_ACC); - msg.linear_acceleration.y = m.at(IMU_Y_ACC); - msg.linear_acceleration.z = m.at(IMU_Z_ACC); - - msg.linear_acceleration_covariance.fill(0.01); - } - else - { - msg.linear_acceleration_covariance.fill(-1); - } - - if (obj.has(IMU_WX)) - { - msg.angular_velocity.x = m.at(IMU_WX); - msg.angular_velocity.y = m.at(IMU_WY); - msg.angular_velocity.z = m.at(IMU_WZ); - - msg.angular_velocity_covariance.fill(0.01); - } - else - { - msg.angular_velocity_covariance.fill(-1); - } - - return true; -} - -/* -std_msgs/Header header -geometry_msgs/Quaternion orientation -float64[9] orientation_covariance -geometry_msgs/Vector3 angular_velocity -float64[9] angular_velocity_covariance -geometry_msgs/Vector3 linear_acceleration -float64[9] linear_acceleration_covariance - */ diff --git a/libs/ros2bridge/src/laser_scan.cpp b/libs/ros2bridge/src/laser_scan.cpp deleted file mode 100644 index 991cdb602c..0000000000 --- a/libs/ros2bridge/src/laser_scan.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include -#include -#include -#include - -#include -#include - -bool mrpt::ros2bridge::fromROS( - const sensor_msgs::msg::LaserScan& msg, - const mrpt::poses::CPose3D& pose, - mrpt::obs::CObservation2DRangeScan& obj) -{ - obj.timestamp = fromROS(msg.header.stamp); - obj.rightToLeft = true; - obj.sensorLabel = msg.header.frame_id; - obj.aperture = msg.angle_max - msg.angle_min; - obj.maxRange = msg.range_max; - obj.sensorPose = pose; - - ASSERT_(msg.ranges.size() > 1); - - const size_t N = msg.ranges.size(); - const double ang_step = obj.aperture / (N - 1); - const double fov05 = 0.5 * obj.aperture; - const double inv_ang_step = (N - 1) / obj.aperture; - - obj.resizeScan(N); - for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++) - { - // ROS indices go from msg.angle_min to msg.angle_max, while - // in MRPT they go from -FOV/2 to +FOV/2. - int i_ros = inv_ang_step * (-fov05 - msg.angle_min + ang_step * i_mrpt); - if (i_ros < 0) - i_ros += N; - else if (i_ros >= (int)N) - i_ros -= N; // wrap around 2PI... - - // set the scan - const float r = msg.ranges[i_ros]; - obj.setScanRange(i_mrpt, r); - - // set the validity of the scan - const auto ri = obj.getScanRange(i_mrpt); - const bool r_valid = ((ri < (msg.range_max * 0.99)) && (ri > msg.range_min)); - obj.setScanRangeValidity(i_mrpt, r_valid); - } - - return true; -} - -bool mrpt::ros2bridge::toROS( - const mrpt::obs::CObservation2DRangeScan& obj, sensor_msgs::msg::LaserScan& msg) -{ - const size_t nRays = obj.getScanSize(); - if (!nRays) return false; - - msg.angle_min = -0.5f * obj.aperture; - msg.angle_max = 0.5f * obj.aperture; - msg.angle_increment = obj.aperture / (obj.getScanSize() - 1); - - // setting the following values to zero solves a rviz visualization problem - msg.time_increment = 0.0; // 1./30.; // Anything better? - msg.scan_time = 0.0; // msg.time_increment; // idem? - - msg.range_min = 0.02f; - msg.range_max = obj.maxRange; - - msg.ranges.resize(nRays); - for (size_t i = 0; i < nRays; i++) msg.ranges[i] = obj.getScanRange(i); - - // Set header data: - msg.header.stamp = toROS(obj.timestamp); - msg.header.frame_id = obj.sensorLabel; - - return true; -} - -bool mrpt::ros2bridge::toROS( - const mrpt::obs::CObservation2DRangeScan& obj, - sensor_msgs::msg::LaserScan& msg, - geometry_msgs::msg::Pose& pose) -{ - toROS(obj, msg); - pose = toROS_Pose(obj.sensorPose); - return true; -} diff --git a/libs/ros2bridge/src/map.cpp b/libs/ros2bridge/src/map.cpp deleted file mode 100644 index edd8bb475f..0000000000 --- a/libs/ros2bridge/src/map.cpp +++ /dev/null @@ -1,219 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include // for fileExists() -#include // for lowerCase() -#include - -#include - -using namespace mrpt::config; -using namespace mrpt::io; -using mrpt::maps::CLogOddsGridMapLUT; -using mrpt::maps::CMultiMetricMap; -using mrpt::maps::COccupancyGridMap2D; -using mrpt::maps::CSimpleMap; - -#ifndef INT8_MAX // avoid duplicated #define's -#define INT8_MAX 0x7f -#define INT8_MIN (-INT8_MAX - 1) -#define INT16_MAX 0x7fff -#define INT16_MIN (-INT16_MAX - 1) -#endif // INT8_MAX - -using namespace mrpt::ros2bridge; - -MapHdl::MapHdl() -{ - // MRPT -> ROS LUT: - CLogOddsGridMapLUT table; - -#ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS - const int i_min = INT8_MIN, i_max = INT8_MAX; -#else - const int i_min = INT16_MIN, i_max = INT16_MAX; -#endif - - for (int i = i_min; i <= i_max; i++) - { - int8_t ros_val; - if (i == 0) - { - // Unknown cell (no evidence data): - ros_val = -1; - } - else - { - float p = 1.0 - table.l2p(i); - ros_val = round(p * 100.); - // printf("- cell -> ros = %4i -> %4i, p=%4.3f\n", i, ros_val, p); - } - - lut_cellmrpt2ros[static_cast(i) - i_min] = ros_val; - } - - // ROS -> MRPT: [0,100] -> - for (int i = 0; i <= 100; i++) - { - const float p = 1.0 - (i / 100.0); - lut_cellros2mrpt[i] = table.p2l(p); - - // printf("- ros->cell=%4i->%4i p=%4.3f\n",i, lut_cellros2mrpt[i], p); - } -} -MapHdl* MapHdl::instance() -{ - static MapHdl m; // singeleton instance - return &m; -} - -bool mrpt::ros2bridge::fromROS(const nav_msgs::msg::OccupancyGrid& src, COccupancyGridMap2D& des) -{ - MRPT_START - if ((src.info.origin.orientation.x != 0) || (src.info.origin.orientation.y != 0) || - (src.info.origin.orientation.z != 0) || (src.info.origin.orientation.w != 1)) - { - std::cerr << "[fromROS] Rotated maps are not supported!\n"; - return false; - } - float xmin = src.info.origin.position.x; - float ymin = src.info.origin.position.y; - float xmax = xmin + src.info.width * src.info.resolution; - float ymax = ymin + src.info.height * src.info.resolution; - - des.setSize(xmin, xmax, ymin, ymax, src.info.resolution); - auto inst = MapHdl::instance(); - for (unsigned int h = 0; h < src.info.height; h++) - { - COccupancyGridMap2D::cellType* pDes = des.getRow(h); - const int8_t* pSrc = &src.data[h * src.info.width]; - for (unsigned int w = 0; w < src.info.width; w++) *pDes++ = inst->cellRos2Mrpt(*pSrc++); - } - return true; - MRPT_END -} -bool mrpt::ros2bridge::toROS( - const COccupancyGridMap2D& src, - nav_msgs::msg::OccupancyGrid& des, - const std_msgs::msg::Header& header, - bool as_costmap) -{ - des.header = header; - return toROS(src, des, as_costmap); -} - -bool mrpt::ros2bridge::toROS( - const COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& des, bool as_costmap) -{ - des.info.width = src.getSizeX(); - des.info.height = src.getSizeY(); - des.info.resolution = src.getResolution(); - - des.info.origin.position.x = src.getXMin(); - des.info.origin.position.y = src.getYMin(); - des.info.origin.position.z = 0; - - des.info.origin.orientation.x = 0; - des.info.origin.orientation.y = 0; - des.info.origin.orientation.z = 0; - des.info.origin.orientation.w = 1; - - // I hope the data is always aligned - des.data.resize(des.info.width * des.info.height); - for (unsigned int h = 0; h < des.info.height; h++) - { - const COccupancyGridMap2D::cellType* pSrc = src.getRow(h); - ASSERT_(pSrc); - int8_t* pDes = &des.data[h * des.info.width]; - for (unsigned int w = 0; w < des.info.width; w++) - { - if (as_costmap) - *pDes++ = static_cast(*pSrc++); - else - *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++); - } - } - return true; -} - -bool MapHdl::loadMap( - CMultiMetricMap& _metric_map, - const CConfigFileBase& _config_file, - const std::string& _map_file, - const std::string& _section_name, - bool _debug) -{ - using namespace mrpt::maps; - - TSetOfMetricMapInitializers mapInitializers; - mapInitializers.loadFromConfigFile(_config_file, _section_name); - - CSimpleMap simpleMap; - - // Load the set of metric maps to consider in the experiments: - _metric_map.setListOfMaps(mapInitializers); - if (_debug) mapInitializers.dumpToConsole(); - - if (_debug) printf("%s, _map_file.size() = %zu\n", _map_file.c_str(), _map_file.size()); - // Load the map (if any): - if (_map_file.size() < 3) - { - if (_debug) printf("No mrpt map file!\n"); - return false; - } - else - { - ASSERT_(mrpt::system::fileExists(_map_file)); - - // Detect file extension: - std::string mapExt = mrpt::system::lowerCase( - mrpt::system::extractFileExtension(_map_file, true)); // Ignore possible .gz extensions - - if (!mapExt.compare("simplemap")) - { - // It's a ".simplemap": - if (_debug) printf("Loading '.simplemap' file..."); - CFileGZInputStream f(_map_file); - mrpt::serialization::archiveFrom(f) >> simpleMap; - - ASSERTMSG_(simpleMap.size() > 0, "Simplemap was aparently loaded OK, but it is empty!"); - - // Build metric map: - if (_debug) printf("Building metric map(s) from '.simplemap'..."); - _metric_map.loadFromSimpleMap(simpleMap); - if (_debug) printf("Ok\n"); - } - else if (!mapExt.compare("gridmap")) - { - // It's a ".gridmap": - if (_debug) printf("Loading gridmap from '.gridmap'..."); - ASSERTMSG_( - _metric_map.countMapsByClass() == 1, - "Error: Trying to load a gridmap into a multi-metric map " - "requires 1 gridmap member."); - CFileGZInputStream fm(_map_file); - mrpt::serialization::archiveFrom(fm) >> (*_metric_map.mapByClass()); - if (_debug) printf("Ok\n"); - } - else - { - THROW_EXCEPTION(mrpt::format("Map file has unknown extension: '%s'", mapExt.c_str())); - return false; - } - } - return true; -} diff --git a/libs/ros2bridge/src/map_unittest.cpp b/libs/ros2bridge/src/map_unittest.cpp deleted file mode 100644 index d81bd796e8..0000000000 --- a/libs/ros2bridge/src/map_unittest.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * test_map.cpp - * - * Created on: July 21, 2014 - * Author: Markus Bader - */ - -#include -#include -#include - -#include - -using mrpt::maps::COccupancyGridMap2D; - -void getEmptyRosMsg(nav_msgs::msg::OccupancyGrid& msg) -{ - msg.info.width = 300; - msg.info.height = 500; - msg.info.resolution = 0.1; - msg.info.origin.position.x = rand() % 10 - 5; - msg.info.origin.position.y = rand() % 10 - 5; - msg.info.origin.position.z = 0; - - msg.info.origin.orientation.x = 0; - msg.info.origin.orientation.y = 0; - msg.info.origin.orientation.z = 0; - msg.info.origin.orientation.w = 1; - - msg.data.resize(msg.info.width * msg.info.height, -1); -} - -TEST(Map, basicTestHeader) -{ - nav_msgs::msg::OccupancyGrid srcRos; - COccupancyGridMap2D desMrpt; - - getEmptyRosMsg(srcRos); - - srcRos.info.origin.orientation.x = 0; // fix rotation - EXPECT_TRUE(mrpt::ros2bridge::fromROS(srcRos, desMrpt)); - - EXPECT_EQ(srcRos.info.width, desMrpt.getSizeX()); - EXPECT_EQ(srcRos.info.height, desMrpt.getSizeY()); - EXPECT_EQ(srcRos.info.resolution, desMrpt.getResolution()); - for (uint32_t h = 0; h < srcRos.info.width; h++) - { - for (uint32_t w = 0; w < srcRos.info.width; w++) - { - EXPECT_EQ(desMrpt.getPos(w, h), 0.5); // all -1 entreis should map to 0.5 - } - } -} - -TEST(Map, check_ros2mrpt_and_back) -{ - nav_msgs::msg::OccupancyGrid srcRos; - COccupancyGridMap2D desMrpt; - nav_msgs::msg::OccupancyGrid desRos; - - // Test empty gridmap: - getEmptyRosMsg(srcRos); - - ASSERT_TRUE(mrpt::ros2bridge::fromROS(srcRos, desMrpt)); - ASSERT_TRUE(mrpt::ros2bridge::toROS(desMrpt, desRos, desRos.header)); - // all -1 entries should map back to -1 - for (uint32_t h = 0; h < srcRos.info.width; h++) - for (uint32_t w = 0; w < srcRos.info.width; w++) - EXPECT_EQ(desRos.data[h * srcRos.info.width + h], -1); - - // Test gridmap with values: 0 to 100 - for (int i = 0; i <= 100; i++) - { - // 50 is mid-gray -> unknown = -1 in ROS - srcRos.data[i] = (std::abs(i - 50) <= 2) ? -1 : i; - } - - EXPECT_TRUE(mrpt::ros2bridge::fromROS(srcRos, desMrpt)); - EXPECT_TRUE(mrpt::ros2bridge::toROS(desMrpt, desRos, desRos.header)); - - for (int i = 0; i <= 100; i++) - { - /*printf( - "%4i, %4.3f = %4.3f,%4i\n", srcRos.data[i], 1.0f - 0.01f * i, - desMrpt.getCell(i, 0), desRos.data[i]); */ - EXPECT_NEAR(1.0f - 0.01f * i, desMrpt.getCell(i, 0), 0.03f) << "ros to mprt" - << "i=" << i; - EXPECT_NEAR(srcRos.data[i], desRos.data[i], 1) << "ros to mprt to ros"; - } -} diff --git a/libs/ros2bridge/src/point_cloud.cpp b/libs/ros2bridge/src/point_cloud.cpp deleted file mode 100644 index c74bdec9f0..0000000000 --- a/libs/ros2bridge/src/point_cloud.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include - -using namespace mrpt::maps; - -bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud& msg, CSimplePointsMap& obj) -{ - const size_t N = msg.points.size(); - - obj.clear(); - obj.reserve(N); - for (size_t i = 0; i < N; i++) obj.insertPoint(msg.points[i].x, msg.points[i].y, msg.points[i].z); - - return true; -} - -bool mrpt::ros2bridge::toROS( - const CSimplePointsMap& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::PointCloud& msg) -{ - // 1) sensor_msgs::PointCloud:: header - msg.header = msg_header; - - // 2) sensor_msgs::PointCloud:: points - const size_t N = obj.size(); - msg.points.resize(N); - for (size_t i = 0; i < N; i++) - { - geometry_msgs::msg::Point32& pt = msg.points[i]; - obj.getPoint(i, pt.x, pt.y, pt.z); - } - - // 3) sensor_msgs::PointCloud:: channels - msg.channels.clear(); - - return true; -} diff --git a/libs/ros2bridge/src/point_cloud2.cpp b/libs/ros2bridge/src/point_cloud2.cpp deleted file mode 100644 index 4f2e906d9c..0000000000 --- a/libs/ros2bridge/src/point_cloud2.cpp +++ /dev/null @@ -1,617 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include -#include -#include -#include - -#include -#include -// -#include // MRPT_IS_BIG_ENDIAN - -#include - -using namespace mrpt::maps; - -namespace -{ -bool check_field( - const sensor_msgs::msg::PointField& input_field, - std::string check_name, - const sensor_msgs::msg::PointField** output) -{ - bool coherence_error = false; - if (input_field.name == check_name) - { - if (input_field.datatype != sensor_msgs::msg::PointField::FLOAT32 && - input_field.datatype != sensor_msgs::msg::PointField::FLOAT64 && - input_field.datatype != sensor_msgs::msg::PointField::UINT16 && - input_field.datatype != sensor_msgs::msg::PointField::UINT32 && - input_field.datatype != sensor_msgs::msg::PointField::UINT8) - { - *output = nullptr; - coherence_error = true; - } - else - { - *output = &input_field; - } - } - return coherence_error; -} - -void get_float_from_field( - const sensor_msgs::msg::PointField* field, const unsigned char* data, float& output) -{ - if (field != nullptr) - { - if (field->datatype == sensor_msgs::msg::PointField::FLOAT32) - output = *(reinterpret_cast(&data[field->offset])); - else if (field->datatype == sensor_msgs::msg::PointField::FLOAT64) - output = static_cast(*(reinterpret_cast(&data[field->offset]))); - } - else - output = 0.0; -} - -void get_double_from_field( - const sensor_msgs::msg::PointField* field, const unsigned char* data, double& output) -{ - if (field != nullptr) - { - if (field->datatype == sensor_msgs::msg::PointField::FLOAT32) - output = static_cast(*(reinterpret_cast(&data[field->offset]))); - else if (field->datatype == sensor_msgs::msg::PointField::FLOAT64) - output = *(reinterpret_cast(&data[field->offset])); - } - else - output = 0.0; -} - -void get_uint16_from_field( - const sensor_msgs::msg::PointField* field, const unsigned char* data, uint16_t& output) -{ - if (field != nullptr) - { - if (field->datatype == sensor_msgs::msg::PointField::UINT16) - output = *(reinterpret_cast(&data[field->offset])); - else if (field->datatype == sensor_msgs::msg::PointField::UINT8) - output = *(reinterpret_cast(&data[field->offset])); - } - else - output = 0; -} -void get_uint32_from_field( - const sensor_msgs::msg::PointField* field, const unsigned char* data, uint32_t& output) -{ - if (field != nullptr) - { - if (field->datatype == sensor_msgs::msg::PointField::UINT32) - output = *(reinterpret_cast(&data[field->offset])); - } - else - output = 0; -} -} // namespace - -std::set mrpt::ros2bridge::extractFields(const sensor_msgs::msg::PointCloud2& msg) -{ - std::set lst; - for (const auto& f : msg.fields) lst.insert(f.name); - return lst; -} - -/** Convert sensor_msgs/PointCloud2 -> mrpt::slam::CSimplePointsMap - * - * \return true on successful conversion, false on any error. - */ -bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CSimplePointsMap& obj) -{ - // Copy point data - unsigned int num_points = msg.width * msg.height; - obj.clear(); - obj.reserve(num_points); - - bool incompatible = false; - const sensor_msgs::msg::PointField *x_field = nullptr, *y_field = nullptr, *z_field = nullptr; - - for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) - { - incompatible |= check_field(msg.fields[i], "x", &x_field); - incompatible |= check_field(msg.fields[i], "y", &y_field); - incompatible |= check_field(msg.fields[i], "z", &z_field); - } - - if (incompatible || (!x_field || !y_field || !z_field)) return false; - - // If not, memcpy each group of contiguous fields separately - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - - float x = 0, y = 0, z = 0; - get_float_from_field(x_field, msg_data, x); - get_float_from_field(y_field, msg_data, y); - get_float_from_field(z_field, msg_data, z); - obj.insertPoint(x, y, z); - } - } - - return true; -} - -bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CPointsMapXYZI& obj) -{ - // Copy point data - unsigned int num_points = msg.width * msg.height; - obj.clear(); - obj.reserve(num_points); - - bool incompatible = false; - const sensor_msgs::msg::PointField *x_field = nullptr, *y_field = nullptr, *z_field = nullptr, - *i_field = nullptr; - - for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) - { - incompatible |= check_field(msg.fields[i], "x", &x_field); - incompatible |= check_field(msg.fields[i], "y", &y_field); - incompatible |= check_field(msg.fields[i], "z", &z_field); - incompatible |= check_field(msg.fields[i], "intensity", &i_field); - } - - if (incompatible || (!x_field || !y_field || !z_field || !i_field)) return false; - - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - - float x = 0, y = 0, z = 0, i = 0; - get_float_from_field(x_field, msg_data, x); - get_float_from_field(y_field, msg_data, y); - get_float_from_field(z_field, msg_data, z); - get_float_from_field(i_field, msg_data, i); - obj.insertPoint(x, y, z); - obj.insertPointField_Intensity(i); - } - } - return true; -} - -bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CPointsMapXYZIRT& obj) -{ - // Copy point data - unsigned int num_points = msg.width * msg.height; - - bool incompatible = false; - const sensor_msgs::msg::PointField *x_field = nullptr, *y_field = nullptr, *z_field = nullptr, - *i_field = nullptr, *r_field = nullptr, *t_field = nullptr; - - for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) - { - incompatible |= check_field(msg.fields[i], "x", &x_field); - incompatible |= check_field(msg.fields[i], "y", &y_field); - incompatible |= check_field(msg.fields[i], "z", &z_field); - incompatible |= check_field(msg.fields[i], "intensity", &i_field); - incompatible |= check_field(msg.fields[i], "ring", &r_field); - - incompatible |= check_field(msg.fields[i], "timestamp", &t_field); - incompatible |= check_field(msg.fields[i], "time", &t_field); - incompatible |= check_field(msg.fields[i], "t", &t_field); - } - - if (incompatible || (!x_field || !y_field || !z_field)) return false; - - obj.resize_XYZIRT(num_points, !!i_field, !!r_field, !!t_field); - - unsigned int idx = 0; - std::optional baseTimeStamp; - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col, ++idx) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - - float x = 0, y = 0, z = 0; - get_float_from_field(x_field, msg_data, x); - get_float_from_field(y_field, msg_data, y); - get_float_from_field(z_field, msg_data, z); - obj.setPointFast(idx, x, y, z); - - if (i_field) - { - float i = 0; - get_float_from_field(i_field, msg_data, i); - obj.setPointIntensity(idx, i); - } - if (r_field) - { - uint16_t ring_id = 0; - get_uint16_from_field(r_field, msg_data, ring_id); - obj.setPointRing(idx, ring_id); - } - if (t_field) - { - double t = 0; - - if (t_field->datatype == sensor_msgs::msg::PointField::FLOAT32 || - t_field->datatype == sensor_msgs::msg::PointField::FLOAT64) - { - get_double_from_field(t_field, msg_data, t); - } - else - { - uint32_t tim = 0; - - get_uint32_from_field(t_field, msg_data, tim); - - // Convention: they seem to be nanoseconds: - t = tim * 1e-9; - } - - // If the sensor uses absolute timestamp, convert them to relative - // since otherwise precision is lost in the double->float conversion: - if (std::abs(t) > 5.0) - { - // It looks like absolute timestamps, convert to relative: - if (!baseTimeStamp) baseTimeStamp = t; - obj.setPointTime(idx, static_cast(t - *baseTimeStamp)); - } - else - { - // It looks relative timestamps: - obj.setPointTime(idx, static_cast(t)); - } - } - } - } - return true; -} - -bool mrpt::ros2bridge::toROS( - const CSimplePointsMap& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::PointCloud2& msg) -{ - msg.header = msg_header; - - // 2D structure of the point cloud. If the cloud is unordered, height is - // 1 and width is the length of the point cloud. - msg.height = 1; - msg.width = obj.size(); - - std::array names = {"x", "y", "z"}; - std::array offsets = {0, sizeof(float) * 1, sizeof(float) * 2}; - - msg.fields.resize(3); - for (size_t i = 0; i < 3; i++) - { - auto& f = msg.fields.at(i); - - f.count = 1; - f.offset = offsets[i]; - f.datatype = sensor_msgs::msg::PointField::FLOAT32; - f.name = names[i]; - } - -#if MRPT_IS_BIG_ENDIAN - msg.is_bigendian = true; -#else - msg.is_bigendian = false; -#endif - - msg.point_step = sizeof(float) * 3; - msg.row_step = msg.width * msg.point_step; - - // data: - msg.data.resize(msg.row_step * msg.height); - - const auto& xs = obj.getPointsBufferRef_x(); - const auto& ys = obj.getPointsBufferRef_y(); - const auto& zs = obj.getPointsBufferRef_z(); - - float* pointDest = reinterpret_cast(msg.data.data()); - for (size_t i = 0; i < xs.size(); i++) - { - *pointDest++ = xs[i]; - *pointDest++ = ys[i]; - *pointDest++ = zs[i]; - } - - return true; -} - -bool mrpt::ros2bridge::toROS( - const CPointsMapXYZI& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::PointCloud2& msg) -{ - msg.header = msg_header; - - // 2D structure of the point cloud. If the cloud is unordered, height is - // 1 and width is the length of the point cloud. - msg.height = 1; - msg.width = obj.size(); - - std::array names = {"x", "y", "z", "intensity"}; - std::array offsets = {0, sizeof(float) * 1, sizeof(float) * 2, sizeof(float) * 3}; - - msg.fields.resize(4); - for (size_t i = 0; i < 4; i++) - { - auto& f = msg.fields.at(i); - - f.count = 1; - f.offset = offsets[i]; - f.datatype = sensor_msgs::msg::PointField::FLOAT32; - f.name = names[i]; - } - -#if MRPT_IS_BIG_ENDIAN - msg.is_bigendian = true; -#else - msg.is_bigendian = false; -#endif - - msg.point_step = sizeof(float) * 4; - msg.row_step = msg.width * msg.point_step; - - // data: - msg.data.resize(msg.row_step * msg.height); - - const auto& xs = obj.getPointsBufferRef_x(); - const auto& ys = obj.getPointsBufferRef_y(); - const auto& zs = obj.getPointsBufferRef_z(); - const auto* Is = obj.getPointsBufferRef_intensity(); - - float* pointDest = reinterpret_cast(msg.data.data()); - for (size_t i = 0; i < xs.size(); i++) - { - *pointDest++ = xs[i]; - *pointDest++ = ys[i]; - *pointDest++ = zs[i]; - *pointDest++ = (*Is)[i]; - } - - return true; -} - -bool mrpt::ros2bridge::toROS( - const CPointsMapXYZIRT& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::PointCloud2& msg) -{ - msg.header = msg_header; - - // 2D structure of the point cloud. If the cloud is unordered, height is - // 1 and width is the length of the point cloud. - msg.height = 1; - msg.width = obj.size(); - - std::vector names = {"x", "y", "z"}; - std::vector offsets = {0, sizeof(float) * 1, sizeof(float) * 2}; - - msg.point_step = sizeof(float) * 3; - - if (obj.hasIntensityField()) - { - ASSERT_EQUAL_(obj.getPointsBufferRef_intensity()->size(), obj.size()); - names.push_back("intensity"); - offsets.push_back(msg.point_step); - msg.point_step += sizeof(float); - } - if (obj.hasTimeField()) - { - ASSERT_EQUAL_(obj.getPointsBufferRef_timestamp()->size(), obj.size()); - names.push_back("time"); - offsets.push_back(msg.point_step); - msg.point_step += sizeof(float); - } - if (obj.hasRingField()) - { - ASSERT_EQUAL_(obj.getPointsBufferRef_ring()->size(), obj.size()); - names.push_back("ring"); - offsets.push_back(msg.point_step); - msg.point_step += sizeof(uint16_t); - } - - msg.fields.resize(names.size()); - for (size_t i = 0; i < names.size(); i++) - { - auto& f = msg.fields.at(i); - - f.count = 1; - f.offset = offsets[i]; - f.datatype = (names[i] == "ring") ? sensor_msgs::msg::PointField::UINT16 - : sensor_msgs::msg::PointField::FLOAT32; - f.name = names[i]; - } - -#if MRPT_IS_BIG_ENDIAN - msg.is_bigendian = true; -#else - msg.is_bigendian = false; -#endif - - msg.row_step = msg.width * msg.point_step; - - // data: - msg.data.resize(msg.row_step * msg.height); - - const auto& xs = obj.getPointsBufferRef_x(); - const auto& ys = obj.getPointsBufferRef_y(); - const auto& zs = obj.getPointsBufferRef_z(); - const auto& Is = *obj.getPointsBufferRef_intensity(); - const auto& Rs = *obj.getPointsBufferRef_ring(); - const auto& Ts = *obj.getPointsBufferRef_timestamp(); - - uint8_t* pointDest = msg.data.data(); - for (size_t i = 0; i < xs.size(); i++) - { - int f = 0; - memcpy(pointDest + offsets[f++], &xs[i], sizeof(float)); - memcpy(pointDest + offsets[f++], &ys[i], sizeof(float)); - memcpy(pointDest + offsets[f++], &zs[i], sizeof(float)); - - if (obj.hasIntensityField()) memcpy(pointDest + offsets[f++], &Is[i], sizeof(float)); - - if (obj.hasTimeField()) memcpy(pointDest + offsets[f++], &Ts[i], sizeof(float)); - - if (obj.hasRingField()) memcpy(pointDest + offsets[f++], &Rs[i], sizeof(uint16_t)); - - pointDest += msg.point_step; - } - - return true; -} - -/** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan */ -bool mrpt::ros2bridge::fromROS( - const sensor_msgs::msg::PointCloud2& msg, - mrpt::obs::CObservationRotatingScan& obj, - const mrpt::poses::CPose3D& sensorPoseOnRobot, - unsigned int num_azimuth_divisions, - float max_intensity) -{ - // Copy point data - obj.timestamp = mrpt::ros2bridge::fromROS(msg.header.stamp); - obj.originalReceivedTimestamp = obj.timestamp; - - bool incompatible = false; - const sensor_msgs::msg::PointField *x_field = nullptr, *y_field = nullptr, *z_field = nullptr, - *i_field = nullptr, *ring_field = nullptr; - - for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) - { - incompatible |= check_field(msg.fields[i], "x", &x_field); - incompatible |= check_field(msg.fields[i], "y", &y_field); - incompatible |= check_field(msg.fields[i], "z", &z_field); - incompatible |= check_field(msg.fields[i], "ring", &ring_field); - check_field(msg.fields[i], "intensity", &i_field); - } - - if (incompatible || (!x_field || !y_field || !z_field || !ring_field)) return false; - - // 1st: go through the scan and find ring count: - uint16_t ring_min = 0, ring_max = 0; - - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - uint16_t ring_id = 0; - get_uint16_from_field(ring_field, msg_data, ring_id); - - mrpt::keep_min(ring_min, ring_id); - mrpt::keep_max(ring_max, ring_id); - } - } - ASSERT_NOT_EQUAL_(ring_min, ring_max); - - obj.rowCount = ring_max - ring_min + 1; - - const bool inputCloudIsOrganized = msg.height != 1; - - if (!num_azimuth_divisions) - { - if (inputCloudIsOrganized) - { - ASSERT_GT_(msg.width, 0U); - num_azimuth_divisions = msg.width; - } - else - { - THROW_EXCEPTION( - "An explicit value for num_azimuth_divisions must be given if " - "the input cloud is not 'organized'"); - } - } - - obj.columnCount = num_azimuth_divisions; - - obj.rangeImage.resize(obj.rowCount, obj.columnCount); - obj.rangeImage.fill(0); - - obj.sensorPose = sensorPoseOnRobot; - - // Default unit: 1cm - if (obj.rangeResolution == 0) obj.rangeResolution = 1e-2; - - if (i_field) - { - obj.intensityImage.resize(obj.rowCount, obj.columnCount); - obj.intensityImage.fill(0); - } - else - obj.intensityImage.resize(0, 0); - - if (inputCloudIsOrganized) - { - obj.organizedPoints.resize(obj.rowCount, obj.columnCount); - } - - // If not, memcpy each group of contiguous fields separately - for (unsigned int row = 0; row < msg.height; ++row) - { - const unsigned char* row_data = &msg.data[row * msg.row_step]; - for (uint32_t col = 0; col < msg.width; ++col) - { - const unsigned char* msg_data = row_data + col * msg.point_step; - - float x = 0, y = 0, z = 0; - uint16_t ring_id = 0; - get_float_from_field(x_field, msg_data, x); - get_float_from_field(y_field, msg_data, y); - get_float_from_field(z_field, msg_data, z); - get_uint16_from_field(ring_field, msg_data, ring_id); - - const mrpt::math::TPoint3D localPt = {x, y, z}; - - unsigned int az_idx; - if (inputCloudIsOrganized) - { - // "azimuth index" is just the "column": - az_idx = col; - } - else - { - // Recover "azimuth index" from trigonometry: - const double azimuth = std::atan2(localPt.y, localPt.x); - - az_idx = lround((num_azimuth_divisions - 1) * (azimuth + M_PI) / (2 * M_PI)); - ASSERT_LE_(az_idx, num_azimuth_divisions - 1); - } - - // Store in matrix form: - obj.rangeImage(ring_id, az_idx) = lround(localPt.norm() / obj.rangeResolution); - - if (i_field) - { - float intensity = 0; - get_float_from_field(i_field, msg_data, intensity); - obj.intensityImage(ring_id, az_idx) = lround(255 * intensity / max_intensity); - } - - if (inputCloudIsOrganized) obj.organizedPoints(ring_id, az_idx) = localPt; - } - } - - return true; -} diff --git a/libs/ros2bridge/src/pointcloud2_unittest.cpp b/libs/ros2bridge/src/pointcloud2_unittest.cpp deleted file mode 100644 index a832bfc187..0000000000 --- a/libs/ros2bridge/src/pointcloud2_unittest.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * test_pose_conversions.cpp - * - * Created on: Mar 15, 2012 - * Author: Pablo Iñigo Blasco - */ - -#include -#include -#include - -#if HAVE_PCL_CONVERSIONS -#include -#include -#include -#include - -TEST(PointCloud2, basicTest) -{ - pcl::PointCloud point_cloud; - - point_cloud.height = 10; - point_cloud.width = 10; - point_cloud.is_dense = true; - - int num_points = point_cloud.height * point_cloud.width; - point_cloud.points.resize(num_points); - - float i_f = 0; - for (int i = 0; i < num_points; i++) - { - pcl::PointXYZ& point = point_cloud.points[i]; - point.x = i_f; - point.y = -i_f; - point.z = -2 * i_f; - i_f += 1.0; - } - - // pcl_conversions: - sensor_msgs::msg::PointCloud2 point_cloud2_msg; - toROSMsg(point_cloud, point_cloud2_msg); - - mrpt::maps::CSimplePointsMap mrpt_pc; - mrpt::ros2bridge::fromROS(point_cloud2_msg, mrpt_pc); - - i_f = 0; - for (int i = 0; i < num_points; i++) - { - float mrpt_x, mrpt_y, mrpt_z; - mrpt_pc.getPoint(i, mrpt_x, mrpt_y, mrpt_z); - EXPECT_FLOAT_EQ(mrpt_x, i_f); - EXPECT_FLOAT_EQ(mrpt_y, -i_f); - EXPECT_FLOAT_EQ(mrpt_z, -2 * i_f); - - i_f += 1.0; - } -} -#endif // HAVE_PCL - -TEST(PointCloud2, toROS) -{ - mrpt::maps::CSimplePointsMap pc1; - - const size_t num_points = 1000; - pc1.resize(num_points); - - float i_f = 0; - for (size_t i = 0; i < num_points; i++) - { - pc1.setPoint(i, i_f, -i_f, -2 * i_f); - i_f += 1.0; - } - - sensor_msgs::msg::PointCloud2 pc_msg; - std_msgs::msg::Header hdr; - hdr.frame_id = "map"; - bool ok = mrpt::ros2bridge::toROS(pc1, hdr, pc_msg); - ASSERT_(ok); - - EXPECT_EQ(pc_msg.header.frame_id, hdr.frame_id); - - // - mrpt::maps::CSimplePointsMap pc2; - bool ok2 = mrpt::ros2bridge::fromROS(pc_msg, pc2); - ASSERT_(ok2); - - EXPECT_EQ(pc1.size(), pc2.size()); - for (size_t i = 0; i < pc1.size(); i++) - { - mrpt::math::TPoint3D pt1, pt2; - pc1.getPoint(i, pt1); - pc2.getPoint(i, pt2); - EXPECT_TRUE(pt1 == pt2); - } -} diff --git a/libs/ros2bridge/src/pose.cpp b/libs/ros2bridge/src/pose.cpp deleted file mode 100644 index a034c28407..0000000000 --- a/libs/ros2bridge/src/pose.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * pose.cpp - * - * Created on: Mar 14, 2012 - * Author: Pablo Iñigo Blasco - * - * To understand better how this is implemented see the references: - * - http://www.mrpt.org/2D_3D_Geometry - * - * Aug 17, 2019: Refactored into mrpt::ros2bridge library for MRPT 2.0 (JLBC) - * - */ - -#if !defined(IS_MRPT_ROS1BRIDGE) -// ROS1 -#include -#include -#include -#else -// ROS2 -#include -#include -#include -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// MRPT -> ROS functions: -tf2::Matrix3x3 mrpt::ros2bridge::toROS(const mrpt::math::CMatrixDouble33& src) -{ - tf2::Matrix3x3 des; - for (int r = 0; r < 3; r++) - for (int c = 0; c < 3; c++) des[r][c] = src(r, c); - return des; -} - -tf2::Transform mrpt::ros2bridge::toROS_tfTransform(const mrpt::poses::CPose2D& src) -{ - return toROS_tfTransform(mrpt::poses::CPose3D(src)); -} - -geometry_msgs::msg::Pose mrpt::ros2bridge::toROS_Pose(const mrpt::poses::CPose2D& src) -{ - return toROS_Pose(mrpt::poses::CPose3D(src)); -} - -tf2::Transform mrpt::ros2bridge::toROS_tfTransform(const mrpt::math::TPose2D& src) -{ - return toROS_tfTransform(mrpt::poses::CPose3D(mrpt::math::TPose3D(src))); -} - -geometry_msgs::msg::Pose mrpt::ros2bridge::toROS_Pose(const mrpt::math::TPose2D& src) -{ - geometry_msgs::msg::Pose des; - - des.position.x = src.x; - des.position.y = src.y; - des.position.z = 0; - - const double yaw = src.phi; - if (std::abs(yaw) < 1e-10) - { - des.orientation.x = 0.; - des.orientation.y = 0.; - des.orientation.z = .5 * yaw; - des.orientation.w = 1.; - } - else - { - const double s = ::sin(yaw * .5); - const double c = ::cos(yaw * .5); - des.orientation.x = 0.; - des.orientation.y = 0.; - des.orientation.z = s; - des.orientation.w = c; - } - - return des; -} - -tf2::Transform mrpt::ros2bridge::toROS_tfTransform(const mrpt::poses::CPose3D& src) -{ - tf2::Transform des; - des.setBasis(toROS(src.getRotationMatrix())); - des.setOrigin(tf2::Vector3(src.x(), src.y(), src.z())); - return des; -} - -geometry_msgs::msg::Pose mrpt::ros2bridge::toROS_Pose(const mrpt::poses::CPose3D& src) -{ - geometry_msgs::msg::Pose des; - des.position.x = src.x(); - des.position.y = src.y(); - des.position.z = src.z(); - - mrpt::math::CQuaternionDouble q; - src.getAsQuaternion(q); - - des.orientation.x = q.x(); - des.orientation.y = q.y(); - des.orientation.z = q.z(); - des.orientation.w = q.r(); - - return des; -} - -tf2::Transform mrpt::ros2bridge::toROS_tfTransform(const mrpt::math::TPose3D& src) -{ - return toROS_tfTransform(mrpt::poses::CPose3D(src)); -} - -geometry_msgs::msg::Pose mrpt::ros2bridge::toROS_Pose(const mrpt::math::TPose3D& src) -{ - return toROS_Pose(mrpt::poses::CPose3D(src)); -} - -geometry_msgs::msg::PoseWithCovariance mrpt::ros2bridge::toROS_Pose( - const mrpt::poses::CPose3DPDFGaussian& src) -{ - geometry_msgs::msg::PoseWithCovariance des; - des.pose = toROS_Pose(src.mean); - - // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation - // # Row-major representation of the 6x6 covariance matrix - // # The orientation parameters use a fixed-axis representation. - // # In order, the parameters are: - // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about - // Z axis) - // float64[36] covariance - // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a - // transform Jacobian here!" - // JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations. - - // X,Y,Z,YAW,PITCH,ROLL - const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3}; - for (int i = 0; i < 6; i++) - for (int j = 0; j < 6; j++) des.covariance[indxs_map[i] * 6 + indxs_map[j]] = src.cov(i, j); - return des; -} - -geometry_msgs::msg::PoseWithCovariance mrpt::ros2bridge::toROS( - const mrpt::poses::CPose3DPDFGaussianInf& src) -{ - mrpt::poses::CPose3DPDFGaussian src2; - src2.copyFrom(src); - return toROS_Pose(src2); -} - -geometry_msgs::msg::PoseWithCovariance mrpt::ros2bridge::toROS( - const mrpt::poses::CPosePDFGaussian& src) -{ - geometry_msgs::msg::PoseWithCovariance des; - - des.pose = toROS_Pose(src.mean); - - // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation - // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a - // transform Jacobian here!" - // JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations. - - // geometry_msgs/PoseWithCovariance msg stores the covariance matrix in - // row-major representation - // Indexes are : - // [ 0 1 2 3 4 5 ] - // [ 6 7 8 9 10 11 ] - // [ 12 13 14 15 16 17 ] - // [ 18 19 20 21 22 23 ] - // [ 24 25 26 27 28 29 ] - // [ 30 31 32 33 34 35 ] - - des.covariance[0] = src.cov(0, 0); - des.covariance[1] = src.cov(0, 1); - des.covariance[5] = src.cov(0, 2); - des.covariance[6] = src.cov(1, 0); - des.covariance[7] = src.cov(1, 1); - des.covariance[11] = src.cov(1, 2); - des.covariance[30] = src.cov(2, 0); - des.covariance[31] = src.cov(2, 1); - des.covariance[35] = src.cov(2, 2); - - return des; -} - -geometry_msgs::msg::PoseWithCovariance mrpt::ros2bridge::toROS( - const mrpt::poses::CPosePDFGaussianInf& src) -{ - mrpt::poses::CPosePDFGaussian src2; - src2.copyFrom(src); - - return toROS(src2); -} - -geometry_msgs::msg::Quaternion mrpt::ros2bridge::toROS(const mrpt::math::CQuaternionDouble& src) -{ - geometry_msgs::msg::Quaternion des; - des.x = src.x(); - des.y = src.y(); - des.z = src.z(); - des.w = src.r(); - return des; -} - -// ROS -> MRPT functions: -mrpt::poses::CPose3D mrpt::ros2bridge::fromROS(const tf2::Transform& src) -{ - mrpt::poses::CPose3D des; - const tf2::Vector3& t = src.getOrigin(); - des.x(t[0]); - des.y(t[1]); - des.z(t[2]); - des.setRotationMatrix(fromROS(src.getBasis())); - return des; -} -mrpt::math::CMatrixDouble33 mrpt::ros2bridge::fromROS(const tf2::Matrix3x3& src) -{ - mrpt::math::CMatrixDouble33 des; - for (int r = 0; r < 3; r++) - for (int c = 0; c < 3; c++) des(r, c) = src[r][c]; - return des; -} - -mrpt::poses::CPose3D mrpt::ros2bridge::fromROS(const geometry_msgs::msg::Pose& src) -{ - const mrpt::math::CQuaternionDouble q( - src.orientation.w, src.orientation.x, src.orientation.y, src.orientation.z); - return mrpt::poses::CPose3D(q, src.position.x, src.position.y, src.position.z); -} - -mrpt::poses::CPose3DPDFGaussian mrpt::ros2bridge::fromROS( - const geometry_msgs::msg::PoseWithCovariance& src) -{ - mrpt::poses::CPose3DPDFGaussian des; - - des.mean = fromROS(src.pose); - - const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3}; - - for (int i = 0; i < 6; i++) - for (int j = 0; j < 6; j++) des.cov(i, j) = src.covariance[indxs_map[i] * 6 + indxs_map[j]]; - - return des; -} - -mrpt::math::CQuaternionDouble mrpt::ros2bridge::fromROS(const geometry_msgs::msg::Quaternion& src) -{ - mrpt::math::CQuaternionDouble des; - des.x(src.x); - des.y(src.y); - des.z(src.z); - des.r(src.w); - return des; -} diff --git a/libs/ros2bridge/src/pose_unittest.cpp b/libs/ros2bridge/src/pose_unittest.cpp deleted file mode 100644 index a2ac491428..0000000000 --- a/libs/ros2bridge/src/pose_unittest.cpp +++ /dev/null @@ -1,161 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * test_pose.cpp - * - * Created on: Mar 15, 2012 - * Author: Pablo Iñigo Blasco - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -using namespace std; - -TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33) -{ - tf2::Matrix3x3 src(0, 1, 2, 3, 4, 5, 6, 7, 8); - const mrpt::math::CMatrixDouble33 des = mrpt::ros2bridge::fromROS(src); - - for (int r = 0; r < 3; r++) - for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des(r, c), src[r][c]); -} -TEST(PoseConversions, copyCMatrixDouble33ToMatrix3x3) -{ - mrpt::math::CMatrixDouble33 src; - for (int r = 0, i = 0; r < 3; r++) - for (int c = 0; c < 3; c++, i++) src(r, c) = i; - - const tf2::Matrix3x3 des = mrpt::ros2bridge::toROS(src); - - for (int r = 0; r < 3; r++) - for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des[r][c], src(r, c)); -} - -// Declare a test -TEST(PoseConversions, reference_frame_change_with_rotations) -{ - geometry_msgs::msg::PoseWithCovariance ros_msg_original_pose; - - ros_msg_original_pose.pose.position.x = 1; - ros_msg_original_pose.pose.position.y = 0; - ros_msg_original_pose.pose.position.z = 0; - ros_msg_original_pose.pose.orientation.x = 0; - ros_msg_original_pose.pose.orientation.y = 0; - ros_msg_original_pose.pose.orientation.z = 0; - ros_msg_original_pose.pose.orientation.w = 1; - - // to mrpt - mrpt::poses::CPose3DPDFGaussian mrpt_original_pose = - mrpt::ros2bridge::fromROS(ros_msg_original_pose); - - EXPECT_EQ(ros_msg_original_pose.pose.position.x, mrpt_original_pose.mean[0]); - -#if 0 - // to tf - tf::Pose tf_original_pose; - tf::poseMsgToTF(ros_msg_original_pose.pose, tf_original_pose); - - // rotate yaw pi in MRPT - mrpt::poses::CPose3D rotation_mrpt; - double yaw = M_PI / 2.0; - rotation_mrpt.setFromValues(0, 0, 0, yaw, 0, 0); - mrpt::poses::CPose3D mrpt_result = rotation_mrpt + mrpt_original_pose.mean; - EXPECT_NEAR(mrpt_result[1], 1.0, 0.01); - - // rotate yaw pi in TF - tf::Quaternion rotation_tf; - rotation_tf.setRPY(0, 0, yaw); - tf::Pose rotation_pose_tf; - rotation_pose_tf.setIdentity(); - rotation_pose_tf.setRotation(rotation_tf); - tf::Pose tf_result = rotation_pose_tf * tf_original_pose; - EXPECT_NEAR(tf_result.getOrigin()[1], 1.0, 0.01); - - geometry_msgs::msg::Pose mrpt_ros_result; - mrpt_bridge::convert(mrpt_result, mrpt_ros_result); - - EXPECT_NEAR(mrpt_ros_result.position.x, tf_result.getOrigin()[0], 0.01); - EXPECT_NEAR(mrpt_ros_result.position.y, tf_result.getOrigin()[1], 0.01); - EXPECT_NEAR(mrpt_ros_result.position.z, tf_result.getOrigin()[2], 0.01); -#endif -} - -void check_CPose3D_tofrom_ROS(double x, double y, double z, double yaw, double pitch, double roll) -{ - const mrpt::poses::CPose3D p3D(x, y, z, yaw, pitch, roll); - - // Convert MRPT->ROS - geometry_msgs::msg::Pose ros_p3D = mrpt::ros2bridge::toROS_Pose(p3D); - - // Compare ROS quat vs. MRPT quat: - mrpt::math::CQuaternionDouble q; - p3D.getAsQuaternion(q); - - EXPECT_NEAR(ros_p3D.position.x, p3D.x(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.position.y, p3D.y(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.position.z, p3D.z(), 1e-4) << "p: " << p3D << endl; - - EXPECT_NEAR(ros_p3D.orientation.x, q.x(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.orientation.y, q.y(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.orientation.z, q.z(), 1e-4) << "p: " << p3D << endl; - EXPECT_NEAR(ros_p3D.orientation.w, q.r(), 1e-4) << "p: " << p3D << endl; - - // Test the other path: ROS->MRPT - mrpt::poses::CPose3D p_bis = mrpt::ros2bridge::fromROS(ros_p3D); - - // p_bis==p3D? - EXPECT_NEAR((p_bis.asVectorVal() - p3D.asVectorVal()).array().abs().maxCoeff(), 0, 1e-4) - << "p_bis: " << p_bis << endl - << "p3D: " << p3D << endl; -} - -// Declare a test -TEST(PoseConversions, check_CPose3D_tofrom_ROS) -{ - using mrpt::DEG2RAD; - using mrpt::RAD2DEG; - using namespace mrpt; // for 0.0_deg - - check_CPose3D_tofrom_ROS(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg); - check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg); - - check_CPose3D_tofrom_ROS(1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg); - check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg); - check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg); - - check_CPose3D_tofrom_ROS(1, 2, 3, -5.0_deg, 15.0_deg, -30.0_deg); -} - -// Declare a test -TEST(PoseConversions, check_CPose2D_to_ROS) -{ - const mrpt::poses::CPose2D p2D(1, 2, 0.56); - - // Convert MRPT->ROS - const geometry_msgs::msg::Pose ros_p2D = mrpt::ros2bridge::toROS_Pose(p2D); - - // Compare vs. 3D pose: - const mrpt::poses::CPose3D p3D = mrpt::poses::CPose3D(p2D); - const mrpt::poses::CPose3D p3D_ros = mrpt::ros2bridge::fromROS(ros_p2D); - - // p3D_ros should equal p3D - EXPECT_NEAR((p3D_ros.asVectorVal() - p3D.asVectorVal()).array().abs().maxCoeff(), 0, 1e-4) - << "p3D_ros: " << p3D_ros << endl - << "p3D: " << p3D << endl; -} diff --git a/libs/ros2bridge/src/range.cpp b/libs/ros2bridge/src/range.cpp deleted file mode 100644 index 269f671b34..0000000000 --- a/libs/ros2bridge/src/range.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: range.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#include - -#include - -template -struct has_variance : std::false_type -{ -}; - -template -struct has_variance> : std::true_type -{ -}; - -// for "if constexpr" to work (avoid build errors if field does not exist) -// it must be inside a template: -template -void fromROS_variance(const MSG_T& msg, mrpt::obs::CObservationRange& obj) -{ - if constexpr (has_variance::value) - { - obj.sensedData.at(0).sensorNoiseStdDeviation = std::sqrt(msg.variance); - } -} -template -void toROS_variance(MSG_T& msg, const mrpt::obs::CObservationRange::TMeasurement& m) -{ - if constexpr (has_variance::value) - { - msg.variance = mrpt::square(m.sensorNoiseStdDeviation); - } -} - -bool mrpt::ros2bridge::fromROS( - const sensor_msgs::msg::Range& msg, mrpt::obs::CObservationRange& obj) -{ - obj.minSensorDistance = msg.min_range; - obj.maxSensorDistance = msg.max_range; - obj.sensorConeAperture = msg.field_of_view; - - obj.sensedData.resize(1); - obj.sensedData.at(0).sensedDistance = msg.range; - - // See: https://github.com/MRPT/mrpt/issues/1270 - fromROS_variance(msg, obj); - - return true; -} - -bool mrpt::ros2bridge::toROS( - const mrpt::obs::CObservationRange& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::Range* msg) -{ - const auto num_range = obj.sensedData.size(); - - // 1) sensor_msgs::msg::Range:: header - for (size_t i = 0; i < num_range; i++) msg[i].header = msg_header; - - // 2) sensor_msg::Range parameters - for (size_t i = 0; i < num_range; i++) - { - msg[i].max_range = obj.maxSensorDistance; - msg[i].min_range = obj.minSensorDistance; - msg[i].field_of_view = obj.sensorConeAperture; - - // See: https://github.com/MRPT/mrpt/issues/1270 - toROS_variance(msg[i], obj.sensedData[i]); - } - - /// following part needs to be double checked, it looks incorrect - /// ROS has single number float for range, MRPT has a list of - /// sensedDistances - for (size_t i = 0; i < num_range; i++) msg[i].range = obj.sensedData.at(i).sensedDistance; - - /// currently the following are not available in MRPT for corresponding - /// range ROS message NO corresponding value for MRPT radiation_type at - /// http://mrpt.ual.es/reference/devel/_c_observation_range_8h_source.html - // msg.radiation_type - return true; -} - -/// Range ROS message -/* -uint8 ULTRASOUND=0 -uint8 INFRARED=1 -std_msgs/Header header -uint8 radiation_type -float32 field_of_view -float32 min_range -float32 max_range -float32 range -*/ diff --git a/libs/ros2bridge/src/stereo_image.cpp b/libs/ros2bridge/src/stereo_image.cpp deleted file mode 100644 index f8d5b1472f..0000000000 --- a/libs/ros2bridge/src/stereo_image.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/*--------------------------------------------------------------- - APPLICATION: mrpt_ros bridge - FILE: stereo_image.cpp - AUTHOR: Raghavender Sahdev - ---------------------------------------------------------------*/ - -#include -#include - -#if CV_BRIDGE_VERSION < 0x030400 -#include -#else -#include -#endif - -#include -#include - -using namespace sensor_msgs; -using namespace cv; -using namespace cv_bridge; - -bool mrpt::ros2bridge::toROS( - const mrpt::obs::CObservationStereoImages& obj, - const std_msgs::msg::Header& msg_header, - sensor_msgs::msg::Image& left, - sensor_msgs::msg::Image& right, - stereo_msgs::msg::DisparityImage& disparity) -{ - // left image - const Mat& cvImgL = obj.imageLeft.asCvMatRef(); - - cv_bridge::CvImage img_bridge; - img_bridge = CvImage(left.header, sensor_msgs::image_encodings::BGR8, cvImgL); - img_bridge.toImageMsg(left); - left.encoding = "bgr8"; - left.header = msg_header; - left.height = obj.imageLeft.getHeight(); - left.width = obj.imageLeft.getWidth(); - - // right image - const Mat& cvImgR = obj.imageLeft.asCvMatRef(); - - cv_bridge::CvImage img_bridge2; - img_bridge2 = CvImage(right.header, sensor_msgs::image_encodings::BGR8, cvImgR); - img_bridge2.toImageMsg(right); - right.encoding = "bgr8"; - right.header = msg_header; - right.height = obj.imageRight.getHeight(); - right.width = obj.imageRight.getWidth(); - - if (obj.hasImageDisparity) - { - const Mat& cvImgD = obj.imageDisparity.asCvMatRef(); - - cv_bridge::CvImage img_bridge3; - img_bridge3 = CvImage(disparity.header, sensor_msgs::image_encodings::BGR8, cvImgD); - img_bridge3.toImageMsg(disparity.image); - disparity.image.encoding = "bgr8"; - disparity.image.header = msg_header; - disparity.image.height = obj.imageDisparity.getHeight(); - disparity.image.width = obj.imageDisparity.getWidth(); - } - return true; -} - -// -/* -std_msgs/Header header -uint32 height -uint32 width -string encoding -uint8 is_bigendian -uint32 step -uint8[] data - */ diff --git a/libs/ros2bridge/src/time.cpp b/libs/ros2bridge/src/time.cpp deleted file mode 100644 index 81b3e4d619..0000000000 --- a/libs/ros2bridge/src/time.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include -#include - -#include // std::fmod - -mrpt::system::TTimeStamp mrpt::ros2bridge::fromROS(const rclcpp::Time& src) -{ - return mrpt::Clock::fromDouble(src.seconds()); -} - -rclcpp::Time mrpt::ros2bridge::toROS(const mrpt::system::TTimeStamp& src) -{ - // Convert to "double-version of time_t", then extract integer and - // fractional parts: - const double t = mrpt::Clock::toDouble(src); - return rclcpp::Time( - // seconds: - static_cast(t), - // nanoseconds: - static_cast(std::fmod(t, 1.0) * 1e9 + 0.5 /*round*/)); -} diff --git a/libs/ros2bridge/src/time_unittest.cpp b/libs/ros2bridge/src/time_unittest.cpp deleted file mode 100644 index c2f58123e7..0000000000 --- a/libs/ros2bridge/src/time_unittest.cpp +++ /dev/null @@ -1,28 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -/* - * test_time.cpp - * - * Created on: July 15, 2014 - * Author: Markus Bader - */ - -#include -#include - -TEST(Time, basicTest) -{ - const auto org_time = mrpt::Clock::now(); - - rclcpp::Time ros_tim = mrpt::ros2bridge::toROS(org_time); - mrpt::system::TTimeStamp mrpt_tim = mrpt::ros2bridge::fromROS(ros_tim); - - EXPECT_NEAR(mrpt::system::timeDifference(org_time, mrpt_tim), .0, 1e-6); -} diff --git a/parse-files/config.h.in b/parse-files/config.h.in index 3eaacdb248..9b639cfb2d 100644 --- a/parse-files/config.h.in +++ b/parse-files/config.h.in @@ -114,8 +114,6 @@ ${CMAKE_MRPT_BUILD_SHARED_LIB} #define MRPT_HAS_TBB ${CMAKE_MRPT_HAS_TBB} -#cmakedefine MRPT_ROS_VERSION ${MRPT_ROS_VERSION} - /** These two values are detected in Eigen when building MRPT, so we have the same settings given the user-provided flags */ #define MRPT_MAX_ALIGN_BYTES ${EIGEN_MAX_ALIGN_BYTES}