Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Bug fix in planning_scene, loadGeometryFromStream #113

Merged
merged 2 commits into from Jun 13, 2019

Conversation

Projects
None yet
3 participants
@vmayoral
Copy link
Member

commented Jun 12, 2019

Enabling ASan in the moveit2 tests, before this patch:

root@bf916bb1a977:/opt/ros2_moveit2_ws# build/moveit_core/planning_scene/test_planning_scene
[==========] Running 6 tests from 1 test case.
[----------] Global test environment set-up.
[----------] 6 tests from PlanningScene
[ RUN      ] PlanningScene.LoadRestore
[INFO] [robot_model]: Loading robot model 'pr2'...
[INFO] [robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[       OK ] PlanningScene.LoadRestore (796 ms)
[ RUN      ] PlanningScene.LoadRestoreDiff
[INFO] [robot_model]: Loading robot model 'pr2'...
[INFO] [robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[       OK ] PlanningScene.LoadRestoreDiff (699 ms)
[ RUN      ] PlanningScene.MakeAttachedDiff
[INFO] [robot_model]: Loading robot model 'pr2'...
[INFO] [robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[       OK ] PlanningScene.MakeAttachedDiff (697 ms)
[ RUN      ] PlanningScene.isStateValid
[INFO] [robot_model]: Loading robot model 'pr2'...
[       OK ] PlanningScene.isStateValid (547 ms)
[ RUN      ] PlanningScene.loadGoodSceneGeometry
[INFO] [robot_model]: Loading robot model 'pr2'...
[       OK ] PlanningScene.loadGoodSceneGeometry (437 ms)
[ RUN      ] PlanningScene.loadBadSceneGeometry
[INFO] [robot_model]: Loading robot model 'pr2'...
[ERROR] [moveit.planning_scene]: Bad input stream when loading marker in scene geometry
[ERROR] [moveit.planning_scene]: Improperly formatted color in scene geometry file
[       OK ] PlanningScene.loadBadSceneGeometry (466 ms)
[----------] 6 tests from PlanningScene (3643 ms total)

[----------] Global test environment tear-down
[==========] 6 tests from 1 test case ran. (3645 ms total)
[  PASSED  ] 6 tests.

=================================================================
==38461==ERROR: LeakSanitizer: detected memory leaks

Direct leak of 40 byte(s) in 1 object(s) allocated from:
    #0 0x7f9a7e0b7458 in operator new(unsigned long) (/usr/lib/x86_64-linux-gnu/libasan.so.4+0xe0458)
    #1 0x7f9a7d3480fd in shapes::constructShapeFromText(std::istream&) /opt/ros2_moveit2_ws/src/geometric_shapes/src/shape_operations.cpp:505
    #2 0x7f9a7dc7d561 in planning_scene::PlanningScene::loadGeometryFromStream(std::istream&, Eigen::Transform<double, 3, 1, 0> const&) /opt/ros2_moveit2_ws/src/moveit2/moveit_core/planning_scene/src/planning_scene.cpp:1077
    #3 0x7f9a7dc7c336 in planning_scene::PlanningScene::loadGeometryFromStream(std::istream&) /opt/ros2_moveit2_ws/src/moveit2/moveit_core/planning_scene/src/planning_scene.cpp:1043
    #4 0x555a087ffa9d in PlanningScene_loadBadSceneGeometry_Test::TestBody() /opt/ros2_moveit2_ws/src/moveit2/moveit_core/planning_scene/test/test_planning_scene.cpp:223
    #5 0x555a08888039 in void testing::internal::HandleSehExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:2447
    #6 0x555a0887a18d in void testing::internal::HandleExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:2483
    #7 0x555a088268b5 in testing::Test::Run() /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:2522
    #8 0x555a08827ce0 in testing::TestInfo::Run() /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:2703
    #9 0x555a08828884 in testing::TestCase::Run() /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:2825
    #10 0x555a08843995 in testing::internal::UnitTestImpl::RunAllTests() /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:5216
    #11 0x555a0888aaec in bool testing::internal::HandleSehExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:2447
    #12 0x555a0887c456 in bool testing::internal::HandleExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:2483
    #13 0x555a08840729 in testing::UnitTest::Run() /opt/ros/dashing/src/gtest_vendor/./src/gtest.cc:4824
    #14 0x555a08801ba5 in RUN_ALL_TESTS() (/opt/ros2_moveit2_ws/build/moveit_core/planning_scene/test_planning_scene+0x55ba5)
    #15 0x555a088000be in main /opt/ros2_moveit2_ws/src/moveit2/moveit_core/planning_scene/test/test_planning_scene.cpp:229
    #16 0x7f9a7b2a2b96 in __libc_start_main (/lib/x86_64-linux-gnu/libc.so.6+0x21b96)

SUMMARY: AddressSanitizer: 40 byte(s) leaked in 1 allocation(s).

After the patch:

root@bf916bb1a977:/opt/ros2_moveit2_ws# build-asan/moveit_core/planning_scene/test_planning_scene
[==========] Running 6 tests from 1 test case.
[----------] Global test environment set-up.
[----------] 6 tests from PlanningScene
[ RUN      ] PlanningScene.LoadRestore
[INFO] [robot_model]: Loading robot model 'pr2'...
[INFO] [robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[       OK ] PlanningScene.LoadRestore (601 ms)
[ RUN      ] PlanningScene.LoadRestoreDiff
[INFO] [robot_model]: Loading robot model 'pr2'...
[INFO] [robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[       OK ] PlanningScene.LoadRestoreDiff (535 ms)
[ RUN      ] PlanningScene.MakeAttachedDiff
[INFO] [robot_model]: Loading robot model 'pr2'...
[INFO] [robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[       OK ] PlanningScene.MakeAttachedDiff (526 ms)
[ RUN      ] PlanningScene.isStateValid
[INFO] [robot_model]: Loading robot model 'pr2'...
[       OK ] PlanningScene.isStateValid (465 ms)
[ RUN      ] PlanningScene.loadGoodSceneGeometry
[INFO] [robot_model]: Loading robot model 'pr2'...
[       OK ] PlanningScene.loadGoodSceneGeometry (431 ms)
[ RUN      ] PlanningScene.loadBadSceneGeometry
[INFO] [robot_model]: Loading robot model 'pr2'...
[ERROR] [moveit.planning_scene]: Bad input stream when loading marker in scene geometry
[ERROR] [moveit.planning_scene]: Improperly formatted color in scene geometry file
[       OK ] PlanningScene.loadBadSceneGeometry (425 ms)
[----------] 6 tests from PlanningScene (2984 ms total)

[----------] Global test environment tear-down
[==========] 6 tests from 1 test case ran. (2985 ms total)
[  PASSED  ] 6 tests.
Fix a bug in planning_scene loadGeometryFromStream
Using ASan the following memory leak was detected:
    #0 0x7f9a7e0b7458 in operator new(unsigned long)
(/usr/lib/x86_64-linux-gnu/libasan.so.4+0xe0458)
    #1 0x7f9a7d3480fd in shapes::constructShapeFromText(std::istream&)
/opt/ros2_moveit2_ws/src/geometric_shapes/src/shape_operations.cpp:505
    #2 0x7f9a7dc7d561 in planning_scene::PlanningScene::
loadGeometryFromStream(std::istream&, Eigen::Transform<double, 3, 1, 0>
const&) /opt/ros2_moveit2_ws/src/moveit2/moveit_core/planning_scene/
src/planning_scene.cpp:1077
    #3 0x7f9a7dc7c336 in planning_scene::PlanningScene::
loadGeometryFromStream(std::istream&) /opt/ros2_moveit2_ws/src/
moveit2/moveit_core/planning_scene/src/planning_scene.cpp:1043
    ...

The changes proposed here release the shapes::Shape instance
allocated and prevent the memory leak.
@ahcorde

This comment has been minimized.

Copy link

commented Jun 13, 2019

I don't understand why you are doing this.
if(!s) means that the object is not build, you should not destroy the object because is not there. The delete call should go in this other if.

I wouldn't accept this PR.

@LanderU
Copy link
Member

left a comment

Could you please address the indications from @ahcorde?

@vmayoral

This comment has been minimized.

Copy link
Member Author

commented Jun 13, 2019

@ahcorde is right about


it isn't necessary but the other are unless we want to incur a memory leek since we're returning without releasing the booked memory.

@vmayoral

This comment has been minimized.

Copy link
Member Author

commented Jun 13, 2019

@ahcorde, review the code again please.

@vmayoral

This comment has been minimized.

Copy link
Member Author

commented Jun 13, 2019

I don't understand why you are doing this.
if(!s) means that the object is not build, you should not destroy the object because is not there. The delete call should go in this other if.

First part addressed but the second is incorrect, there's no return in

if (s)
{
Eigen::Isometry3d pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz);
// Transform pose by input pose offset
pose = offset * pose;
world_->addToObject(ns, shapes::ShapePtr(s), pose);
if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f)
{
std_msgs::msg::ColorRGBA color;
color.r = r;
color.g = g;
color.b = b;
color.a = a;
setObjectColor(ns, color);
}
}
. Not necessary to delete memory IMHO here.

@vmayoral

This comment has been minimized.

Copy link
Member Author

commented Jun 13, 2019

Note that the changes apply fix the memory leaks when reviewing it with the ASan tags.

@ahcorde

This comment has been minimized.

Copy link

commented Jun 13, 2019

you are missing one delete more at the end of the for, right?

@vmayoral

This comment has been minimized.

Copy link
Member Author

commented Jun 13, 2019

you are missing one delete more at the end of the for, right?

I believe I haven't missed anything but feel free to make a suggestion if you think differently.

@ahcorde

This comment has been minimized.

Copy link

commented Jun 13, 2019

I mean here

s is not a smart pointer. Probably you need to delete it.

@vmayoral

This comment has been minimized.

Copy link
Member Author

commented Jun 13, 2019

I don't think that's needed @ahcorde. AFAIK, the scope of s is only the outer for which breaks either a) by itself (finishing the iterations) or b) by returning. This PR addresses only the b) case.

If you believe there's an issue with a), let's address it in another PR but make sure you reproduce my work above with the changes you propose.

@ahcorde ahcorde merged commit f1be628 into master Jun 13, 2019

0 of 2 checks passed

continuous-integration/travis-ci/pr The Travis CI build failed
Details
continuous-integration/travis-ci/push The Travis CI build failed
Details

@ahcorde ahcorde deleted the fix-bug-planning-scene branch Jun 13, 2019

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.