diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 6b21a078..f59c25b1 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mvsim ^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.9.2 (2024-03-21) +------------------ +* BUG FIX: 3D lidars should not 'see' XYZ corners of wheels +* BUG FIX: gridmaps were published at a too high rate in the ROS 2 node +* remove dead code +* update header version +* Contributors: Jose Luis Blanco-Claraco + 0.9.1 (2024-03-05) ------------------ * Fix use of mrpt bridge to publish XYZIRT point clouds too for ROS 1 diff --git a/README.md b/README.md index a7bbad6a..2f75cde7 100644 --- a/README.md +++ b/README.md @@ -57,7 +57,7 @@ Build matrix status | ROS 1 Noetic (u20.04) | [![Build Status](https://build.ros.org/job/Ndev__mvsim__ubuntu_focal_amd64/badge/icon)](https://build.ros.org/job/Ndev__mvsim__ubuntu_focal_amd64/) | [![Build Status](https://build.ros.org/job/Nbin_uF64__mvsim__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros.org/job/Nbin_uF64__mvsim__ubuntu_focal_amd64__binary/) | [![Version](https://img.shields.io/ros/v/noetic/mvsim)](https://index.ros.org/search/?term=mvsim) | | ROS 2 Humble (u22.04) | [![Build Status](https://build.ros2.org/job/Hdev__mvsim__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__mvsim__ubuntu_jammy_amd64/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mvsim__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mvsim__ubuntu_jammy_amd64__binary/) | [![Version](https://img.shields.io/ros/v/humble/mvsim)](https://index.ros.org/search/?term=mvsim) | | ROS 2 Iron (u22.04) | [![Build Status](https://build.ros2.org/job/Idev__mvsim__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Idev__mvsim__ubuntu_jammy_amd64/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mvsim__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mvsim__ubuntu_jammy_amd64__binary/) | [![Version](https://img.shields.io/ros/v/iron/mvsim)](https://index.ros.org/search/?term=mvsim) | -| ROS 2 Rolling (u22.04) | [![Build Status](https://build.ros2.org/job/Rdev__mvsim__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Rdev__mvsim__ubuntu_jammy_amd64/) | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__mvsim__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__mvsim__ubuntu_jammy_amd64__binary/) | [![Version](https://img.shields.io/ros/v/rolling/mvsim)](https://index.ros.org/search/?term=mvsim) | +| ROS 2 Rolling (u24.04) | [![Build Status](https://build.ros2.org/job/Rdev__mvsim__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Rdev__mvsim__ubuntu_noble_amd64/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mvsim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mvsim__ubuntu_noble_amd64__binary/) | [![Version](https://img.shields.io/ros/v/rolling/mvsim)](https://index.ros.org/search/?term=mvsim) | | EOL distro | Stable version | | --- | --- | diff --git a/modules/simulator/include/mvsim/VehicleBase.h b/modules/simulator/include/mvsim/VehicleBase.h index 522fbcf6..7f953701 100644 --- a/modules/simulator/include/mvsim/VehicleBase.h +++ b/modules/simulator/include/mvsim/VehicleBase.h @@ -223,8 +223,9 @@ class VehicleBase : public VisualObject, public Simulable // Called from internalGuiUpdate() void internal_internalGuiUpdate_forces(mrpt::opengl::COpenGLScene& scene); - mrpt::opengl::CSetOfObjects::Ptr glChassis_; - std::vector glWheels_; + mrpt::opengl::CSetOfObjects::Ptr glChassisViz_, glChassisPhysical_; + std::vector glWheelsViz_, + glWheelsPhysical_; mrpt::opengl::CSetOfLines::Ptr glForces_; mrpt::opengl::CSetOfLines::Ptr glMotorTorques_; diff --git a/modules/simulator/include/mvsim/Wheel.h b/modules/simulator/include/mvsim/Wheel.h index 767381f4..313f316f 100644 --- a/modules/simulator/include/mvsim/Wheel.h +++ b/modules/simulator/include/mvsim/Wheel.h @@ -73,7 +73,7 @@ class Wheel : public VisualObject // methods ---- - void getAs3DObject(mrpt::opengl::CSetOfObjects& obj); + void getAs3DObject(mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene); void loadFromXML(const rapidxml::xml_node* xml_node); void internalGuiUpdate( diff --git a/modules/simulator/include/mvsim/mvsim_version.h b/modules/simulator/include/mvsim/mvsim_version.h index 27705883..1fd0fc43 100644 --- a/modules/simulator/include/mvsim/mvsim_version.h +++ b/modules/simulator/include/mvsim/mvsim_version.h @@ -12,7 +12,7 @@ // clang-format off #define MVSIM_MAJOR_VERSION 0 #define MVSIM_MINOR_VERSION 9 -#define MVSIM_PATCH_VERSION 1 +#define MVSIM_PATCH_VERSION 2 #define MVSIM_STR_EXP(__A) #__A #define MVSIM_STR(__A) MVSIM_STR_EXP(__A) diff --git a/modules/simulator/src/VehicleBase.cpp b/modules/simulator/src/VehicleBase.cpp index 4d357d48..9e1e8886 100644 --- a/modules/simulator/src/VehicleBase.cpp +++ b/modules/simulator/src/VehicleBase.cpp @@ -697,18 +697,26 @@ void VehicleBase::internalGuiUpdate( // 1st time call?? -> Create objects // ---------------------------------- const size_t nWs = this->getNumWheels(); - if (!glChassis_ && viz && physical) + if (!glChassisViz_ && viz && physical) { - glChassis_ = mrpt::opengl::CSetOfObjects::Create(); - glChassis_->setName("vehicle_chassis_"s + name_); + glChassisViz_ = mrpt::opengl::CSetOfObjects::Create(); + glChassisViz_->setName("vehicle_chassis_"s + name_); + + glChassisPhysical_ = mrpt::opengl::CSetOfObjects::Create(); + glChassisPhysical_->setName("vehicle_chassis_"s + name_); // Wheels shape: - glWheels_.resize(nWs); + glWheelsViz_.resize(nWs); + glWheelsPhysical_.resize(nWs); for (size_t i = 0; i < nWs; i++) { - glWheels_[i] = mrpt::opengl::CSetOfObjects::Create(); - this->getWheelInfo(i).getAs3DObject(*glWheels_[i]); - glChassis_->insert(glWheels_[i]); + glWheelsViz_[i] = mrpt::opengl::CSetOfObjects::Create(); + this->getWheelInfo(i).getAs3DObject(*glWheelsViz_[i], false); + glChassisViz_->insert(glWheelsViz_[i]); + + glWheelsPhysical_[i] = mrpt::opengl::CSetOfObjects::Create(); + this->getWheelInfo(i).getAs3DObject(*glWheelsPhysical_[i], true); + glChassisPhysical_->insert(glWheelsPhysical_[i]); } if (!childrenOnly) @@ -718,11 +726,12 @@ void VehicleBase::internalGuiUpdate( chassis_poly_, chassis_z_max_ - chassis_z_min_); gl_poly->setLocation(0, 0, chassis_z_min_); gl_poly->setColor_u8(chassis_color_); - glChassis_->insert(gl_poly); + glChassisViz_->insert(gl_poly); + glChassisPhysical_->insert(gl_poly); } - viz->get().insert(glChassis_); - physical->get().insert(glChassis_); + viz->get().insert(glChassisViz_); + physical->get().insert(glChassisPhysical_); } // Update them: @@ -732,13 +741,18 @@ void VehicleBase::internalGuiUpdate( // need/can't acquire it again: const auto objectPose = viz.has_value() ? getPose() : getPoseNoLock(); - if (glChassis_) + if (glChassisViz_) { - glChassis_->setPose(objectPose); + ASSERT_(glChassisPhysical_); + + glChassisViz_->setPose(objectPose); + glChassisPhysical_->setPose(objectPose); for (size_t i = 0; i < nWs; i++) { const Wheel& w = getWheelInfo(i); - glWheels_[i]->setPose(mrpt::math::TPose3D( + glWheelsPhysical_[i]->setPose(mrpt::math::TPose3D( + w.x, w.y, 0.5 * w.diameter, w.yaw, w.getPhi(), 0.0)); + glWheelsViz_[i]->setPose(mrpt::math::TPose3D( w.x, w.y, 0.5 * w.diameter, w.yaw, w.getPhi(), 0.0)); if (!w.linked_yaw_object_name.empty()) @@ -853,8 +867,8 @@ void VehicleBase::registerOnServer(mvsim::Client& c) void VehicleBase::chassisAndWheelsVisible(bool visible) { - if (glChassis_) glChassis_->setVisibility(visible); - for (auto& glW : glWheels_) + if (glChassisViz_) glChassisViz_->setVisibility(visible); + for (auto& glW : glWheelsViz_) { if (glW) glW->setVisibility(visible); } diff --git a/modules/simulator/src/Wheel.cpp b/modules/simulator/src/Wheel.cpp index e8cefd80..dff22b2e 100644 --- a/modules/simulator/src/Wheel.cpp +++ b/modules/simulator/src/Wheel.cpp @@ -24,7 +24,8 @@ using namespace std; Wheel::Wheel(World* world) : VisualObject(world) { recalcInertia(); } -void Wheel::getAs3DObject(mrpt::opengl::CSetOfObjects& obj) +void Wheel::getAs3DObject( + mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene) { obj.clear(); @@ -40,19 +41,22 @@ void Wheel::getAs3DObject(mrpt::opengl::CSetOfObjects& obj) gl_wheel->setPose( mrpt::poses::CPose3D(0, 0.5 * width, 0, 0, 0, mrpt::DEG2RAD(90))); - auto gl_wheel_frame = mrpt::opengl::CSetOfObjects::Create(); - gl_wheel_frame->setName("gl_wheel_frame"); - gl_wheel_frame->insert(gl_wheel); + if (!isPhysicalScene) { - mrpt::opengl::CSetOfObjects::Ptr gl_xyz = - mrpt::opengl::stock_objects::CornerXYZSimple( - 0.9 * diameter, 2.0); + auto gl_wheel_frame = mrpt::opengl::CSetOfObjects::Create(); + gl_wheel_frame->setName("gl_wheel_frame"); + gl_wheel_frame->insert(gl_wheel); + { + mrpt::opengl::CSetOfObjects::Ptr gl_xyz = + mrpt::opengl::stock_objects::CornerXYZSimple( + 0.9 * diameter, 2.0); #if MRPT_VERSION >= 0x270 - gl_xyz->castShadows(false); + gl_xyz->castShadows(false); #endif - gl_wheel_frame->insert(gl_xyz); + gl_wheel_frame->insert(gl_xyz); + } + obj.insert(gl_wheel_frame); } - obj.insert(gl_wheel_frame); } obj.setPose(mrpt::math::TPose3D(x, y, 0.5 * diameter, yaw, 0.0, 0.0)); diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 093ad522..3e32666f 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -497,10 +497,13 @@ void MVSimNode::publishVehicles([[maybe_unused]] mvsim::VehicleBase& veh) void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj) { // GridMaps -------------- + static mrpt::system::CTicTac lastMapPublished; if (mvsim::OccupancyGridMap* grid = dynamic_cast(&obj); - grid) + grid && lastMapPublished.Tac() > 2.0) { + lastMapPublished.Tic(); + static Msg_OccupancyGrid ros_map; static mvsim::OccupancyGridMap* cachedGrid = nullptr; @@ -542,11 +545,12 @@ void MVSimNode::notifyROSWorldIsUpdated() static mrpt::system::CTicTac lastMapPublished; if (lastMapPublished.Tac() > 2.0) { + lastMapPublished.Tic(); + mvsim_world_->runVisitorOnWorldElements( [this](mvsim::WorldElementBase& obj) { publishWorldElements(obj); }); - lastMapPublished.Tic(); } #endif @@ -1539,9 +1543,6 @@ void MVSimNode::internalOn( msg_header.stamp = now; msg_header.frame_id = sSensorFrameId_points; - mrpt::obs::T3DPointsProjectionParams pp; - pp.takeIntoAccountSensorPoseOnRobot = false; - #if defined(HAVE_POINTS_XYZIRT) if (auto* xyzirt = dynamic_cast( obs.pointcloud.get()); diff --git a/package.xml b/package.xml index 26d0ffff..1c51c463 100644 --- a/package.xml +++ b/package.xml @@ -5,7 +5,7 @@ A lightweight multivehicle simulation framework. - 0.9.1 + 0.9.2 Jose-Luis Blanco-Claraco BSD