diff --git a/modules/simulator/include/mvsim/Sensors/CameraSensor.h b/modules/simulator/include/mvsim/Sensors/CameraSensor.h index c475c817..71e90e61 100644 --- a/modules/simulator/include/mvsim/Sensors/CameraSensor.h +++ b/modules/simulator/include/mvsim/Sensors/CameraSensor.h @@ -43,6 +43,17 @@ class CameraSensor : public SensorBase const mrpt::optional_ref& 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_; diff --git a/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h b/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h index b21293bd..82309eaf 100644 --- a/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h +++ b/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h @@ -54,6 +54,17 @@ class DepthCameraSensor : public SensorBase const mrpt::optional_ref& 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_; diff --git a/modules/simulator/include/mvsim/Sensors/IMU.h b/modules/simulator/include/mvsim/Sensors/IMU.h index edbf9d07..c365aeb4 100644 --- a/modules/simulator/include/mvsim/Sensors/IMU.h +++ b/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] diff --git a/modules/simulator/include/mvsim/Sensors/LaserScanner.h b/modules/simulator/include/mvsim/Sensors/LaserScanner.h index 92957581..74551810 100644 --- a/modules/simulator/include/mvsim/Sensors/LaserScanner.h +++ b/modules/simulator/include/mvsim/Sensors/LaserScanner.h @@ -52,11 +52,22 @@ class LaserScanner : public SensorBase const mrpt::optional_ref& 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) diff --git a/modules/simulator/include/mvsim/Sensors/Lidar3D.h b/modules/simulator/include/mvsim/Sensors/Lidar3D.h index ffdab006..1d96a06e 100644 --- a/modules/simulator/include/mvsim/Sensors/Lidar3D.h +++ b/modules/simulator/include/mvsim/Sensors/Lidar3D.h @@ -48,6 +48,17 @@ class Lidar3D : public SensorBase const mrpt::optional_ref& 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; diff --git a/modules/simulator/include/mvsim/Sensors/SensorBase.h b/modules/simulator/include/mvsim/Sensors/SensorBase.h index 2fb250be..e93cc683 100644 --- a/modules/simulator/include/mvsim/Sensors/SensorBase.h +++ b/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 diff --git a/modules/simulator/include/mvsim/Simulable.h b/modules/simulator/include/mvsim/Simulable.h index c8f2a3ff..b1f13657 100644 --- a/modules/simulator/include/mvsim/Simulable.h +++ b/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; diff --git a/modules/simulator/src/Sensors/CameraSensor.cpp b/modules/simulator/src/Sensors/CameraSensor.cpp index b44ca15b..959d407f 100644 --- a/modules/simulator/src/Sensors/CameraSensor.cpp +++ b/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(); } diff --git a/modules/simulator/src/Sensors/DepthCameraSensor.cpp b/modules/simulator/src/Sensors/DepthCameraSensor.cpp index 6ca25d97..2b3d7089 100644 --- a/modules/simulator/src/Sensors/DepthCameraSensor.cpp +++ b/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() diff --git a/modules/simulator/src/Sensors/IMU.cpp b/modules/simulator/src/Sensors/IMU.cpp index b06b73cd..6605c201 100644 --- a/modules/simulator/src/Sensors/IMU.cpp +++ b/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; diff --git a/modules/simulator/src/Sensors/LaserScanner.cpp b/modules/simulator/src/Sensors/LaserScanner.cpp index dfb80b39..37eca1e4 100644 --- a/modules/simulator/src/Sensors/LaserScanner.cpp +++ b/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) diff --git a/modules/simulator/src/Sensors/Lidar3D.cpp b/modules/simulator/src/Sensors/Lidar3D.cpp index 0ce94ef1..9753c15e 100644 --- a/modules/simulator/src/Sensors/Lidar3D.cpp +++ b/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() diff --git a/modules/simulator/src/Simulable.cpp b/modules/simulator/src/Simulable.cpp index 58c30178..d2f14b39 100644 --- a/modules/simulator/src/Simulable.cpp +++ b/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(this)->notifySimulableSetPose(p); } mrpt::math::TPose3D Simulable::getPose() const diff --git a/modules/simulator/src/World_gui.cpp b/modules/simulator/src/World_gui.cpp index 405012ae..f173c519 100644 --- a/modules/simulator/src/World_gui.cpp +++ b/modules/simulator/src/World_gui.cpp @@ -218,14 +218,22 @@ void World::GUI::prepare_editor_window() w->add("Selected object", "sans-bold"); + // Auxiliary lambda placeholder for when the user clicks on an object + // being able to load its current pose in the GUI controls yet to be + // constructed later on: + static std::function onEntitySelected; + auto lckListObjs = mrpt::lockHelper(parent_.getListOfSimulableObjectsMtx()); if (!parent_.getListOfSimulableObjects().empty()) { auto tab = w->add(); - nanogui::Widget* tabs[4] = { - tab->createTab("Vehicles"), tab->createTab("Blocks"), - tab->createTab("Elements"), tab->createTab("Misc.")}; + constexpr size_t NUM_TABS = 5; + + std::array tabs = { + tab->createTab("Vehicles"), tab->createTab("Sensors"), + tab->createTab("Blocks"), tab->createTab("Elements"), + tab->createTab("Misc.")}; tab->setActiveTab(0); @@ -234,18 +242,16 @@ void World::GUI::prepare_editor_window() nanogui::Orientation::Vertical, nanogui::Alignment::Minimum, 3, 3)); - nanogui::VScrollPanel* vscrolls[4] = { - tabs[0]->add(), - tabs[1]->add(), - tabs[2]->add(), - tabs[3]->add()}; + std::array vscrolls; + for (size_t i = 0; i < NUM_TABS; i++) + vscrolls[i] = tabs[i]->add(); for (auto vs : vscrolls) vs->setFixedSize({pnWidth, pnHeight}); // vscroll should only have *ONE* child. this is what `wrapper` // is for - nanogui::Widget* wrappers[4]; - for (int i = 0; i < 4; i++) + std::array wrappers; + for (size_t i = 0; i < NUM_TABS; i++) { wrappers[i] = vscrolls[i]->add(); wrappers[i]->setFixedSize({pnWidth, pnHeight}); @@ -254,31 +260,57 @@ void World::GUI::prepare_editor_window() nanogui::Alignment::Minimum, 3, 3)); } + // Extend the list of world objects with the robot sensors: + SimulableList listAllObjs = parent_.getListOfSimulableObjects(); + // Yes: we iterate over parent.getList...(), *not* the local copy, since + // it will be modified within the loop. for (const auto& o : parent_.getListOfSimulableObjects()) + { + if (auto v = dynamic_cast(o.second.get()); v) + { + auto& sensors = v->getSensors(); + for (auto& sensor : sensors) + { + const auto sensorFullName = + sensor->vehicle().getName() + "."s + sensor->getName(); + listAllObjs.insert({sensorFullName, sensor}); + } + } + } + + // Now, fill the editor list with all the existing objects: + for (const auto& o : listAllObjs) { InfoPerObject ipo; const auto& name = o.first; - bool isVehicle = false; + int wrapperIdx = -1; // default. The tag "page" to show this at. if (auto v = dynamic_cast(o.second.get()); v) { - isVehicle = true; + wrapperIdx = 0; ipo.visual = dynamic_cast(v); } - bool isBlock = false; if (auto v = dynamic_cast(o.second.get()); v) { - isBlock = true; + wrapperIdx = 2; + ipo.visual = dynamic_cast(v); + } + if (auto v = dynamic_cast(o.second.get()); v) + { + wrapperIdx = 1; ipo.visual = dynamic_cast(v); } // bool isWorldElement = false; if (auto v = dynamic_cast(o.second.get()); v) { // isWorldElement = true; + wrapperIdx = 3; ipo.visual = dynamic_cast(v); } - auto wrapper = - isVehicle ? wrappers[0] : (isBlock ? wrappers[1] : wrappers[2]); + + if (wrapperIdx < 0) continue; // unknown / non-editable item. + + auto wrapper = wrappers[wrapperIdx]; std::string label = name; if (label.empty()) label = "(unnamed)"; @@ -308,12 +340,16 @@ void World::GUI::prepare_editor_window() const bool btnsEnabled = !!gui_selectedObject.simulable; for (auto b : btns_selectedOps) b->setEnabled(btnsEnabled); + + // Set current coordinates in controls: + if (ipo.simulable && onEntitySelected) + onEntitySelected(ipo.simulable->getRelativePose()); }); } // "misc." tab // -------------- - wrappers[3] + wrappers[4] ->add("Save 3D scene...", ENTYPO_ICON_EXPORT) ->setCallback([this]() { try @@ -340,27 +376,61 @@ void World::GUI::prepare_editor_window() w->add(" "); + // Replace with mouse: btnReplaceObject = w->add("Click to replace..."); btnReplaceObject->setFlags(nanogui::Button::Flags::ToggleButton); btns_selectedOps.push_back(btnReplaceObject); + // Reorient (yaw/pitch/roll): + constexpr float REPOSITION_SLIDER_RANGE = 1.0; // Meters + + std::array slidersCoords; + const std::array coordsNames = { + "Move 'x':", "Move 'y':", "Move 'z':", + "Reorient 'yaw':", "Reorient 'pitch':", "Reorient 'roll':"}; + + for (int axis = 0; axis < 6; axis++) { auto pn = w->add(); pn->setLayout(new nanogui::BoxLayout( nanogui::Orientation::Horizontal, nanogui::Alignment::Fill, 2, 2)); - pn->add("Reorient:"); - auto slAngle = pn->add(); - slAngle->setRange({-M_PI, M_PI}); - slAngle->setCallback([this](float v) { + pn->add(coordsNames[axis]); + + auto slCoord = pn->add(); + slidersCoords[axis] = slCoord; + + if (axis >= 3) + slCoord->setRange({-M_PI, M_PI}); + else + slCoord->setRange( + {-REPOSITION_SLIDER_RANGE, REPOSITION_SLIDER_RANGE}); + + slCoord->setCallback([this, axis](float v) { if (!gui_selectedObject.simulable) return; - auto p = gui_selectedObject.simulable->getPose(); - p.yaw = v; - gui_selectedObject.simulable->setPose(p); + auto p = gui_selectedObject.simulable->getRelativePose(); + p[axis] = v; + gui_selectedObject.simulable->setRelativePose(p); }); - slAngle->setFixedWidth(150); - btns_selectedOps.push_back(slAngle); + slCoord->setFixedWidth(170); + btns_selectedOps.push_back(slCoord); } + // Now, we can define the lambda for filling in the current object pose in + // the GUI controls: + onEntitySelected = [slidersCoords](const mrpt::math::TPose3D p) { + // Positions: + for (int i = 0; i < 3; i++) + { + slidersCoords[i]->setRange( + {p[i] - REPOSITION_SLIDER_RANGE, + p[i] + REPOSITION_SLIDER_RANGE}); + slidersCoords[i]->setValue(p[i]); + } + // Angles: + for (int i = 0; i < 3; i++) slidersCoords[i + 3]->setValue(p[i + 3]); + }; + + // Replace with coordinates: auto btnPlaceCoords = w->add("Replace by coordinates..."); btns_selectedOps.push_back(btnPlaceCoords); btnPlaceCoords->setCallback([this]() { @@ -371,30 +441,37 @@ void World::GUI::prepare_editor_window() formPose->setLayout(new nanogui::GridLayout( nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5)); - nanogui::TextBox* lbs[3]; + nanogui::TextBox* lbs[6]; - formPose->add("x coordinate:"); + formPose->add("x:"); lbs[0] = formPose->add(); - formPose->add("y coordinate:"); + formPose->add("y:"); lbs[1] = formPose->add(); - formPose->add("Orientation:"); + formPose->add("z:"); lbs[2] = formPose->add(); - - for (int i = 0; i < 3; i++) + formPose->add("Yaw:"); + lbs[3] = formPose->add(); + formPose->add("Pitch:"); + lbs[4] = formPose->add(); + formPose->add("Roll:"); + lbs[5] = formPose->add(); + + for (int i = 0; i < 6; i++) { lbs[i]->setEditable(true); lbs[i]->setFixedSize({100, 20}); lbs[i]->setValue("0.0"); - lbs[i]->setUnits(i == 2 ? "[deg]" : "[m]"); + lbs[i]->setUnits(i >= 3 ? "[deg]" : "[m]"); lbs[i]->setDefaultValue("0.0"); lbs[i]->setFontSize(16); lbs[i]->setFormat("[-]?[0-9]*\\.?[0-9]+"); } - const auto pos = gui_selectedObject.simulable->getPose(); - lbs[0]->setValue(std::to_string(pos.x)); - lbs[1]->setValue(std::to_string(pos.y)); - lbs[2]->setValue(std::to_string(mrpt::RAD2DEG(pos.yaw))); + const auto pos = gui_selectedObject.simulable->getRelativePose(); + for (int i = 0; i < 3; i++) lbs[i]->setValue(std::to_string(pos[i])); + + for (int i = 3; i < 6; i++) + lbs[i]->setValue(std::to_string(mrpt::RAD2DEG(pos[i]))); formPose->add(""); formPose->add(""); @@ -404,19 +481,23 @@ void World::GUI::prepare_editor_window() formPose->add("Accept")->setCallback( [formPose, this, lbs]() { - gui_selectedObject.simulable->setPose( - {// X: - std::stod(lbs[0]->value()), - // Y: - std::stod(lbs[1]->value()), - // Z: - .0, - // Yaw - mrpt::DEG2RAD(std::stod(lbs[2]->value())), - // Pitch - 0.0, - // Roll: - 0.0}); + const mrpt::math::TPose3D newPose = { + // X: + std::stod(lbs[0]->value()), + // Y: + std::stod(lbs[1]->value()), + // Z: + std::stod(lbs[2]->value()), + // Yaw + mrpt::DEG2RAD(std::stod(lbs[3]->value())), + // Pitch + mrpt::DEG2RAD(std::stod(lbs[4]->value())), + // Roll: + mrpt::DEG2RAD(std::stod(lbs[5]->value()))}; + + gui_selectedObject.simulable->setRelativePose(newPose); + onEntitySelected(newPose); + formPose->dispose(); }); @@ -425,6 +506,7 @@ void World::GUI::prepare_editor_window() formPose->setVisible(true); }); + // Disable all edit-controls since no object is selected: for (auto b : btns_selectedOps) b->setEnabled(false); // Minimize subwindow: