From d109b08e6bc615dae0c10ffb0eb27b9465f54f7c Mon Sep 17 00:00:00 2001 From: Crola1702 Date: Mon, 30 Jan 2023 13:32:04 +0000 Subject: [PATCH 1/8] Fix warning C4244 Signed-off-by: Crola1702 --- src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 3fc0458e08..98d5d79d57 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -805,10 +805,10 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Forces buffer is composed of XYZ coordinates, while _msg buffer is // made up of XYZRGB values bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); - normalForcesBuffer.get()[bufferIndex] = normalForce.X(); - normalForcesBuffer.get()[bufferIndex + sizeof(float)] = normalForce.Y(); + normalForcesBuffer.get()[bufferIndex] = static_cast(normalForce.X()); + normalForcesBuffer.get()[bufferIndex + sizeof(float)] = static_cast(normalForce.Y()); normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] = - normalForce.Z(); + static_cast(normalForce.Z()); if (!_visualizeForces) continue; From a1d912366db9349be591cae419e3b2ef81c03468 Mon Sep 17 00:00:00 2001 From: Crola1702 Date: Mon, 30 Jan 2023 13:32:37 +0000 Subject: [PATCH 2/8] Fix warning C4305 Signed-off-by: Crola1702 --- src/Conversions_TEST.cc | 8 ++++---- test/integration/scene_broadcaster_system.cc | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index f1a3809381..0d7462c279 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -999,8 +999,8 @@ TEST(Conversions, ParticleEmitter) emitter.SetMaxVelocity(0.2); emitter.SetSize(math::Vector3d(1, 2, 3)); emitter.SetParticleSize(math::Vector3d(4, 5, 6)); - emitter.SetColorStart(math::Color(0.1, 0.2, 0.3)); - emitter.SetColorEnd(math::Color(0.4, 0.5, 0.6)); + emitter.SetColorStart(math::Color(0.1f, 0.2f, 0.3f)); + emitter.SetColorEnd(math::Color(0.4f, 0.5f, 0.6f)); emitter.SetColorRangeImage("range_image"); emitter.SetTopic("my_topic"); emitter.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); @@ -1030,9 +1030,9 @@ TEST(Conversions, ParticleEmitter) EXPECT_NEAR(0.2, emitterMsg.max_velocity().data(), 1e-3); EXPECT_EQ(math::Vector3d(1, 2, 3), msgs::Convert(emitterMsg.size())); EXPECT_EQ(math::Vector3d(4, 5, 6), msgs::Convert(emitterMsg.particle_size())); - EXPECT_EQ(math::Color(0.1, 0.2, 0.3), + EXPECT_EQ(math::Color(0.1f, 0.2f, 0.3f), msgs::Convert(emitterMsg.color_start())); - EXPECT_EQ(math::Color(0.4, 0.5, 0.6), msgs::Convert(emitterMsg.color_end())); + EXPECT_EQ(math::Color(0.4f, 0.5f, 0.6f), msgs::Convert(emitterMsg.color_end())); EXPECT_EQ("range_image", emitterMsg.color_range_image().data()); auto header = emitterMsg.header().data(0); diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index c1f9314bc0..01447c6a50 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -952,10 +952,10 @@ TEST_P(SceneBroadcasterTest, EXPECT_TRUE(result); ASSERT_TRUE(res.has_ambient()); - EXPECT_EQ(math::Color(1.0, 1.0, 1.0, 1.0), msgs::Convert(res.ambient())); + EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f), msgs::Convert(res.ambient())); ASSERT_TRUE(res.has_background()); - EXPECT_EQ(math::Color(0.8, 0.8, 0.8, 1.0), msgs::Convert(res.background())); + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1.0f), msgs::Convert(res.background())); EXPECT_TRUE(res.shadows()); EXPECT_FALSE(res.grid()); From 781be125d4efb4d750aa24758ac2ab05f84ea13b Mon Sep 17 00:00:00 2001 From: Crola1702 Date: Mon, 30 Jan 2023 09:05:42 -0500 Subject: [PATCH 3/8] Fix line lenght linter error Signed-off-by: Crola1702 --- src/Conversions_TEST.cc | 3 ++- src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 0d7462c279..40d0140973 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -1032,7 +1032,8 @@ TEST(Conversions, ParticleEmitter) EXPECT_EQ(math::Vector3d(4, 5, 6), msgs::Convert(emitterMsg.particle_size())); EXPECT_EQ(math::Color(0.1f, 0.2f, 0.3f), msgs::Convert(emitterMsg.color_start())); - EXPECT_EQ(math::Color(0.4f, 0.5f, 0.6f), msgs::Convert(emitterMsg.color_end())); + EXPECT_EQ(math::Color(0.4f, 0.5f, 0.6f), + msgs::Convert(emitterMsg.color_end())); EXPECT_EQ("range_image", emitterMsg.color_range_image().data()); auto header = emitterMsg.header().data(0); diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 98d5d79d57..c67779a68a 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -805,8 +805,10 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Forces buffer is composed of XYZ coordinates, while _msg buffer is // made up of XYZRGB values bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); - normalForcesBuffer.get()[bufferIndex] = static_cast(normalForce.X()); - normalForcesBuffer.get()[bufferIndex + sizeof(float)] = static_cast(normalForce.Y()); + normalForcesBuffer.get()[bufferIndex] = + static_cast(normalForce.X()); + normalForcesBuffer.get()[bufferIndex + sizeof(float)] = + static_cast(normalForce.Y()); normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] = static_cast(normalForce.Z()); From c1d4a860a84d2b0fe1ba68070b1056d50a3e7ac3 Mon Sep 17 00:00:00 2001 From: Crola1702 Date: Mon, 30 Jan 2023 12:06:52 -0500 Subject: [PATCH 4/8] Fix linter issues again Signed-off-by: Crola1702 --- src/Conversions_TEST.cc | 2 +- .../optical_tactile_plugin/OpticalTactilePlugin.cc | 8 ++++---- test/integration/scene_broadcaster_system.cc | 6 ++++-- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 40d0140973..ad862ee513 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -1032,7 +1032,7 @@ TEST(Conversions, ParticleEmitter) EXPECT_EQ(math::Vector3d(4, 5, 6), msgs::Convert(emitterMsg.particle_size())); EXPECT_EQ(math::Color(0.1f, 0.2f, 0.3f), msgs::Convert(emitterMsg.color_start())); - EXPECT_EQ(math::Color(0.4f, 0.5f, 0.6f), + EXPECT_EQ(math::Color(0.4f, 0.5f, 0.6f), msgs::Convert(emitterMsg.color_end())); EXPECT_EQ("range_image", emitterMsg.color_range_image().data()); diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index c67779a68a..35fd5b3c87 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -805,10 +805,10 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Forces buffer is composed of XYZ coordinates, while _msg buffer is // made up of XYZRGB values bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); - normalForcesBuffer.get()[bufferIndex] = - static_cast(normalForce.X()); - normalForcesBuffer.get()[bufferIndex + sizeof(float)] = - static_cast(normalForce.Y()); + normalForcesBuffer.get()[bufferIndex] = + static_cast(normalForce.X()); + normalForcesBuffer.get()[bufferIndex + sizeof(float)] = + static_cast(normalForce.Y()); normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] = static_cast(normalForce.Z()); diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 01447c6a50..c30a5e6eab 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -952,10 +952,12 @@ TEST_P(SceneBroadcasterTest, EXPECT_TRUE(result); ASSERT_TRUE(res.has_ambient()); - EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f), msgs::Convert(res.ambient())); + EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f), + msgs::Convert(res.ambient())); ASSERT_TRUE(res.has_background()); - EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1.0f), msgs::Convert(res.background())); + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1.0f), + msgs::Convert(res.background())); EXPECT_TRUE(res.shadows()); EXPECT_FALSE(res.grid()); From 3f6019de47cf67e9d7a6f09690aa6a5007d48edd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crist=C3=B3bal=20Arroyo?= <69475004+Crola1702@users.noreply.github.com> Date: Tue, 31 Jan 2023 08:24:50 -0500 Subject: [PATCH 5/8] Update src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc Co-authored-by: Addisu Z. Taddese Signed-off-by: Crola1702 --- src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 35fd5b3c87..f46897cd54 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -805,7 +805,7 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Forces buffer is composed of XYZ coordinates, while _msg buffer is // made up of XYZRGB values bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); - normalForcesBuffer.get()[bufferIndex] = + std::memcpy(&normalForcesBuffer.get()[bufferIndex], &normalForce.X(), sizeof(float)); static_cast(normalForce.X()); normalForcesBuffer.get()[bufferIndex + sizeof(float)] = static_cast(normalForce.Y()); From 3dbaf13b6a14b9678c72ecc6fabbb13355c79ca7 Mon Sep 17 00:00:00 2001 From: Crola1702 Date: Tue, 31 Jan 2023 09:51:45 -0500 Subject: [PATCH 6/8] Apply PR suggestions Signed-off-by: Crola1702 --- .../optical_tactile_plugin/OpticalTactilePlugin.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index f46897cd54..f48d64e1dd 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -805,12 +805,12 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Forces buffer is composed of XYZ coordinates, while _msg buffer is // made up of XYZRGB values bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); - std::memcpy(&normalForcesBuffer.get()[bufferIndex], &normalForce.X(), sizeof(float)); - static_cast(normalForce.X()); - normalForcesBuffer.get()[bufferIndex + sizeof(float)] = - static_cast(normalForce.Y()); - normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] = - static_cast(normalForce.Z()); + std::memcpy(&normalForcesBuffer.get()[bufferIndex], + &normalForce.X(), sizeof(float)); + std::memcpy(&normalForcesBuffer.get()[bufferIndex] + sizeof(float), + &normalForce.Y(), sizeof(float)); + std::memcpy(&normalForcesBuffer.get()[bufferIndex] + 2 * sizeof(float), + &normalForce.Z(), sizeof(float)); if (!_visualizeForces) continue; From 5d27b81596bdeca528d89a44e278ba1458a90375 Mon Sep 17 00:00:00 2001 From: Crola1702 Date: Tue, 31 Jan 2023 11:05:56 -0500 Subject: [PATCH 7/8] Fix bracket position Signed-off-by: Crola1702 --- src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index f48d64e1dd..909a4fb077 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -807,9 +807,9 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); std::memcpy(&normalForcesBuffer.get()[bufferIndex], &normalForce.X(), sizeof(float)); - std::memcpy(&normalForcesBuffer.get()[bufferIndex] + sizeof(float), + std::memcpy(&normalForcesBuffer.get()[bufferIndex + sizeof(float)], &normalForce.Y(), sizeof(float)); - std::memcpy(&normalForcesBuffer.get()[bufferIndex] + 2 * sizeof(float), + std::memcpy(&normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)], &normalForce.Z(), sizeof(float)); if (!_visualizeForces) From 0a996781d47504f4083fab7dc4c0a93889138adf Mon Sep 17 00:00:00 2001 From: Crola1702 Date: Thu, 2 Feb 2023 21:01:05 +0000 Subject: [PATCH 8/8] Aplly test fix done in gz-sim7 (gz-sim #1771) More details: https://github.com/gazebosim/gz-sim/pull/1771/commits/0a84abe41c78e7a626828d0a59f593d6f1793d8b Signed-off-by: Crola1702 --- .../OpticalTactilePlugin.cc | 22 +++++++++---------- test/integration/optical_tactile_plugin.cc | 12 +++++----- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 909a4fb077..6007d95259 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -767,8 +767,7 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( normalsMsg.set_step(3 * sizeof(float) * _msg.width()); normalsMsg.set_pixel_format_type(ignition::msgs::PixelFormatType::R_FLOAT32); - uint32_t bufferSize = 3 * sizeof(float) * _msg.width() * _msg.height(); - std::shared_ptr normalForcesBuffer(new char[bufferSize]); + std::vector normalForcesBuffer(3 * _msg.width() * _msg.height()); uint32_t bufferIndex; // Marker messages representing the normal forces @@ -804,13 +803,11 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Add force to buffer // Forces buffer is composed of XYZ coordinates, while _msg buffer is // made up of XYZRGB values - bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); - std::memcpy(&normalForcesBuffer.get()[bufferIndex], - &normalForce.X(), sizeof(float)); - std::memcpy(&normalForcesBuffer.get()[bufferIndex + sizeof(float)], - &normalForce.Y(), sizeof(float)); - std::memcpy(&normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)], - &normalForce.Z(), sizeof(float)); + + bufferIndex = j * (_msg.width() * 3) + i * 3; + normalForcesBuffer[bufferIndex] = normalForce.X(); + normalForcesBuffer[bufferIndex+1] = normalForce.Y(); + normalForcesBuffer[bufferIndex+2] = normalForce.Z(); if (!_visualizeForces) continue; @@ -822,9 +819,12 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( } } + std::string *dataStr = normalsMsg.mutable_data(); + dataStr->resize(sizeof(float) * normalForcesBuffer.size()); + memcpy(&((*dataStr)[0]), normalForcesBuffer.data(), dataStr->size()); + // Publish message - normalsMsg.set_data(normalForcesBuffer.get(), - 3 * sizeof(float) * _msg.width() * _msg.height()); + this->normalForcesPub.Publish(normalsMsg); if (_visualizeForces) diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index bab4228a30..76fb2e4571 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -67,14 +67,14 @@ class OpticalTactilePluginTest : public InternalFixture<::testing::Test> uint32_t msgBufferIndex = _j * this->normalForces.step() + _i * 3 * sizeof(float); - measuredPoint.X() = static_cast( - msgBuffer[msgBufferIndex]); + measuredPoint.X() = *reinterpret_cast( + &msgBuffer[msgBufferIndex]); - measuredPoint.Y() = static_cast( - msgBuffer[msgBufferIndex + sizeof(float)]); + measuredPoint.Y() = *reinterpret_cast( + &msgBuffer[msgBufferIndex + sizeof(float)]); - measuredPoint.Z() = static_cast( - msgBuffer[msgBufferIndex + 2*sizeof(float)]); + measuredPoint.Z() = *reinterpret_cast( + &msgBuffer[msgBufferIndex + 2*sizeof(float)]); return measuredPoint; }