Merge branch 'develop'

apps/carmen2rawlog/carmen2rawlog_main.cpp

@@ -57,6 +57,10 @@ TCLAP::ValueArg<int> arg_gz_level(
 	"z", "compress-level", "Output GZ-compress level (optional)", false, 5,
 	"0: none, 1-9: min-max", cmd);
 
+TCLAP::ValueArg<double> arg_obs_period(
+	"p", "period", "Observation recording period in seconds (optional)", false,
+	0.1, "period [s]", cmd);
+
 // Declarations:
 #define VERBOSE_COUT                                                           \
 	if (verbose) cout << "[carmen2rawlog] "
@@ -110,6 +114,8 @@ int main(int argc, char** argv)
 		vector<CObservation::Ptr> importedObservations;
 		map<TTimeStamp, TPose2D> groundTruthPoses;	// If found...
 		unsigned int nSavedObs = 0;
+		const double dt = arg_obs_period.getValue();
+		double tim = mrpt::Clock::nowDouble();
 
 		const mrpt::system::TTimeStamp base_timestamp = mrpt::system::now();
 
@@ -119,6 +125,13 @@ int main(int argc, char** argv)
 		while (carmen_log_parse_line(
 			input_stream, importedObservations, base_timestamp))
 		{
+			// fix timestamps. Carmen logs did not store timing information:
+			for (auto& o : importedObservations)
+				o->timestamp = mrpt::Clock::fromDouble(tim);
+
+			tim += dt;	// for the next carmen line
+
+			// save them:
 			for (size_t i = 0; i < importedObservations.size(); i++)
 			{
 				mrpt::serialization::archiveFrom(out_rawlog)

apps/kinect-stereo-calib/kinect_calibrate_guiMain.cpp

@@ -1816,8 +1816,13 @@ void kinect_calibrate_guiDialog::ProcessNewSelectedImageListBox()
 				// ======= Undistorted image =======
 				case 3:
 				{
-					il.undistort(il, m_calib_result.cam_params.leftCamera);
-					ir.undistort(ir, m_calib_result.cam_params.rightCamera);
+					mrpt::img::CImage ilD;
+					il.undistort(ilD, m_calib_result.cam_params.leftCamera);
+					il = ilD;
+
+					mrpt::img::CImage irD;
+					ir.undistort(irD, m_calib_result.cam_params.rightCamera);
+					ir = irD;
 				}
 				break;
 				// ======= Rectified images =======

apps/rosbag2rawlog/rosbag2rawlog_main.cpp

@@ -43,6 +43,7 @@
 #include <rosbag/bag.h>	 // rosbag_storage C++ lib
 #include <rosbag/view.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/CompressedImage.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/LaserScan.h>
@@ -379,14 +380,31 @@ Obs toRangeImage(
 
 Obs toImage(std::string_view msg, const rosbag::MessageInstance& rosmsg)
 {
-	auto image = rosmsg.instantiate<sensor_msgs::Image>();
+	cv_bridge::CvImagePtr cv_ptr;
+	mrpt::Clock::time_point timeStamp;
 
-	auto cv_ptr = cv_bridge::toCvShare(image);
+	if (auto image = rosmsg.instantiate<sensor_msgs::Image>(); image)
+	{
+		cv_ptr = cv_bridge::toCvCopy(image);
+		timeStamp = mrpt::ros1bridge::fromROS(image->header.stamp);
+	}
+	else if (auto imC = rosmsg.instantiate<sensor_msgs::CompressedImage>(); imC)
+	{
+		cv_ptr = cv_bridge::toCvCopy(imC);
+		timeStamp = mrpt::ros1bridge::fromROS(imC->header.stamp);
+	}
+	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 = mrpt::ros1bridge::fromROS(image->header.stamp);
+	imgObs->timestamp = timeStamp;
 
 	imgObs->image = mrpt::img::CImage(cv_ptr->image, mrpt::img::SHALLOW_COPY);
 
@@ -462,6 +480,7 @@ class Transcriber
 				auto callback = [=](const rosbag::MessageInstance& m) {
 					return toImage(sensorName, m);
 				};
+				ASSERT_(sensor.count("image_topic") != 0);
 				m_lookup[sensor.at("image_topic").as<std::string>()]
 					.emplace_back(callback);
 			}

appveyor.yml

@@ -1,5 +1,5 @@
 # version format
-version: 2.11.6-{branch}-build{build}
+version: 2.11.7-{branch}-build{build}
 
 os: Visual Studio 2019
 

doc/source/doxygen-docs/changelog.md

@@ -1,5 +1,14 @@
 \page changelog Change Log
 
+# Version 2.11.7: Released Jan 25th, 2024
+- Changes in apps:
+  - carmen2rawlog: Generate valid timestamps.
+- Changes in libraries:
+  - rosbag2rawlog: Add support for `sensor_msgs/CompressedImage` topics.
+- BUG FIXES:
+  - mrpt::hwdrivers::CImageGrabber_dc1394 did not mark the right image as present in stereo cameras.
+  - kinect-stereo-calib: Fix exception un-distorting images.
+
 # Version 2.11.6: Released Jan 13th, 2024
 - Changes in libraries:
   - \ref mrpt_obs_grp

libs/graphslam/include/mrpt/graphslam/ERD/CLoopCloserERD_impl.h

@@ -1281,7 +1281,7 @@ void CLoopCloserERD<GRAPH_T>::execDijkstraProjection(
 	// ending_node is either INVALID_NODEID or one of the already registered
 	// nodeIDs
 	ASSERTDEB_(
-		ending_node == INVALID_NODEID ||
+		ending_node == mrpt::graphs::INVALID_NODEID ||
 		(ending_node >= 0 && ending_node < this->m_graph->nodeCount()));
 	ASSERTDEBMSG_(
 		starting_node != ending_node, "Starting and Ending nodes coincede");

libs/hwdrivers/src/CImageGrabber_dc1394.cpp

@@ -605,6 +605,7 @@ bool CImageGrabber_dc1394::getObservation(
 			return false;
 		}
 
+		out_observation.hasImageRight = true;
 		out_observation.imageLeft.loadFromMemoryBuffer(
 			width, height, true, imageBufRGB);	// Left cam.
 		out_observation.imageRight.loadFromMemoryBuffer(

libs/nav/src/tpspace/CPTG_DiffDrive_CollisionGridBased.cpp

@@ -815,7 +815,11 @@ void CPTG_DiffDrive_CollisionGridBased::internal_initialize(
 
 			}  // n
 
-			if (verbose) cout << k << "/" << Ki << ",";
+			if (verbose)
+			{
+				cout << k << "/" << Ki << ",";
+				cout.flush();
+			}
 		}  // k
 
 		if (verbose) cout << format("Done! [%.03f sec]\n", tictac.Tac());

libs/opengl/src/CFBORender_unittest.cpp

@@ -46,6 +46,10 @@
 #if defined(__riscv)
 #undef RUN_OFFSCREEN_RENDER_TESTS
 #endif
+// Idem with Loongarch arch:
+#if defined(__loongarch__)
+#undef RUN_OFFSCREEN_RENDER_TESTS
+#endif
 
 namespace
 {

libs/system/src/CFileSystemWatcher.cpp

@@ -188,13 +188,13 @@ void CFileSystemWatcher::getChanges(TFileSystemChangeList& out_list)
 	}
 	else if (FD_ISSET(m_fd, &rfds))
 	{
-// inotify events are available! : Read them!
+		// inotify events are available! : Read them!
 
-/* size of the event structure, not counting name */
-#define EVENT_SIZE (sizeof(struct inotify_event))
+		/* size of the event structure, not counting name */
+		constexpr size_t EVENT_SIZE = (sizeof(struct inotify_event));
 
-/* reasonable guess as to size of 1024 events */
-#define BUF_LEN (1024 * (EVENT_SIZE + 16))
+		/* reasonable guess as to size of 1024 events */
+		constexpr size_t BUF_LEN = (1024 * (EVENT_SIZE + 16));
 
 		char buf[BUF_LEN];
 		ssize_t i = 0;
@@ -215,17 +215,12 @@ void CFileSystemWatcher::getChanges(TFileSystemChangeList& out_list)
 
 		while (i < len)
 		{
-			struct inotify_event event_val
-			{
-			};
-			std::memcpy(
-				&event_val, &buf[i],
-				sizeof(event_val));	 // Was: event = (struct inotify_event *) ;
-			struct inotify_event* event = &event_val;
+			const inotify_event* event =
+				reinterpret_cast<const inotify_event*>(&buf[i]);
 
 			i += EVENT_SIZE + event->len;
 
-			//			printf ("wd=%d mask=%u cookie=%u len=%u\n",event->wd,
+			// printf ("wd=%d mask=%u cookie=%u len=%u\n",event->wd,
 			// event->mask,event->cookie, event->len);
 
 			string eventName;

package.xml

@@ -7,7 +7,7 @@
 <package format="3">
   <name>mrpt2</name>
   <!-- Before updating version number, read [MRPT_ROOT]/version_prefix.txt first -->
-  <version>2.11.6</version>
+  <version>2.11.7</version>
   <description>Mobile Robot Programming Toolkit (MRPT) version 2.x</description>
 
   <author     email="[email protected]">Jose-Luis Blanco-Claraco</author>

share/mrpt/config_files/rawlog-grabber/camera_stereoBumblebee_libdc1394.ini

@@ -39,7 +39,7 @@ process_rate	= 80		// Hz
 sensorLabel		= BBCamera
 
 grabber_type	    = bumblebee_dc1394
-preview_decimation  = 1
+#preview_decimation  = 1 
 
 pose_x			= 0// position on the robot (meters)
 pose_y			= 0

version_prefix.txt

@@ -1,4 +1,4 @@
-2.11.6
+2.11.7
 # IMPORTANT: This file is parsed by CMake, don't add any comment to
 # the first line.
 # This file is used in both Windows and Linux scripts to automatically