New GUI editor feature: move sensor poses

modules/simulator/include/mvsim/Sensors/CameraSensor.h

@@ -43,6 +43,17 @@ class CameraSensor : public SensorBase
 		const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& physical,
 		bool childrenOnly) override;
 
+	void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override;
+
+	mrpt::math::TPose3D getRelativePose() const override
+	{
+		return sensor_params_.sensorPose();
+	}
+	void setRelativePose(const mrpt::math::TPose3D& p) override
+	{
+		sensor_params_.setSensorPose(mrpt::poses::CPose3D(p));
+	}
+
 	// Store here all sensor intrinsic parameters. This obj will be copied as a
 	// "pattern" to fill it with actual scan data.
 	mrpt::obs::CObservationImage sensor_params_;

modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h

@@ -54,6 +54,17 @@ class DepthCameraSensor : public SensorBase
 		const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& physical,
 		bool childrenOnly) override;
 
+	void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override;
+
+	mrpt::math::TPose3D getRelativePose() const override
+	{
+		return sensor_params_.sensorPose.asTPose();
+	}
+	void setRelativePose(const mrpt::math::TPose3D& p) override
+	{
+		sensor_params_.setSensorPose(mrpt::poses::CPose3D(p));
+	}
+
 	// Store here all sensor intrinsic parameters. This obj will be copied as a
 	// "pattern" to fill it with actual scan data.
 	mrpt::obs::CObservation3DRangeScan sensor_params_;

modules/simulator/include/mvsim/Sensors/IMU.h

@@ -44,6 +44,13 @@ class IMU : public SensorBase
 			physical,
 		[[maybe_unused]] bool childrenOnly) override;
 
+	void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override;
+
+	mrpt::math::TPose3D getRelativePose() const override { return {}; }
+	void setRelativePose(const mrpt::math::TPose3D&) override
+	{
+		// sensor_params_.setSensorPose(mrpt::poses::CPose3D(p));
+	}
 	void internal_simulate_imu(const TSimulContext& context);
 
 	double angularVelocityStdNoise_ = 2e-4;	 //!< [rad/s]

modules/simulator/include/mvsim/Sensors/LaserScanner.h

@@ -52,11 +52,22 @@ class LaserScanner : public SensorBase
 		const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& physical,
 		bool childrenOnly) override;
 
+	void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override;
+
+	mrpt::math::TPose3D getRelativePose() const override
+	{
+		return scan_model_.sensorPose.asTPose();
+	}
+	void setRelativePose(const mrpt::math::TPose3D& p) override
+	{
+		scan_model_.setSensorPose(mrpt::poses::CPose3D(p));
+	}
+
 	// when not using the 3D raytrace mode.
 	void internal_simulate_lidar_2d_mode(const TSimulContext& context);
 
 	int z_order_;  //!< to help rendering multiple scans
-	mrpt::poses::CPose2D sensor_pose_on_veh_;
+
 	double rangeStdNoise_ = 0.01;
 	double angleStdNoise_ = mrpt::DEG2RAD(0.01);
 	/** Whether all box2d "fixtures" are visible (solid) or not (Default=true)

modules/simulator/include/mvsim/Sensors/Lidar3D.h

@@ -48,6 +48,17 @@ class Lidar3D : public SensorBase
 		const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& physical,
 		bool childrenOnly) override;
 
+	void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override;
+
+	mrpt::math::TPose3D getRelativePose() const override
+	{
+		return sensorPoseOnVeh_.asTPose();
+	}
+	void setRelativePose(const mrpt::math::TPose3D& p) override
+	{
+		sensorPoseOnVeh_ = mrpt::poses::CPose3D(p);
+	}
+
 	mrpt::poses::CPose3D sensorPoseOnVeh_;
 
 	double rangeStdNoise_ = 0.01;

modules/simulator/include/mvsim/Sensors/SensorBase.h

@@ -60,6 +60,10 @@ class SensorBase : public VisualObject, public Simulable
 
 	double sensor_period() const { return sensor_period_; }
 
+	/** The vehicle this sensor is attached to */
+	Simulable& vehicle() { return vehicle_; }
+	const Simulable& vehicle() const { return vehicle_; }
+
    protected:
 	/** Should be called within each derived class simul_post_timestep() method
 	 *  to update sensor_last_timestamp_ and check if the sensor should be

modules/simulator/include/mvsim/Simulable.h

@@ -65,6 +65,12 @@ class Simulable
 	 */
 	mrpt::math::TPose3D getPose() const;
 
+	/** Like getPose(), but gets the relative pose with respect to the parent
+	 * object, or just exactly like getPose() (global pose) if this is a
+	 * top-level entity.
+	 */
+	virtual mrpt::math::TPose3D getRelativePose() const { return getPose(); }
+
 	/// No thread-safe version. Used internally only.
 	mrpt::math::TPose3D getPoseNoLock() const;
 
@@ -78,7 +84,11 @@ class Simulable
 
 	/** Manually override vehicle pose (Use with caution!) (purposely set a
 	 * "const")*/
-	void setPose(const mrpt::math::TPose3D& p) const;
+	void setPose(const mrpt::math::TPose3D& p, bool notifyChange = true) const;
+
+	/** Changes the relative pose of this object with respect to its parent, or
+	 * the global frame if its a top-level entity. */
+	virtual void setRelativePose(const mrpt::math::TPose3D& p) { setPose(p); }
 
 	void setTwist(const mrpt::math::TTwist2D& dq) const;
 
@@ -131,6 +141,14 @@ class Simulable
 
 	void internalHandlePublish(const TSimulContext& context);
 
+	/** Will be called after the global pose of the object has changed due to a
+	 * direct call to setPose() */
+	virtual void notifySimulableSetPose(
+		[[maybe_unused]] const mrpt::math::TPose3D& newPose)
+	{
+		// Default: do nothing
+	}
+
    private:
 	World* simulable_parent_ = nullptr;
 

modules/simulator/src/Sensors/CameraSensor.cpp

@@ -150,8 +150,9 @@ void CameraSensor::internalGuiUpdate(
 	gl_sensor_fov_->setPose(p);
 	gl_sensor_origin_->setPose(p);
 
-	if (glCustomVisual_)
-		glCustomVisual_->setPose(p + sensor_params_.cameraPose.asTPose());
+	const auto globalSensorPose = p + sensor_params_.cameraPose.asTPose();
+
+	if (glCustomVisual_) glCustomVisual_->setPose(globalSensorPose);
 }
 
 void CameraSensor::simul_pre_timestep(
@@ -253,6 +254,19 @@ void CameraSensor::simul_post_timestep(const TSimulContext& context)
 		has_to_render_ = context;
 		world_->mark_as_pending_running_sensors_on_3D_scene();
 	}
+
+	// Keep sensor global pose up-to-date:
+	const auto& p = vehicle_.getPose();
+	const auto globalSensorPose = p + sensor_params_.cameraPose.asTPose();
+	Simulable::setPose(globalSensorPose, false /*do not notify*/);
+}
+
+void CameraSensor::notifySimulableSetPose(const mrpt::math::TPose3D& newPose)
+{
+	// The editor has moved the sensor in global coordinates.
+	// Convert back to local:
+	const auto& p = vehicle_.getPose();
+	sensor_params_.cameraPose = mrpt::poses::CPose3D(newPose - p);
 }
 
 void CameraSensor::freeOpenGLResources() { fbo_renderer_rgb_.reset(); }

modules/simulator/src/Sensors/DepthCameraSensor.cpp

@@ -441,6 +441,19 @@ void DepthCameraSensor::simul_post_timestep(const TSimulContext& context)
 		has_to_render_ = context;
 		world_->mark_as_pending_running_sensors_on_3D_scene();
 	}
+	// Keep sensor global pose up-to-date:
+	const auto& p = vehicle_.getPose();
+	const auto globalSensorPose = p + sensor_params_.sensorPose.asTPose();
+	Simulable::setPose(globalSensorPose, false /*do not notify*/);
+}
+
+void DepthCameraSensor::notifySimulableSetPose(
+	const mrpt::math::TPose3D& newPose)
+{
+	// The editor has moved the sensor in global coordinates.
+	// Convert back to local:
+	const auto& p = vehicle_.getPose();
+	sensor_params_.sensorPose = mrpt::poses::CPose3D(newPose - p);
 }
 
 void DepthCameraSensor::freeOpenGLResources()

modules/simulator/src/Sensors/IMU.cpp

@@ -92,6 +92,10 @@ void IMU::simul_post_timestep(const TSimulContext& context)
 	{
 		internal_simulate_imu(context);
 	}
+	// Keep sensor global pose up-to-date:
+	const auto& p = vehicle_.getPose();
+	const auto globalSensorPose = p + obs_model_.sensorPose.asTPose();
+	Simulable::setPose(globalSensorPose, false /*do not notify*/);
 }
 
 void IMU::internal_simulate_imu(const TSimulContext& context)
@@ -138,6 +142,14 @@ void IMU::internal_simulate_imu(const TSimulContext& context)
 	SensorBase::reportNewObservation(last_obs_, context);
 }
 
+void IMU::notifySimulableSetPose(const mrpt::math::TPose3D&)
+{
+	// The editor has moved the sensor in global coordinates.
+	// Convert back to local:
+	// const auto& p = vehicle_.getPose();
+	// sensor_params_.sensorPose = mrpt::poses::CPose3D(newPose - p);
+}
+
 void IMU::registerOnServer(mvsim::Client& c)
 {
 	using namespace std::string_literals;

modules/simulator/src/Sensors/LaserScanner.cpp

@@ -229,6 +229,19 @@ void LaserScanner::simul_post_timestep(const TSimulContext& context)
 			internal_simulate_lidar_2d_mode(context);
 		}
 	}
+
+	// Keep sensor global pose up-to-date:
+	const auto& p = vehicle_.getPose();
+	const auto globalSensorPose = p + scan_model_.sensorPose.asTPose();
+	Simulable::setPose(globalSensorPose, false /*do not notify*/);
+}
+
+void LaserScanner::notifySimulableSetPose(const mrpt::math::TPose3D& newPose)
+{
+	// The editor has moved the sensor in global coordinates.
+	// Convert back to local:
+	const auto& p = vehicle_.getPose();
+	scan_model_.sensorPose = mrpt::poses::CPose3D(newPose - p);
 }
 
 void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context)

modules/simulator/src/Sensors/Lidar3D.cpp

@@ -165,6 +165,19 @@ void Lidar3D::simul_post_timestep(const TSimulContext& context)
 		has_to_render_ = context;
 		world_->mark_as_pending_running_sensors_on_3D_scene();
 	}
+
+	// Keep sensor global pose up-to-date:
+	const auto& p = vehicle_.getPose();
+	const auto globalSensorPose = p + sensorPoseOnVeh_.asTPose();
+	Simulable::setPose(globalSensorPose, false /*do not notify*/);
+}
+
+void Lidar3D::notifySimulableSetPose(const mrpt::math::TPose3D& newPose)
+{
+	// The editor has moved the sensor in global coordinates.
+	// Convert back to local:
+	const auto& p = vehicle_.getPose();
+	sensorPoseOnVeh_ = mrpt::poses::CPose3D(newPose - p);
 }
 
 void Lidar3D::freeOpenGLResources()

modules/simulator/src/Simulable.cpp

@@ -420,7 +420,7 @@ void Simulable::registerOnServer(mvsim::Client& c)
 	MRPT_END
 }
 
-void Simulable::setPose(const mrpt::math::TPose3D& p) const
+void Simulable::setPose(const mrpt::math::TPose3D& p, bool notifyChange) const
 {
 	q_mtx_.lock();
 
@@ -433,6 +433,8 @@ void Simulable::setPose(const mrpt::math::TPose3D& p) const
 		vo->guiUpdate(std::nullopt, std::nullopt);
 
 	q_mtx_.unlock();
+
+	if (notifyChange) const_cast<Simulable*>(this)->notifySimulableSetPose(p);
 }
 
 mrpt::math::TPose3D Simulable::getPose() const