moveit2 setup assistant crashes -- Returning dirty collision body transforms

[LHY2021]

Description

By following Getting Started Tutorials(https://moveit.picknik.ai/humble/doc/tutorials/getting_started/getting_started.html), I have created a workspace for MoveIt2. By command "ros2 run moveit_setup_assistant moveit_setup_assistant", I can successfully access to the client. I can successfully generate collision matrix and add planning group. Howeveer, everytime I try to move the robot from the "Robot poses" menu, the client will crash. Running in verbose mode the appearing errors are the following:

[INFO] [1720080734.863226271] [moveit_setup_assistant.Start Screen]: Loading Setup Assistant Complete
[INFO] [1720080742.214238854] [collision_updater]: -------------------------------------------------------------------------------
[INFO] [1720080742.214266885] [collision_updater]: Statistics:
[INFO] [1720080742.214304860] [collision_updater]: Total Links : 7
[INFO] [1720080742.214318505] [collision_updater]: Total possible collisions : 21.000000
[INFO] [1720080742.214321084] [collision_updater]: Always in collision : 0
[INFO] [1720080742.214322915] [collision_updater]: Never in collision : 9
[INFO] [1720080742.214325204] [collision_updater]: Default in collision : 0
[INFO] [1720080742.214327758] [collision_updater]: Adjacent links disabled : 6
[INFO] [1720080742.214330077] [collision_updater]: Sometimes in collision : 6
[INFO] [1720080742.214332751] [collision_updater]: TOTAL DISABLED : 15
[INFO] [1720080742.214599312] [moveit_setup_assistant.Self-Collisions]: Thread complete 21
[INFO] [1720080744.190334533] [moveit_robot_model.robot_model]: Loading robot model 'air4a'...
[INFO] [1720080744.190363171] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
Warning: Group 'arm' is empty.
         at line 262 in /home/bjae/ws_moveit2/src/srdfdom/src/model.cpp
[INFO] [1720080753.371842138] [moveit_robot_model.robot_model]: Loading robot model 'air4a'...
[INFO] [1720080753.371868937] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[WARN] [1720080753.572603630] [moveit_robot_model.robot_model]: Group 'arm' must have at least one valid joint
[WARN] [1720080753.572621321] [moveit_robot_model.robot_model]: Failed to add group 'arm'
[INFO] [1720080759.038625870] [moveit_robot_model.robot_model]: Loading robot model 'air4a'...
[INFO] [1720080759.038659728] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[WARN] [1720080768.429340876] [moveit_robot_state.robot_state]: Returning dirty collision body transforms
moveit_setup_assistant: /home/bjae/ws_moveit2/src/moveit2/moveit_core/robot_state/include/moveit/robot_state/robot_state.h:1397: const Isometry3d& moveit::core::RobotState::getCollisionBodyTransform(const moveit::core::LinkModel*, std::size_t) const: Assertion `checkCollisionTransforms()' failed.
Stack trace (most recent call last):
#31   Object "/usr/lib/x86_64-linux-gnu/libQt5Core.so.5.15.3", at 0x7f6b26cb9e39, in QCoreApplication::notifyInternal2(QObject*, QEvent*)
#30   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b2796c712, in QApplicationPrivate::notify_helper(QObject*, QEvent*)
#29   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b279cbfd4, in 
#28   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b279c8d3f, in 
#27   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27972e46, in QApplicationPrivate::sendMouseEvent(QWidget*, QMouseEvent*, QWidget*, QWidget*, QWidget**, QPointer<QWidget>&, bool, bool)
#26   Object "/usr/lib/x86_64-linux-gnu/libQt5Core.so.5.15.3", at 0x7f6b26cb9e39, in QCoreApplication::notifyInternal2(QObject*, QEvent*)
#25   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27974363, in QApplication::notify(QObject*, QEvent*)
#24   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b2796c712, in QApplicationPrivate::notify_helper(QObject*, QEvent*)
#23   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b279af4ed, in QWidget::event(QEvent*)
#22   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27b164a2, in QSlider::mouseMoveEvent(QMouseEvent*)
#21   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27a66229, in QAbstractSlider::setValue(int)
#20   Object "/usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5.15.3", at 0x7f6b27a65aa1, in QAbstractSlider::valueChanged(int)
#19   Object "/usr/lib/x86_64-linux-gnu/libQt5Core.so.5.15.3", at 0x7f6b26cf17c7, in 
#18   Source "/home/bjae/ws_moveit2/build/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/moc_robot_poses_widget.cpp", line 247, in qt_static_metacall [0x7f6b1138488a]
        244:         (void)_t;
        245:         switch (_id) {
        246:         case 0: _t->jointValueChanged((*reinterpret_cast< const std::string(*)>(_a[1])),(*reinterpret_cast< double(*)>(_a[2]))); break;
      > 247:         case 1: _t->changeJointValue((*reinterpret_cast< int(*)>(_a[1]))); break;
        248:         case 2: _t->changeJointSlider(); break;
        249:         default: ;
        250:         }
#17   Source "/home/bjae/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp", line 790, in changeJointValue [0x7f6b11375b9d]
        787:   joint_value_->setText(QString("%1").arg(double_value, 0, 'f', 4));
        788: 
        789:   // Send event to parent widget
      > 790:   Q_EMIT jointValueChanged(joint_model_->getName(), double_value);
        791: }
        792: 
        793: // ******************************************************************************************
#16   Source "/home/bjae/ws_moveit2/build/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/moc_robot_poses_widget.cpp", line 318, in jointValueChanged [0x7f6b11384b1a]
        315: void moveit_setup::srdf_setup::SliderWidget::jointValueChanged(const std::string & _t1, double _t2)
        316: {
        317:     void *_a[] = { nullptr, const_cast<void*>(reinterpret_cast<const void*>(std::addressof(_t1))), const_cast<void*>(reinterpret_cast<const void*>(std::addressof(_t2))) };
      > 318:     QMetaObject::activate(this, &staticMetaObject, 0, _a);
        319: }
        320: QT_WARNING_POP
        321: QT_END_MOC_NAMESPACE
#15   Object "/usr/lib/x86_64-linux-gnu/libQt5Core.so.5.15.3", at 0x7f6b26cf17c7, in 
#14   Source "/home/bjae/ws_moveit2/build/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/moc_robot_poses_widget.cpp", line 127, in qt_static_metacall [0x7f6b11384665]
        124:         case 7: _t->loadJointSliders((*reinterpret_cast< const QString(*)>(_a[1]))); break;
        125:         case 8: _t->showDefaultPose(); break;
        126:         case 9: _t->playPoses(); break;
      > 127:         case 10: _t->updateRobotModel((*reinterpret_cast< const std::string(*)>(_a[1])),(*reinterpret_cast< double(*)>(_a[2]))); break;
        128:         default: ;
        129:         }
        130:     } else if (_c == QMetaObject::RegisterMethodArgumentMetaType) {
#13   Source "/home/bjae/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp", line 687, in updateRobotModel [0x7f6b113751d3]
        684:   robot_state.setVariablePosition(name, value);
        685: 
        686:   // Update the robot model/rviz
      > 687:   updateStateAndCollision(robot_state);
        688: }
        689: 
        690: void RobotPosesWidget::updateStateAndCollision(const moveit::core::RobotState& robot_state)
#12   Source "/home/bjae/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp", line 696, in updateStateAndCollision [0x7f6b1137522e]
        694:   // if in collision, show warning
        695:   // if no collision, hide warning
      > 696:   collision_warning_->setHidden(!setup_step_.checkSelfCollision(robot_state));
        697: }
        698: 
        699: // ******************************************************************************************
#11   Source "/home/bjae/ws_moveit2/src/moveit2/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp", line 115, in checkSelfCollision [0x7f6b11340ba4]
        112: {
        113:   // Decide if current state is in collision
        114:   collision_detection::CollisionResult result;
      > 115:   srdf_config_->getPlanningScene()->checkSelfCollision(request_, result, robot_state, allowed_collision_matrix_);
        116:   return !result.contacts.empty();
        117: }
#10   Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h", line 474, in checkSelfCollision [0x7f6b257fdf60]
        471:                           const collision_detection::AllowedCollisionMatrix& acm) const
        472:   {
        473:     // do self-collision checking with the unpadded version of the robot
      > 474:     getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
        475:   }
        476: 
        477:   /** \brief Get the names of the links that are involved in collisions for the current state */
#9    Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp", line 258, in checkSelfCollision [0x7f6b225c1364]
        255: void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
        256:                                          const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const
        257: {
      > 258:   checkSelfCollisionHelper(req, res, state, &acm);
        259: }
        260: 
        261: void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res,
#8    Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp", line 266, in checkSelfCollisionHelper [0x7f6b225c13d8]
        263:                                                const AllowedCollisionMatrix* acm) const
        264: {
        265:   FCLManager manager;
      > 266:   allocSelfCollisionBroadPhase(state, manager);
        267:   CollisionData cd(&req, &res, acm);
        268:   cd.enableGroup(getRobotModel());
        269:   manager.manager_->collide(&cd, &collisionCallback);
#7    Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp", line 245, in allocSelfCollisionBroadPhase [0x7f6b225c128d]
        242: {
        243:   manager.manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
        244: 
      > 245:   constructFCLObjectRobot(state, manager.object_);
        246:   manager.object_.registerTo(manager.manager_.get());
        247: }
#6    Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp", line 211, in constructFCLObjectRobot [0x7f6b225c0e2c]
        208:   for (std::size_t i = 0; i < robot_geoms_.size(); ++i)
        209:     if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_)
        210:     {
      > 211:       transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link,
        212:                                                     robot_geoms_[i]->collision_geometry_data_->shape_index),
        213:                     fcl_tf);
        214:       auto coll_obj = new fcl::CollisionObjectd(*robot_fcl_objs_[i]);
#5    Source "/home/bjae/ws_moveit2/src/moveit2/moveit_core/robot_state/include/moveit/robot_state/robot_state.h", line 1397, in getCollisionBodyTransform [0x7f6b225c4d8e]
       1395:   const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
       1396:   {
      >1397:     assert(checkCollisionTransforms());
       1398:     return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
       1399:   }
#4    Source "./assert/assert.c", line 101, in __assert_fail [0x7f6b26239e95]
#3    Source "./assert/assert.c", line 92, in __assert_fail_base [0x7f6b2622871a]
#2    Source "./stdlib/abort.c", line 79, in abort [0x7f6b262287f2]
#1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7f6b26242475]
#0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
    | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
      Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7f6b262969fc]
Aborted (Signal sent by tkill() 321327 1000)

Your environment

• ROS Distro: Humble
• OS Version: Ubuntu 22.04
• Source build
• If source, which branch: Humble
• Which RMW (Fast DDS or Cyclone DDS)? rmw_cyclonedds_cpp.

Steps to reproduce

ros2 launch moveit_setup_assistant moveit_setup_assistant

Expected behaviour

Hope to move the robot from the 'Robot poses' menu.

Actual behaviour

Slide the control slider, then exit the window.

Backtrace or Console output

Window crashes.

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[github-actions]

This issue was closed because it has been stalled for 45 days with no activity.