From 72d47eb0f08e24d8a5c570d26c6e360d97ab9242 Mon Sep 17 00:00:00 2001 From: Kartik Mohta Date: Fri, 8 Apr 2022 03:47:47 +0530 Subject: [PATCH 01/12] Fix typo in TBB target check in gazebo-config.cmake.in (#3207) --- cmake/gazebo-config.cmake.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/gazebo-config.cmake.in b/cmake/gazebo-config.cmake.in index 9d7b544c67..ce09d8d304 100644 --- a/cmake/gazebo-config.cmake.in +++ b/cmake/gazebo-config.cmake.in @@ -226,7 +226,7 @@ list(APPEND @PKG_NAME@_LDFLAGS -L${GAZEBO_INSTALL_LIB_DIR}/gazebo-@GAZEBO_MAJOR_ set (GAZEBO_HAS_TBB_GREATER_OR_EQUAL_2021 @HAVE_TBB_GREATER_OR_EQUAL_2021@) if (GAZEBO_HAS_TBB_GREATER_OR_EQUAL_2021) find_package(TBB CONFIG) - if (TARGET tbb::tbb) + if (TARGET TBB::tbb) list(APPEND @PKG_NAME@_LIBRARIES TBB::tbb) endif () endif () From 2f0f7af4868883d1a6fea30086b3fcd703d583fc Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Wed, 4 May 2022 09:46:37 +1000 Subject: [PATCH 02/12] Fix missing namespace for string (#3211) --- gazebo/msgs/generator/GazeboGenerator.cc | 2 +- gazebo/msgs/generator/GazeboGenerator.hh | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gazebo/msgs/generator/GazeboGenerator.cc b/gazebo/msgs/generator/GazeboGenerator.cc index 0a681f6dec..10d76c7df6 100644 --- a/gazebo/msgs/generator/GazeboGenerator.cc +++ b/gazebo/msgs/generator/GazeboGenerator.cc @@ -37,7 +37,7 @@ namespace cpp { GazeboGenerator::GazeboGenerator(const std::string &/*_name*/) {} GazeboGenerator::~GazeboGenerator() {} bool GazeboGenerator::Generate(const FileDescriptor *_file, - const string &/*parameter*/, + const std::string &/*parameter*/, OutputDirectory *_generator_context, std::string * /*_error*/) const { diff --git a/gazebo/msgs/generator/GazeboGenerator.hh b/gazebo/msgs/generator/GazeboGenerator.hh index 6900e281aa..665deb8329 100644 --- a/gazebo/msgs/generator/GazeboGenerator.hh +++ b/gazebo/msgs/generator/GazeboGenerator.hh @@ -37,9 +37,9 @@ class GazeboGenerator : public CodeGenerator public: virtual ~GazeboGenerator(); public: virtual bool Generate(const FileDescriptor* file, - const string& parameter, + const std::string& parameter, OutputDirectory *directory, - string* error) const; + std::string* error) const; // private: GOOGLE_DISALLOW_EVIL_CONSTRUCTORS(GazeboGenerator); }; From f6d2b940bfe47d9d7841bd793d844e28eb1fc75d Mon Sep 17 00:00:00 2001 From: William Lew Date: Fri, 6 May 2022 14:17:10 -0700 Subject: [PATCH 03/12] Added shininess to models (#3213) * Advertise shininess to renderer Signed-off-by: William Lew * Updated comments Signed-off-by: William Lew * Added codee suggestions Signed-off-by: William Lew * Added codee suggestions Signed-off-by: William Lew * Require sdformat9 9.8 Signed-off-by: William Lew * Added code suggestions Signed-off-by: William Lew --- cmake/gazebo-config.cmake.in | 2 +- gazebo/physics/World.cc | 35 +++++++++++++ gazebo/physics/World.hh | 7 +++ gazebo/physics/WorldPrivate.hh | 3 ++ gazebo/rendering/Visual.cc | 43 ++++++++++++++++ gazebo/rendering/Visual.hh | 2 + gazebo/rendering/VisualPrivate.hh | 3 ++ worlds/shapes_shininess.world | 82 +++++++++++++++++++++++++++++++ 8 files changed, 176 insertions(+), 1 deletion(-) create mode 100644 worlds/shapes_shininess.world diff --git a/cmake/gazebo-config.cmake.in b/cmake/gazebo-config.cmake.in index ce09d8d304..bb9ec355e3 100644 --- a/cmake/gazebo-config.cmake.in +++ b/cmake/gazebo-config.cmake.in @@ -171,7 +171,7 @@ list(APPEND @PKG_NAME@_INCLUDE_DIRS ${PROTOBUF_INCLUDE_DIRS}) list(APPEND @PKG_NAME@_LIBRARIES ${PROTOBUF_LIBRARIES}) # Find SDFormat -find_package(sdformat9 REQUIRED) +find_package(sdformat9 REQUIRED VERSION 9.8) list(APPEND @PKG_NAME@_INCLUDE_DIRS ${SDFormat_INCLUDE_DIRS}) list(APPEND @PKG_NAME@_LIBRARIES ${SDFormat_LIBRARIES}) diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index 6ebd4ee282..b1f43a3937 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -1259,6 +1259,32 @@ ModelPtr World::LoadModel(sdf::ElementPtr _sdf , BasePtr _parent) } } + sdf::ElementPtr linkElem = _sdf->GetElement("link"); + if (linkElem->HasElement("visual") && + linkElem->GetElement("visual")->HasElement("material")) + { + sdf::ElementPtr matElem = linkElem->GetElement("visual")-> + GetElement("material"); + + if (matElem->HasElement("shininess")) + { + this->dataPtr->materialShininessMap[modelName] = + matElem->Get("shininess"); + } + else + { + this->dataPtr->materialShininessMap[modelName] = 0; + } + + std::string materialShininessService("/" + modelName + "/shininess"); + if (!this->dataPtr->ignNode.Advertise(materialShininessService, + &World::MaterialShininessService, this)) + { + gzerr << "Error advertising service [" + << materialShininessService << "]" << std::endl; + } + } + model = this->dataPtr->physicsEngine->CreateModel(_parent); model->SetWorld(shared_from_this()); model->Load(_sdf); @@ -3437,3 +3463,12 @@ bool World::ShadowCasterRenderBackFacesService(ignition::msgs::Boolean &_res) _res.set_data(this->dataPtr->shadowCasterRenderBackFaces); return true; } + +////////////////////////////////////////////////// +bool World::MaterialShininessService( + const ignition::msgs::StringMsg &_req, msgs::Any &_res) +{ + _res.set_type(msgs::Any::DOUBLE); + _res.set_double_value(this->dataPtr->materialShininessMap[_req.data()]); + return true; +} diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index e7131ae586..3ed2ac4315 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -671,6 +671,13 @@ namespace gazebo private: bool ShadowCasterRenderBackFacesService( ignition::msgs::Boolean &_response); + /// \brief Callback for "/shininess" service. + /// \param[in] _request Message containing the model name. + /// \param[out] _response Message containing shininess value. + /// \return True if the info was successfully obtained. + private: bool MaterialShininessService( + const ignition::msgs::StringMsg &_request, msgs::Any &_response); + /// \internal /// \brief Private data pointer. private: std::unique_ptr dataPtr; diff --git a/gazebo/physics/WorldPrivate.hh b/gazebo/physics/WorldPrivate.hh index 0fdf208ce3..d1bf78c42d 100644 --- a/gazebo/physics/WorldPrivate.hh +++ b/gazebo/physics/WorldPrivate.hh @@ -397,6 +397,9 @@ namespace gazebo /// \brief Shadow caster render back faces from scene SDF public: bool shadowCasterRenderBackFaces = true; + + /// \brief Shininess values from scene SDF + public: std::map materialShininessMap; }; } } diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index 1ac23ec702..d7e50b7341 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -346,6 +346,48 @@ void Visual::Load(sdf::ElementPtr _sdf) ////////////////////////////////////////////////// void Visual::Load() { + if (this->dataPtr->sdf->HasElement("material")) + { + // Get shininess value from physics::World + ignition::transport::Node node; + msgs::Any rep; + + const std::string visualName = + this->Name().substr(0, this->Name().find(":")); + + const std::string serviceName = "/" + visualName + "/shininess"; + + const std::string validServiceName = + ignition::transport::TopicUtils::AsValidTopic(serviceName); + + if (validServiceName.empty()) + { + gzerr << "Service name [" << serviceName << "] not valid" << std::endl; + return; + } + + ignition::msgs::StringMsg req; + req.set_data(visualName); + + bool result; + unsigned int timeout = 5000; + bool executed = node.Request(validServiceName, req, timeout, rep, result); + + if (executed) + { + if (result) + this->dataPtr->shininess = rep.double_value(); + else + gzerr << "Service call [" << validServiceName << "] failed" + << std::endl; + } + else + { + gzerr << "Service call [" << validServiceName << "] timed out" + << std::endl; + } + } + std::ostringstream stream; ignition::math::Pose3d pose; Ogre::MovableObject *obj = nullptr; @@ -1432,6 +1474,7 @@ void Visual::SetSpecular(const ignition::math::Color &_color, { pass = technique->getPass(passCount); pass->setSpecular(Conversions::Convert(_color)); + pass->setShininess(this->dataPtr->shininess); } } } diff --git a/gazebo/rendering/Visual.hh b/gazebo/rendering/Visual.hh index 7921bf0e87..d6a6e0c17b 100644 --- a/gazebo/rendering/Visual.hh +++ b/gazebo/rendering/Visual.hh @@ -30,6 +30,8 @@ #include #include #include +#include +#include #include "gazebo/common/Mesh.hh" #include "gazebo/common/Time.hh" diff --git a/gazebo/rendering/VisualPrivate.hh b/gazebo/rendering/VisualPrivate.hh index a0539eeb97..8e33669928 100644 --- a/gazebo/rendering/VisualPrivate.hh +++ b/gazebo/rendering/VisualPrivate.hh @@ -186,6 +186,9 @@ namespace gazebo public: ignition::math::Color specular = ignition::math::Color(0, 0, 0, 0); + /// \brief Specular exponent of the visual. + public: double shininess = 0; + /// \brief Emissive color of the visual. public: ignition::math::Color emissive = ignition::math::Color(0, 0, 0, 0); diff --git a/worlds/shapes_shininess.world b/worlds/shapes_shininess.world new file mode 100644 index 0000000000..85c30333a9 --- /dev/null +++ b/worlds/shapes_shininess.world @@ -0,0 +1,82 @@ + + + + + model://ground_plane + + + model://sun + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + 1 0 0 1 + 1 + + + + 1 1 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + + 0.5 + + + + + + 0 1 0 1 + 5 + + + + 0.5 + + + + + + + 0 -1.5 0.5 0 1.5707 0 + + + + + 0.5 + 1.0 + + + + + + 0 0 1 1 + 10 + + + + 0.5 + 1.0 + + + + + + + From c9c2300016015d40ee49477f939602880cca6bf9 Mon Sep 17 00:00:00 2001 From: William Lew Date: Thu, 19 May 2022 09:54:58 -0700 Subject: [PATCH 04/12] Support element when setting up camera (#3201) * Require sdformat9 9.8 Signed-off-by: William Lew --- gazebo/rendering/Camera.cc | 6 +++++- gazebo/rendering/CameraPrivate.hh | 3 +++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/gazebo/rendering/Camera.cc b/gazebo/rendering/Camera.cc index 502f122a13..a97e425234 100644 --- a/gazebo/rendering/Camera.cc +++ b/gazebo/rendering/Camera.cc @@ -118,6 +118,8 @@ Camera::Camera(const std::string &_name, ScenePtr _scene, this->dataPtr->node = transport::NodePtr(new transport::Node()); this->dataPtr->node->Init(); + + this->dataPtr->antiAliasingValue = 4; } ////////////////////////////////////////////////// @@ -143,6 +145,8 @@ void Camera::Load() this->imageHeight = imgElem->Get("height"); this->imageFormat = this->OgrePixelFormat( imgElem->Get("format")); + + this->dataPtr->antiAliasingValue = imgElem->Get("anti_aliasing"); } else gzthrow("Camera has no tag."); @@ -1480,7 +1484,7 @@ void Camera::CreateRenderTexture(const std::string &_textureName) RenderEngine::Instance()->FSAALevels(); // check if target fsaa is supported - unsigned int targetFSAA = 4; + unsigned int targetFSAA = this->dataPtr->antiAliasingValue; auto const it = std::find(fsaaLevels.begin(), fsaaLevels.end(), targetFSAA); if (it != fsaaLevels.end()) fsaa = targetFSAA; diff --git a/gazebo/rendering/CameraPrivate.hh b/gazebo/rendering/CameraPrivate.hh index 686333c742..ac8b24e20f 100644 --- a/gazebo/rendering/CameraPrivate.hh +++ b/gazebo/rendering/CameraPrivate.hh @@ -117,6 +117,9 @@ namespace gazebo /// \brief Fixed axis to yaw around. public: ignition::math::Vector3d yawFixedAxis; + + /// \brief Anti-aliasing value + public: uint32_t antiAliasingValue; }; } } From 1f8c52a737183b4bae05529e75e6bb3c19b4301d Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 24 May 2022 10:57:28 -0700 Subject: [PATCH 05/12] Get/Set friction coefficients of wheels (#3219) Signed-off-by: Jenn Nguyen --- plugins/WheelSlipPlugin.cc | 54 ++++++++++++++++++++++++++++++++++++++ plugins/WheelSlipPlugin.hh | 18 +++++++++++++ 2 files changed, 72 insertions(+) diff --git a/plugins/WheelSlipPlugin.cc b/plugins/WheelSlipPlugin.cc index 27256809fb..6e21527665 100644 --- a/plugins/WheelSlipPlugin.cc +++ b/plugins/WheelSlipPlugin.cc @@ -481,6 +481,60 @@ void WheelSlipPlugin::SetSlipComplianceLongitudinal(std::string _wheel_name, con } } +///////////////////////////////////////////////// +std::map WheelSlipPlugin::GetFrictionCoefficients() +{ + std::lock_guard lock(this->dataPtr->mutex); + std::map frictionCoeffs; + for (const auto &linkSurface : this->dataPtr->mapLinkSurfaceParams) + { + auto link = linkSurface.first.lock(); + auto surface = linkSurface.second.surface.lock(); + if (!link || !surface) + continue; + + ignition::math::Vector2d friction; + friction.X(surface->FrictionPyramid()->MuPrimary()); + friction.Y(surface->FrictionPyramid()->MuSecondary()); + + frictionCoeffs[link->GetName()] = friction; + } + + return frictionCoeffs; +} + +///////////////////////////////////////////////// +bool WheelSlipPlugin::SetMuPrimary(const std::string &_wheel_name, double _mu) +{ + std::lock_guard lock(this->dataPtr->mutex); + if (this->dataPtr->mapLinkNames.count(_wheel_name) == 0) + return false; + + auto link = this->dataPtr->mapLinkNames[_wheel_name]; + auto surface = this->dataPtr->mapLinkSurfaceParams[link].surface.lock(); + if (surface == nullptr) + return false; + + surface->FrictionPyramid()->SetMuPrimary(_mu); + return true; +} + +///////////////////////////////////////////////// +bool WheelSlipPlugin::SetMuSecondary(const std::string &_wheel_name, double _mu) +{ + std::lock_guard lock(this->dataPtr->mutex); + if (this->dataPtr->mapLinkNames.count(_wheel_name) == 0) + return false; + + auto link = this->dataPtr->mapLinkNames[_wheel_name]; + auto surface = this->dataPtr->mapLinkSurfaceParams[link].surface.lock(); + if (surface == nullptr) + return false; + + surface->FrictionPyramid()->SetMuSecondary(_mu); + return true; +} + ///////////////////////////////////////////////// void WheelSlipPlugin::Update() { diff --git a/plugins/WheelSlipPlugin.hh b/plugins/WheelSlipPlugin.hh index fdd3ad119d..5cff396a39 100644 --- a/plugins/WheelSlipPlugin.hh +++ b/plugins/WheelSlipPlugin.hh @@ -151,6 +151,24 @@ namespace gazebo /// \param[in] _msg Slip compliance encoded as string. private: void OnLongitudinalCompliance(ConstGzStringPtr &_msg); + /// brief Get friction coefficients for each wheel + /// \return Map of wheel name to a Vector2 of friction coefficients + /// The Vector2.X value is the friction coefficient in the primary direction and + /// the Vector2.Y value is the friction coefficient in the secondary direction. + public: std::map GetFrictionCoefficients(); + + /// \brief Set the friction coefficient in the primary direction for a particular wheel. + /// \param[in] _wheel_name name of the wheel link on which _mu should be set. + /// \param[in] _mu Friction coefficient. + /// \return True if the friction coefficient was successfully set. False otherwise. + public: bool SetMuPrimary(const std::string &_wheel_name, double _mu); + + /// \brief Set the friction coefficient in the secondary direction for a particular wheel. + /// \param[in] _wheel_name name of the wheel link on which _mu should be set. + /// \param[in] _mu Friction coefficient. + /// \return True if the friction coefficient was successfully set. False otherwise. + public: bool SetMuSecondary(const std::string &_wheel_name, double _mu); + /// \brief Update the plugin. This is updated every iteration of /// simulation. private: void Update(); From 4f3523b5d1bb1d70e9bc81367706887b55fe5630 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 25 May 2022 19:59:02 +0200 Subject: [PATCH 06/12] Fix compatibility with FFmpeg 5.0 (#3195) --- gazebo/common/AudioDecoder.cc | 123 ++++++++++++++++++++++++++++++++-- gazebo/common/Video.cc | 85 ++++++++++++++++++++--- gazebo/common/VideoEncoder.cc | 10 ++- 3 files changed, 199 insertions(+), 19 deletions(-) diff --git a/gazebo/common/AudioDecoder.cc b/gazebo/common/AudioDecoder.cc index 92844f5319..83508b1474 100644 --- a/gazebo/common/AudioDecoder.cc +++ b/gazebo/common/AudioDecoder.cc @@ -59,8 +59,12 @@ void AudioDecoder::Cleanup() #ifdef HAVE_FFMPEG bool AudioDecoder::Decode(uint8_t **_outBuffer, unsigned int *_outBufferSize) { - AVPacket packet, packet1; +#if LIBAVFORMAT_VERSION_MAJOR < 59 + AVPacket *packet, packet1; int bytesDecoded = 0; +#else + AVPacket *packet; +#endif unsigned int maxBufferSize = 0; AVFrame *decodedFrame = nullptr; @@ -97,14 +101,66 @@ bool AudioDecoder::Decode(uint8_t **_outBuffer, unsigned int *_outBufferSize) else common::AVFrameUnref(decodedFrame); - av_init_packet(&packet); - while (av_read_frame(this->formatCtx, &packet) == 0) + packet = av_packet_alloc(); + if (!packet) + { + gzerr << "Failed to allocate AVPacket" << std::endl; + return false; + } + while (av_read_frame(this->formatCtx, packet) == 0) { - if (packet.stream_index == this->audioStream) + if (packet->stream_index == this->audioStream) { +#if LIBAVFORMAT_VERSION_MAJOR >= 59 + // Inspired from + // https://github.com/FFmpeg/FFmpeg/blob/n5.0/doc/examples/decode_audio.c#L71 + + // send the packet with the compressed data to the decoder + int ret = avcodec_send_packet(this->codecCtx, packet); + if (ret < 0) + { + gzerr << "Error submitting the packet to the decoder" << std::endl; + return false; + } + + // read all the output frames + // (in general there may be any number of them) + while (ret >= 0) + { + ret = avcodec_receive_frame(this->codecCtx, decodedFrame); + if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) + { + break; + } + else if (ret < 0) + { + gzerr << "Error during decoding" << std::endl; + return false; + } + + // Total size of the data. Some padding can be added to + // decodedFrame->data[0], which is why we can't use + // decodedFrame->linesize[0]. + int size = decodedFrame->nb_samples * + av_get_bytes_per_sample(this->codecCtx->sample_fmt) * + this->codecCtx->channels; + + // Resize the audio buffer as necessary + if (*_outBufferSize + size > maxBufferSize) + { + maxBufferSize += size * 5; + *_outBuffer = reinterpret_cast(realloc(*_outBuffer, + maxBufferSize * sizeof(*_outBuffer[0]))); + } + + memcpy(*_outBuffer + *_outBufferSize, decodedFrame->data[0], + size); + *_outBufferSize += size; + } +#else int gotFrame = 0; - packet1 = packet; + packet1 = *packet; while (packet1.size) { // Some frames rely on multiple packets, so we have to make sure @@ -144,11 +200,12 @@ bool AudioDecoder::Decode(uint8_t **_outBuffer, unsigned int *_outBufferSize) packet1.data += bytesDecoded; packet1.size -= bytesDecoded; } +#endif } - AVPacketUnref(&packet); + av_packet_unref(packet); } - AVPacketUnref(&packet); + av_packet_unref(packet); // Seek to the beginning so that it can be decoded again, if necessary. av_seek_frame(this->formatCtx, this->audioStream, 0, 0); @@ -214,7 +271,11 @@ bool AudioDecoder::SetFile(const std::string &_filename) # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" #endif +#if LIBAVFORMAT_VERSION_MAJOR >= 59 + if (this->formatCtx->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_AUDIO) +#else if (this->formatCtx->streams[i]->codec->codec_type == AVMEDIA_TYPE_AUDIO) +#endif #ifndef _WIN32 # pragma GCC diagnostic pop #endif @@ -238,13 +299,61 @@ bool AudioDecoder::SetFile(const std::string &_filename) # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" #endif +#if LIBAVFORMAT_VERSION_MAJOR < 59 this->codecCtx = this->formatCtx->streams[audioStream]->codec; +#endif #ifndef _WIN32 # pragma GCC diagnostic pop #endif // Find a decoder +#if LIBAVFORMAT_VERSION_MAJOR >= 59 + const AVCodec * local_codec = avcodec_find_decoder(this->formatCtx->streams[ + this->audioStream]->codecpar->codec_id); + if (!local_codec) + { + gzerr << "Failed to find the codec" << std::endl; + return false; + } + this->codecCtx = avcodec_alloc_context3(local_codec); + if (!this->codecCtx) + { + gzerr << "Failed to allocate the codec context" << std::endl; + return false; + } + + // Copy all relevant parameters from codepar to codecCtx + if (avcodec_parameters_to_context(this->codecCtx, + this->formatCtx->streams[this->audioStream]->codecpar) < 0) + { + gzerr << "Failed to copy codec parameters to decoder context" + << std::endl; + return false; + } + + // This copy should be done by avcodec_parameters_to_context, but at least on + // conda-forge with 5.0.0 h594f047_1 this is not happening for some reason. + // As temporary workaround, we copy directly the data structures, taking the code from + // https://github.com/FFmpeg/FFmpeg/blob/n5.0/libavcodec/codec_par.c#L120 + AVCodecParameters* par = this->formatCtx->streams[this->audioStream]->codecpar; + this->codecCtx->sample_fmt = static_cast(par->format); + this->codecCtx->channel_layout = par->channel_layout; + this->codecCtx->channels = par->channels; + this->codecCtx->sample_rate = par->sample_rate; + this->codecCtx->block_align = par->block_align; + this->codecCtx->frame_size = par->frame_size; + this->codecCtx->delay = + this->codecCtx->initial_padding = par->initial_padding; + this->codecCtx->trailing_padding = par->trailing_padding; + this->codecCtx->seek_preroll = par->seek_preroll; + + // It would be better to just define codec as const AVCodec *, + // but that is not done to avoid ABI problem. Anyhow, as codec + // it is a private attribute there should be no problem + this->codec = const_cast(local_codec); +#else this->codec = avcodec_find_decoder(codecCtx->codec_id); +#endif if (this->codec == nullptr) { diff --git a/gazebo/common/Video.cc b/gazebo/common/Video.cc index 56eb6fe8ad..d2041e06cb 100644 --- a/gazebo/common/Video.cc +++ b/gazebo/common/Video.cc @@ -39,6 +39,53 @@ using namespace common; // } // #endif +///////////////////////////////////////////////// +#ifdef HAVE_FFMPEG +int GazeboAVCodecDecodeHelper(AVCodecContext *_codecCtx, + AVFrame *_frame, int *_gotFrame, AVPacket *_packet) +{ +#if LIBAVFORMAT_VERSION_MAJOR >= 59 + // from https://blogs.gentoo.org/lu_zero/2016/03/29/new-avcodec-api/ + int ret; + + *_gotFrame = 0; + + if (_packet) + { + ret = avcodec_send_packet(_codecCtx, _packet); + if (ret < 0) + { + return ret == AVERROR_EOF ? 0 : ret; + } + } + + ret = avcodec_receive_frame(_codecCtx, _frame); + if (ret < 0 && ret != AVERROR(EAGAIN)) + { + return ret; + } + if (ret >= 0) + { + *_gotFrame = 1; + } + + // new API always consumes the whole packet + return _packet ? _packet->size : 0; +#else + // this was deprecated in ffmpeg version 3.1 + // github.com/FFmpeg/FFmpeg/commit/7fc329e2dd6226dfecaa4a1d7adf353bf2773726 +# ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +# endif + return avcodec_decode_video2(_codecCtx, _frame, _gotFrame, _packet); +# ifndef _WIN32 +# pragma GCC diagnostic pop +# endif +#endif +} +#endif + ///////////////////////////////////////////////// Video::Video() { @@ -77,7 +124,7 @@ void Video::Cleanup() #ifdef HAVE_FFMPEG bool Video::Load(const std::string &_filename) { - AVCodec *codec = nullptr; + const AVCodec *codec = nullptr; this->videoStream = -1; if (this->formatCtx || this->avFrame || this->codecCtx) @@ -103,6 +150,9 @@ bool Video::Load(const std::string &_filename) // Find the first video stream for (unsigned int i = 0; i < this->formatCtx->nb_streams; ++i) { +#if LIBAVFORMAT_VERSION_MAJOR >= 59 + if (this->formatCtx->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_VIDEO) +#else #ifndef _WIN32 # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" @@ -110,6 +160,7 @@ bool Video::Load(const std::string &_filename) if (this->formatCtx->streams[i]->codec->codec_type == AVMEDIA_TYPE_VIDEO) #ifndef _WIN32 # pragma GCC diagnostic pop +#endif #endif { this->videoStream = static_cast(i); @@ -124,6 +175,26 @@ bool Video::Load(const std::string &_filename) } // Get a pointer to the codec context for the video stream +#if LIBAVFORMAT_VERSION_MAJOR >= 59 + // AVCodecContext is not included in an AVStream as of ffmpeg 3.1 + // allocate a codec context based on updated example + // github.com/FFmpeg/FFmpeg/commit/bba6a03b2816d805d44bce4f9701a71f7d3f8dad + this->codecCtx = avcodec_alloc_context3(codec); + if (!this->codecCtx) + { + gzerr << "Failed to allocate the codec context" << std::endl; + return false; + } + + // Copy codec parameters from input stream to output codec context + if (avcodec_parameters_to_context(this->codecCtx, + this->formatCtx->streams[this->videoStream]->codecpar) < 0) + { + gzerr << "Failed to copy codec parameters to decoder context" + << std::endl; + return false; + } +#else #ifndef _WIN32 # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" @@ -132,6 +203,8 @@ bool Video::Load(const std::string &_filename) #ifndef _WIN32 # pragma GCC diagnostic pop #endif +#endif + // Find the decoder for the video stream codec = avcodec_find_decoder(this->codecCtx->codec_id); @@ -227,15 +300,9 @@ bool Video::GetNextFrame(unsigned char **_buffer) while (tmpPacket.size > 0) { // sending data to libavcodec -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - int processedLength = avcodec_decode_video2(this->codecCtx, this->avFrame, + int processedLength = GazeboAVCodecDecodeHelper(this->codecCtx, this->avFrame, &frameAvailable, &tmpPacket); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#endif + if (processedLength < 0) { gzerr << "Error while processing the data\n"; diff --git a/gazebo/common/VideoEncoder.cc b/gazebo/common/VideoEncoder.cc index 69ebe534ee..ac3006ac82 100644 --- a/gazebo/common/VideoEncoder.cc +++ b/gazebo/common/VideoEncoder.cc @@ -224,7 +224,6 @@ bool VideoEncoder::Start(const std::string &_format, // The remainder of this function handles FFMPEG initialization of a video // stream - AVOutputFormat *outputFormat = nullptr; // This 'if' and 'free' are just for safety. We chech the value of formatCtx // below. @@ -236,6 +235,11 @@ bool VideoEncoder::Start(const std::string &_format, if (this->dataPtr->format.compare("v4l2") == 0) { #if LIBAVDEVICE_VERSION_INT >= AV_VERSION_INT(56, 4, 100) +#if LIBAVFORMAT_VERSION_MAJOR >= 59 + const AVOutputFormat *outputFormat = nullptr; +#else + AVOutputFormat *outputFormat = nullptr; +#endif while ((outputFormat = av_output_video_device_next(outputFormat)) != nullptr) { @@ -256,7 +260,7 @@ bool VideoEncoder::Start(const std::string &_format, } else { - outputFormat = av_guess_format(nullptr, + const AVOutputFormat * outputFormat = av_guess_format(nullptr, this->dataPtr->filename.c_str(), nullptr); if (!outputFormat) @@ -294,7 +298,7 @@ bool VideoEncoder::Start(const std::string &_format, } // find the video encoder - AVCodec *encoder = avcodec_find_encoder( + const AVCodec *encoder = avcodec_find_encoder( this->dataPtr->formatCtx->oformat->video_codec); if (!encoder) { From 8f7e3f528d1b09bb1eaf7e6f1276cc0903eeb80f Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 25 May 2022 21:05:40 +0200 Subject: [PATCH 07/12] Convert OGRE_RESOURCE_PATH with TO_CMAKE_PATH (#3165) --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index dfd2b08e75..2b77db68b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -194,6 +194,7 @@ set(OGRE_RESOURCE_PATH ${OGRE_PLUGINDIR}) # Seems that OGRE_PLUGINDIR can end in a newline, which will cause problems when # we pass it to the compiler later. string(REPLACE "\n" "" OGRE_RESOURCE_PATH ${OGRE_RESOURCE_PATH}) +FILE(TO_CMAKE_PATH "${OGRE_RESOURCE_PATH}" OGRE_RESOURCE_PATH) # Check for DRI capable Display From 323fc1c58178bcbc5076e97bf7e6ff736c21cc7d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 27 May 2022 16:41:32 -0700 Subject: [PATCH 08/12] Fix tests related to shininess (#3223) * World::LoadModel: avoid creating empty links The logic in World::LoadModel currently creates an empty link in models without links by calling GetElement("link") without checking if such an element exists. This checks HasElement("link") before calling GetElement, which fixes a nested model test. * Move shininess service call to Visual::Init The pr2 test was failing due to timeouts in the shininess service call. Move the call from Visual::Load to Visual::Init to allow the service advertisement in World::Load some time to take effect. * Require SDFormat 9.8 * conda CI: don't install dartsim for now Workaround for an issue with libsdformat 9.8 Signed-off-by: Steve Peters --- .github/workflows/conda-forge.yml | 4 ++- cmake/SearchForStuff.cmake | 2 +- gazebo/physics/World.cc | 43 ++++++++++++++++-------------- gazebo/rendering/Visual.cc | 44 ++++++++++++++++--------------- gazebo/rendering/Visual.hh | 2 -- 5 files changed, 50 insertions(+), 45 deletions(-) diff --git a/.github/workflows/conda-forge.yml b/.github/workflows/conda-forge.yml index 99aa3c2fde..a2e7b4aa87 100644 --- a/.github/workflows/conda-forge.yml +++ b/.github/workflows/conda-forge.yml @@ -37,7 +37,9 @@ jobs: # Compilation related dependencies mamba install cmake compilers make ninja pkg-config # Actual dependencies - mamba install libprotobuf libsdformat libignition-cmake2 libignition-math6 libignition-transport8 libignition-common3 libignition-fuel-tools4 qt=5.12.9=*_4 ogre=1.10 freeimage curl tbb-devel qwt tinyxml2 libccd boost-cpp libcurl tinyxml bzip2 zlib ffmpeg graphviz libgdal libusb bullet-cpp dartsim simbody hdf5 openal-soft glib gts + # re-add dartsim once the libsdformat 9.8 issue is resolved + # https://github.com/osrf/gazebo/pull/3223#issuecomment-1140019713 + mamba install libprotobuf libsdformat libignition-cmake2 libignition-math6 libignition-transport8 libignition-common3 libignition-fuel-tools4 qt=5.12.9=*_4 ogre=1.10 freeimage curl tbb-devel qwt tinyxml2 libccd boost-cpp libcurl tinyxml bzip2 zlib ffmpeg graphviz libgdal libusb bullet-cpp simbody hdf5 openal-soft glib gts - name: Linux-only Dependencies [Linux] if: contains(matrix.os, 'ubuntu') diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake index bef281b4e5..75130604fd 100644 --- a/cmake/SearchForStuff.cmake +++ b/cmake/SearchForStuff.cmake @@ -608,7 +608,7 @@ endif () ######################################## # Find SDFormat -set(SDF_MIN_REQUIRED_VERSION 9.3) +set(SDF_MIN_REQUIRED_VERSION 9.8) find_package(sdformat9 ${SDF_MIN_REQUIRED_VERSION} REQUIRED) if (sdformat9_FOUND) message (STATUS "Looking for SDFormat9 - found") diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index b1f43a3937..4c469114e2 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -1259,29 +1259,32 @@ ModelPtr World::LoadModel(sdf::ElementPtr _sdf , BasePtr _parent) } } - sdf::ElementPtr linkElem = _sdf->GetElement("link"); - if (linkElem->HasElement("visual") && - linkElem->GetElement("visual")->HasElement("material")) + if (_sdf->HasElement("link")) { - sdf::ElementPtr matElem = linkElem->GetElement("visual")-> - GetElement("material"); - - if (matElem->HasElement("shininess")) - { - this->dataPtr->materialShininessMap[modelName] = - matElem->Get("shininess"); - } - else + sdf::ElementPtr linkElem = _sdf->GetElement("link"); + if (linkElem->HasElement("visual") && + linkElem->GetElement("visual")->HasElement("material")) { - this->dataPtr->materialShininessMap[modelName] = 0; - } + sdf::ElementPtr matElem = linkElem->GetElement("visual")-> + GetElement("material"); - std::string materialShininessService("/" + modelName + "/shininess"); - if (!this->dataPtr->ignNode.Advertise(materialShininessService, - &World::MaterialShininessService, this)) - { - gzerr << "Error advertising service [" - << materialShininessService << "]" << std::endl; + if (matElem->HasElement("shininess")) + { + this->dataPtr->materialShininessMap[modelName] = + matElem->Get("shininess"); + } + else + { + this->dataPtr->materialShininessMap[modelName] = 0; + } + + std::string materialShininessService("/" + modelName + "/shininess"); + if (!this->dataPtr->ignNode.Advertise(materialShininessService, + &World::MaterialShininessService, this)) + { + gzerr << "Error advertising service [" + << materialShininessService << "]" << std::endl; + } } } diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index d7e50b7341..45ebfd7e24 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -20,6 +20,8 @@ #include #include +#include +#include #include "gazebo/msgs/msgs.hh" @@ -325,27 +327,6 @@ void Visual::Init() this->dataPtr->inheritTransparency = true; this->dataPtr->scale = ignition::math::Vector3d::One; - this->dataPtr->initialized = true; -} - -////////////////////////////////////////////////// -void Visual::LoadFromMsg(const boost::shared_ptr< msgs::Visual const> &_msg) -{ - this->dataPtr->sdf = msgs::VisualToSDF(*_msg.get()); - this->Load(); - this->UpdateFromMsg(_msg); -} - -////////////////////////////////////////////////// -void Visual::Load(sdf::ElementPtr _sdf) -{ - this->dataPtr->sdf->Copy(_sdf); - this->Load(); -} - -////////////////////////////////////////////////// -void Visual::Load() -{ if (this->dataPtr->sdf->HasElement("material")) { // Get shininess value from physics::World @@ -388,6 +369,27 @@ void Visual::Load() } } + this->dataPtr->initialized = true; +} + +////////////////////////////////////////////////// +void Visual::LoadFromMsg(const boost::shared_ptr< msgs::Visual const> &_msg) +{ + this->dataPtr->sdf = msgs::VisualToSDF(*_msg.get()); + this->Load(); + this->UpdateFromMsg(_msg); +} + +////////////////////////////////////////////////// +void Visual::Load(sdf::ElementPtr _sdf) +{ + this->dataPtr->sdf->Copy(_sdf); + this->Load(); +} + +////////////////////////////////////////////////// +void Visual::Load() +{ std::ostringstream stream; ignition::math::Pose3d pose; Ogre::MovableObject *obj = nullptr; diff --git a/gazebo/rendering/Visual.hh b/gazebo/rendering/Visual.hh index d6a6e0c17b..7921bf0e87 100644 --- a/gazebo/rendering/Visual.hh +++ b/gazebo/rendering/Visual.hh @@ -30,8 +30,6 @@ #include #include #include -#include -#include #include "gazebo/common/Mesh.hh" #include "gazebo/common/Time.hh" From 33f7e47c982a11094203ee7c8addd25598de3e7c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 27 May 2022 16:44:15 -0700 Subject: [PATCH 09/12] Prepare for 11.11.0 release (#3221) Signed-off-by: Steve Peters --- CMakeLists.txt | 4 ++-- Changelog.md | 33 +++++++++++++++++++++++++++++++++ 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b77db68b0..3e0ae3d95f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,11 +18,11 @@ string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER) set (GAZEBO_MAJOR_VERSION 11) -set (GAZEBO_MINOR_VERSION 10) +set (GAZEBO_MINOR_VERSION 11) # The patch version may have been bumped for prerelease purposes; be sure to # check gazebo-release/ubuntu/debian/changelog@default to determine what the # next patch version should be for a regular release. -set (GAZEBO_PATCH_VERSION 2) +set (GAZEBO_PATCH_VERSION 0) set (GAZEBO_VERSION ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}) set (GAZEBO_VERSION_FULL ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}.${GAZEBO_PATCH_VERSION}) diff --git a/Changelog.md b/Changelog.md index 646063ad35..58d5ce8dbb 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,37 @@ ## Gazebo 11 +## Gazebo 11.11.0 (2022-05-25) + +1. Convert `OGRE_RESOURCE_PATH` with `TO_CMAKE_PATH` + * [Pull request #3165](https://github.com/osrf/gazebo/pull/3165) + * A contrubition from Silvio Traversaro + +1. Fix compatibility with FFmpeg 5.0 + * [Pull request #3195](https://github.com/osrf/gazebo/pull/3195) + * A contrubition from Silvio Traversaro + +1. Get/Set friction coefficients of wheels + * [Pull request #3219](https://github.com/osrf/gazebo/pull/3219) + +1. Support `` element when setting up camera + * [Pull request #3201](https://github.com/osrf/gazebo/pull/3201) + +1. Added shininess to models + * [Pull request #3213](https://github.com/osrf/gazebo/pull/3213) + * [Pull request #3223](https://github.com/osrf/gazebo/pull/3223) + +1. Fix missing namespace for string + * [Pull request #3211](https://github.com/osrf/gazebo/pull/3211) + +1. Fix typo in TBB target check in `gazebo-config.cmake.in` + * [Pull request #3207](https://github.com/osrf/gazebo/pull/3207) + +1. Separate cache files for each heightmap LOD + * [Pull request #3200](https://github.com/osrf/gazebo/pull/3200) + +1. Parse `ode_quiet` physics parameter from SDFormat + * [Pull request #3194](https://github.com/osrf/gazebo/pull/3194) + ## Gazebo 11.10.2 (2022-03-19) 1. Support plotting for entities with / in the name @@ -5051,3 +5083,4 @@ compilation on Windows. * Numerous bug fixes * APT repository hosted at [http://osrfoundation.org OSRF] * Improved process control prevents zombie processes + From e2849f96ac638bdd21b79e2300c1433beb2e2001 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 7 Jun 2022 16:17:50 -0700 Subject: [PATCH 10/12] Updated README links (#3226) Signed-off-by: Jenn Nguyen --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 884a6c0d85..aef6c2848f 100644 --- a/README.md +++ b/README.md @@ -4,13 +4,13 @@ Gazebo - A dynamic multi-robot simulator This is the Gazebo simulator. Gazebo simulates multiple robots in a 3D environment, with extensive dynamic interaction between objects. - http://gazebosim.org + http://classic.gazebosim.org Installation ------------ Instructions are located at - http://gazebosim.org/install + http://classic.gazebosim.org/install Gazebo cmake parameters available at configuring time: @@ -51,7 +51,7 @@ Gazebo cmake parameters available at configuring time: Uninstallation -------------- -Read the uninstallation instructions (http://gazebosim.org/uninstall) in the +Read the uninstallation instructions (http://classic.gazebosim.org/uninstall) in the online manual for generic instructions. For most people, the following sequence will suffice (might need sudo if it installed in /usr): From 9f41863cbb7870d2aa7a78f213e35ef9c55fb5f7 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 29 Jun 2022 10:52:52 -0700 Subject: [PATCH 11/12] Fix shininess and add tests (#3231) * Add Visual::Shininess accessor and corresponding visual_shininess integration test that confirms shininess values are properly added to Visuals in the shapes_shininess example world. * Fix the test by reverting part of #3223, moving the shininess service call back to Visual::Load. A different fix for the PR2 tests is made by checking for existence of shininess service before calling. Signed-off-by: Steve Peters --- gazebo/rendering/Visual.cc | 96 +++++++++++++--------- gazebo/rendering/Visual.hh | 4 + gazebo/rendering/Visual_TEST.cc | 2 + test/integration/CMakeLists.txt | 1 + test/integration/visual_shininess.cc | 116 +++++++++++++++++++++++++++ 5 files changed, 182 insertions(+), 37 deletions(-) create mode 100644 test/integration/visual_shininess.cc diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index 45ebfd7e24..f4cb663329 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -327,6 +327,27 @@ void Visual::Init() this->dataPtr->inheritTransparency = true; this->dataPtr->scale = ignition::math::Vector3d::One; + this->dataPtr->initialized = true; +} + +////////////////////////////////////////////////// +void Visual::LoadFromMsg(const boost::shared_ptr &_msg) +{ + this->dataPtr->sdf = msgs::VisualToSDF(*_msg.get()); + this->Load(); + this->UpdateFromMsg(_msg); +} + +////////////////////////////////////////////////// +void Visual::Load(sdf::ElementPtr _sdf) +{ + this->dataPtr->sdf->Copy(_sdf); + this->Load(); +} + +////////////////////////////////////////////////// +void Visual::Load() +{ if (this->dataPtr->sdf->HasElement("material")) { // Get shininess value from physics::World @@ -341,55 +362,50 @@ void Visual::Init() const std::string validServiceName = ignition::transport::TopicUtils::AsValidTopic(serviceName); + bool tryServiceCall = true; if (validServiceName.empty()) { gzerr << "Service name [" << serviceName << "] not valid" << std::endl; - return; + tryServiceCall = false; } - ignition::msgs::StringMsg req; - req.set_data(visualName); - - bool result; - unsigned int timeout = 5000; - bool executed = node.Request(validServiceName, req, timeout, rep, result); - - if (executed) { - if (result) - this->dataPtr->shininess = rep.double_value(); - else - gzerr << "Service call [" << validServiceName << "] failed" + std::vector publishers; + if (!node.ServiceInfo(validServiceName, publishers)) + { + gzerr << "Service name [" << validServiceName << "] not advertised, " + << "not attempting to load shininess for visual with name [" + << this->Name() << "]." << std::endl; + tryServiceCall = false; + } } - else - { - gzerr << "Service call [" << validServiceName << "] timed out" - << std::endl; - } - } - this->dataPtr->initialized = true; -} + if (tryServiceCall) + { + ignition::msgs::StringMsg req; + req.set_data(visualName); -////////////////////////////////////////////////// -void Visual::LoadFromMsg(const boost::shared_ptr< msgs::Visual const> &_msg) -{ - this->dataPtr->sdf = msgs::VisualToSDF(*_msg.get()); - this->Load(); - this->UpdateFromMsg(_msg); -} + bool result; + unsigned int timeout = 5000; + bool executed = node.Request(validServiceName, req, timeout, rep, result); -////////////////////////////////////////////////// -void Visual::Load(sdf::ElementPtr _sdf) -{ - this->dataPtr->sdf->Copy(_sdf); - this->Load(); -} + if (executed) + { + if (result) + this->dataPtr->shininess = rep.double_value(); + else + gzerr << "Service call [" << validServiceName << "] failed" + << std::endl; + } + else + { + gzerr << "Service call [" << validServiceName << "] timed out" + << std::endl; + } + } + } -////////////////////////////////////////////////// -void Visual::Load() -{ std::ostringstream stream; ignition::math::Pose3d pose; Ogre::MovableObject *obj = nullptr; @@ -1575,6 +1591,12 @@ ignition::math::Color Visual::Emissive() const return this->dataPtr->emissive; } +///////////////////////////////////////////////// +double Visual::Shininess() const +{ + return this->dataPtr->shininess; +} + ////////////////////////////////////////////////// void Visual::SetWireframe(bool _show) { diff --git a/gazebo/rendering/Visual.hh b/gazebo/rendering/Visual.hh index 7921bf0e87..d396e459ce 100644 --- a/gazebo/rendering/Visual.hh +++ b/gazebo/rendering/Visual.hh @@ -272,6 +272,10 @@ namespace gazebo /// \return Emissive color. public: ignition::math::Color Emissive() const; + /// \brief Get the shininess value of the visual. + /// \return Floating-point shininess value. + public: double Shininess() const; + /// \brief Enable or disable wireframe for this visual. /// \param[in] _show True to enable wireframe for this visual. public: void SetWireframe(bool _show); diff --git a/gazebo/rendering/Visual_TEST.cc b/gazebo/rendering/Visual_TEST.cc index e69f571095..a9fc686399 100644 --- a/gazebo/rendering/Visual_TEST.cc +++ b/gazebo/rendering/Visual_TEST.cc @@ -951,6 +951,7 @@ TEST_F(Visual_TEST, Color) EXPECT_EQ(cylinderVis->Diffuse(), ignmath::Color::Black); EXPECT_EQ(cylinderVis->Specular(), ignmath::Color::Black); EXPECT_EQ(cylinderVis->Emissive(), ignmath::Color::Black); + EXPECT_DOUBLE_EQ(0.0, cylinderVis->Shininess()); sdf::ElementPtr cylinderSDF2(new sdf::Element); sdf::initFile("visual.sdf", cylinderSDF2); @@ -965,6 +966,7 @@ TEST_F(Visual_TEST, Color) EXPECT_EQ(cylinderVis2->Diffuse(), ignmath::Color::Blue); EXPECT_EQ(cylinderVis2->Specular(), ignmath::Color::Red); EXPECT_EQ(cylinderVis2->Emissive(), ignmath::Color::Yellow); + EXPECT_DOUBLE_EQ(0.0, cylinderVis2->Shininess()); // test changing ambient/diffuse/specular colors { diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 07365b666d..3881c106fe 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -205,6 +205,7 @@ set(dri_tests road.cc speed_pr2.cc visual.cc + visual_shininess.cc wideanglecamera_sensor.cc world_remove.cc world_reset.cc diff --git a/test/integration/visual_shininess.cc b/test/integration/visual_shininess.cc new file mode 100644 index 0000000000..5f5de6970e --- /dev/null +++ b/test/integration/visual_shininess.cc @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "test_config.h" + +#include "gazebo/common/Timer.hh" +#include "gazebo/rendering/rendering.hh" + +#include "gazebo/test/ServerFixture.hh" + +using namespace gazebo; +class VisualShininess : public RenderingFixture +{ +}; + +///////////////////////////////////////////////// +void CheckShininessService(const std::string &_modelName, + double _expectedShininess) +{ + std::string serviceName = '/' + _modelName + "/shininess"; + ignition::transport::Node ignNode; + + { + std::vector publishers; + EXPECT_TRUE(ignNode.ServiceInfo(serviceName, publishers)); + } + + ignition::msgs::StringMsg request; + gazebo::msgs::Any reply; + const unsigned int timeout = 3000; + bool result = false; + request.set_data(_modelName); + EXPECT_TRUE(ignNode.Request(serviceName, request, timeout, reply, result)); + EXPECT_TRUE(result); + EXPECT_EQ(msgs::Any_ValueType_DOUBLE, reply.type()); + EXPECT_DOUBLE_EQ(_expectedShininess, reply.double_value()) << _modelName; +} + +///////////////////////////////////////////////// +TEST_F(VisualShininess, ShapesShininessServices) +{ + Load("worlds/shapes_shininess.world", true); + + CheckShininessService("ground_plane", 0.0); + CheckShininessService("box", 1.0); + CheckShininessService("sphere", 5.0); + CheckShininessService("cylinder", 10.0); +} + +///////////////////////////////////////////////// +TEST_F(VisualShininess, ShapesShininess) +{ + Load("worlds/shapes_shininess.world"); + + // Make sure the render engine is available. + if (rendering::RenderEngine::Instance()->GetRenderPathType() == + rendering::RenderEngine::NONE) + { + gzerr << "No rendering engine, unable to run shininess test" + << std::endl; + return; + } + gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene(); + ASSERT_NE(nullptr, scene); + + // Wait until all models are inserted + unsigned int sleep = 0; + unsigned int maxSleep = 30; + rendering::VisualPtr box, sphere, cylinder; + while ((!box || !sphere || !cylinder) && sleep < maxSleep) + { + event::Events::preRender(); + event::Events::render(); + event::Events::postRender(); + + box = scene->GetVisual("box"); + cylinder = scene->GetVisual("cylinder"); + sphere = scene->GetVisual("sphere"); + + common::Time::MSleep(100); + sleep++; + } + ASSERT_NE(nullptr, box); + ASSERT_NE(nullptr, cylinder); + ASSERT_NE(nullptr, sphere); + EXPECT_TRUE(scene->Initialized()); + + std::unordered_map nameToShininess; + nameToShininess["box::link::visual"] = 1.0; + nameToShininess["sphere::link::visual"] = 5.0; + nameToShininess["cylinder::link::visual"] = 10.0; + + for (const auto &nameShininess : nameToShininess) + { + rendering::VisualPtr visual = scene->GetVisual(nameShininess.first); + ASSERT_NE(nullptr, visual); + + // check shininess value + EXPECT_DOUBLE_EQ(nameShininess.second, visual->Shininess()) + << nameShininess.first; + } +} From 3e3e019f72e471fbb93d43ca024f3032ed6d8299 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 1 Jul 2022 14:23:20 -0700 Subject: [PATCH 12/12] Support shininess value for each Visual in a Model (#3235) This moves the parsing of shininess values from World::LoadModel to Link::UpdateVisualMsg and stores the values by scoped Visual name rather than by Model name. The per-model gz services are also replaced by a single /shininess service. A World::ShininessByScopedName method for accessing the shininess map is provided, and a mutex is used to protect access to the map. Signed-off-by: Steve Peters --- gazebo/physics/Link.cc | 15 +++++-- gazebo/physics/World.cc | 59 ++++++++++++++-------------- gazebo/physics/World.hh | 15 ++++++- gazebo/physics/WorldPrivate.hh | 4 ++ gazebo/rendering/Visual.cc | 5 +-- test/integration/visual_shininess.cc | 18 +++++---- worlds/shapes_shininess.world | 12 ++++++ 7 files changed, 82 insertions(+), 46 deletions(-) diff --git a/gazebo/physics/Link.cc b/gazebo/physics/Link.cc index 60291cf472..5b17b55a90 100644 --- a/gazebo/physics/Link.cc +++ b/gazebo/physics/Link.cc @@ -1545,13 +1545,21 @@ void Link::UpdateVisualMsg() common::resolveSdfPose(visualDom->SemanticPose())); } bool newVis = true; - std::string linkName = this->GetScopedName(); + std::string visName = this->GetScopedName() + "::" + msg.name(); + + if (visualElem->HasElement("material")) + { + sdf::ElementPtr matElem = visualElem->GetElement("material"); + if (matElem->HasElement("shininess")) + { + this->world->SetVisualShininess( + visName, matElem->Get("shininess")); + } + } // update visual msg if it exists for (auto &iter : this->visuals) { - std::string visName = linkName + "::" + - visualElem->Get("name"); if (iter.second.name() == visName) { iter.second.mutable_geometry()->CopyFrom(msg.geometry()); @@ -1563,7 +1571,6 @@ void Link::UpdateVisualMsg() // add to visual msgs if not found. if (newVis) { - std::string visName = this->GetScopedName() + "::" + msg.name(); msg.set_name(visName); msg.set_id(physics::getUniqueId()); msg.set_parent_name(this->GetScopedName()); diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index 4c469114e2..b0f1ca6194 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -334,6 +334,14 @@ void World::Load(sdf::ElementPtr _sdf) shadowCasterRenderBackFacesService << "]" << std::endl; } + std::string materialShininessService("/shininess"); + if (!this->dataPtr->ignNode.Advertise(materialShininessService, + &World::MaterialShininessService, this)) + { + gzerr << "Error advertising service [" + << materialShininessService << "]" << std::endl; + } + // This should come before loading of entities sdf::ElementPtr physicsElem = this->dataPtr->sdf->GetElement("physics"); @@ -1259,35 +1267,6 @@ ModelPtr World::LoadModel(sdf::ElementPtr _sdf , BasePtr _parent) } } - if (_sdf->HasElement("link")) - { - sdf::ElementPtr linkElem = _sdf->GetElement("link"); - if (linkElem->HasElement("visual") && - linkElem->GetElement("visual")->HasElement("material")) - { - sdf::ElementPtr matElem = linkElem->GetElement("visual")-> - GetElement("material"); - - if (matElem->HasElement("shininess")) - { - this->dataPtr->materialShininessMap[modelName] = - matElem->Get("shininess"); - } - else - { - this->dataPtr->materialShininessMap[modelName] = 0; - } - - std::string materialShininessService("/" + modelName + "/shininess"); - if (!this->dataPtr->ignNode.Advertise(materialShininessService, - &World::MaterialShininessService, this)) - { - gzerr << "Error advertising service [" - << materialShininessService << "]" << std::endl; - } - } - } - model = this->dataPtr->physicsEngine->CreateModel(_parent); model->SetWorld(shared_from_this()); model->Load(_sdf); @@ -3467,11 +3446,31 @@ bool World::ShadowCasterRenderBackFacesService(ignition::msgs::Boolean &_res) return true; } +////////////////////////////////////////////////// +void World::SetVisualShininess(const std::string &_scopedName, + double _shininess) +{ + std::lock_guard lock(this->dataPtr->materialShininessMutex); + this->dataPtr->materialShininessMap[_scopedName] = _shininess; +} + +////////////////////////////////////////////////// +double World::ShininessByScopedName(const std::string &_scopedName) const +{ + std::lock_guard lock(this->dataPtr->materialShininessMutex); + if (this->dataPtr->materialShininessMap.find(_scopedName) != + this->dataPtr->materialShininessMap.end()) + { + return this->dataPtr->materialShininessMap.at(_scopedName); + } + return 0.0; +} + ////////////////////////////////////////////////// bool World::MaterialShininessService( const ignition::msgs::StringMsg &_req, msgs::Any &_res) { _res.set_type(msgs::Any::DOUBLE); - _res.set_double_value(this->dataPtr->materialShininessMap[_req.data()]); + _res.set_double_value(this->ShininessByScopedName(_req.data())); return true; } diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index 3ed2ac4315..5e709ce8d7 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -461,9 +461,15 @@ namespace gazebo public: std::string UniqueModelName(const std::string &_name); /// \brief Set callback 'waitForSensors' - /// \param[in] function to be called + /// \param[in] _func function to be called public: void SetSensorWaitFunc(std::function _func); + /// \brief Set Visual shininess value by scoped name + /// \param[in] _scopedName Scoped name of visual. + /// \param[in] _shininess Shininess value. + public: void SetVisualShininess(const std::string &_scopedName, + double _shininess); + /// \cond /// This is an internal function. /// \brief Get a model by id. @@ -678,6 +684,13 @@ namespace gazebo private: bool MaterialShininessService( const ignition::msgs::StringMsg &_request, msgs::Any &_response); + /// \brief Helper function for getting shininess values by scoped + /// visual name. + /// \param[in] _scopedName Scoped visual name. + /// \return Shininess value. + private: double ShininessByScopedName(const std::string &_scopedName) + const; + /// \internal /// \brief Private data pointer. private: std::unique_ptr dataPtr; diff --git a/gazebo/physics/WorldPrivate.hh b/gazebo/physics/WorldPrivate.hh index d1bf78c42d..694813c61e 100644 --- a/gazebo/physics/WorldPrivate.hh +++ b/gazebo/physics/WorldPrivate.hh @@ -398,6 +398,10 @@ namespace gazebo /// \brief Shadow caster render back faces from scene SDF public: bool shadowCasterRenderBackFaces = true; + /// \brief This mutex is used to by the SetVisualShininess and + /// ShininessByScopedName methods to protect materialShininessMap. + public: std::mutex materialShininessMutex; + /// \brief Shininess values from scene SDF public: std::map materialShininessMap; }; diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index f4cb663329..c83f7f8c57 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -354,10 +354,9 @@ void Visual::Load() ignition::transport::Node node; msgs::Any rep; - const std::string visualName = - this->Name().substr(0, this->Name().find(":")); + const std::string visualName = this->Name(); - const std::string serviceName = "/" + visualName + "/shininess"; + const std::string serviceName = "/shininess"; const std::string validServiceName = ignition::transport::TopicUtils::AsValidTopic(serviceName); diff --git a/test/integration/visual_shininess.cc b/test/integration/visual_shininess.cc index 5f5de6970e..1bd46d6310 100644 --- a/test/integration/visual_shininess.cc +++ b/test/integration/visual_shininess.cc @@ -28,10 +28,10 @@ class VisualShininess : public RenderingFixture }; ///////////////////////////////////////////////// -void CheckShininessService(const std::string &_modelName, +void CheckShininessService(const std::string &_scopedName, double _expectedShininess) { - std::string serviceName = '/' + _modelName + "/shininess"; + std::string serviceName = "/shininess"; ignition::transport::Node ignNode; { @@ -43,11 +43,11 @@ void CheckShininessService(const std::string &_modelName, gazebo::msgs::Any reply; const unsigned int timeout = 3000; bool result = false; - request.set_data(_modelName); + request.set_data(_scopedName); EXPECT_TRUE(ignNode.Request(serviceName, request, timeout, reply, result)); EXPECT_TRUE(result); EXPECT_EQ(msgs::Any_ValueType_DOUBLE, reply.type()); - EXPECT_DOUBLE_EQ(_expectedShininess, reply.double_value()) << _modelName; + EXPECT_DOUBLE_EQ(_expectedShininess, reply.double_value()) << _scopedName; } ///////////////////////////////////////////////// @@ -55,10 +55,11 @@ TEST_F(VisualShininess, ShapesShininessServices) { Load("worlds/shapes_shininess.world", true); - CheckShininessService("ground_plane", 0.0); - CheckShininessService("box", 1.0); - CheckShininessService("sphere", 5.0); - CheckShininessService("cylinder", 10.0); + CheckShininessService("ground_plane::link::visual", 0.0); + CheckShininessService("box::link::visual", 1.0); + CheckShininessService("box::link::visual2", 10.0); + CheckShininessService("sphere::link::visual", 5.0); + CheckShininessService("cylinder::link::visual", 10.0); } ///////////////////////////////////////////////// @@ -101,6 +102,7 @@ TEST_F(VisualShininess, ShapesShininess) std::unordered_map nameToShininess; nameToShininess["box::link::visual"] = 1.0; + nameToShininess["box::link::visual2"] = 10.0; nameToShininess["sphere::link::visual"] = 5.0; nameToShininess["cylinder::link::visual"] = 10.0; diff --git a/worlds/shapes_shininess.world b/worlds/shapes_shininess.world index 85c30333a9..1f48211428 100644 --- a/worlds/shapes_shininess.world +++ b/worlds/shapes_shininess.world @@ -28,6 +28,18 @@ + + 0 0 3 0 0 0 + + 1 0 0 1 + 10 + + + + 1 1 1 + + +