`RobotState::getRigidlyConnectedParentLinkModel()` does not recognise subframes #3089

rr-tom-noble · 2024-11-14T15:27:27Z

Description

As of #3077, RobotState::setFromIK() should allow subframe names as the tip. This does not appear to have fixed the issue however.

ROS Distro

Humble + #3077

OS and version

Ubuntu 22.04 (Docker)

Source or binary build?

Source

If binary, which release version?

N/A

If source, which branch?

humble + #3077

Which RMW are you using?

FastRTPS

Steps to Reproduce

Load a robot
Attach an object to the robot
Associate a subframe with the object
Call RobotState::setFromIK() with the subframe name as the tip

Expected behavior

The call should succeed. This is because with #3077, the chain of calls is now setFromIK() -> getRigidlyConnectedParentLinkModel() -> getFrameInfo(). The latter function appears to include a search for subframes.

Actual behavior

The call fails with the errors given below. The first error message suggests that getFrameInfo() is returning false and the second message suggests that this is occuring when querying the pose frame

Backtrace or Console output

[move_group-2] [ERROR] [1731596998.921984677] [moveit_robot_state.robot_state]: Unable to find link for frame 'tb_9_3-f686742d-b737-43b8-a712-5318e3f05abc/disc_centre'
[move_group-2] [ERROR] [1731596998.921987769] [moveit_robot_state.robot_state]: The following Pose Frame does not exist: tb_9_3-f686742d-b737-43b8-a712-5318e3f05abc/disc_centre

The text was updated successfully, but these errors were encountered:

TSNoble · 2024-11-16T09:17:15Z

I've run the tests locally in Debug mode a la #3102, however they still pass. This suggests to me that getRigidlyConnectedParentLinkModel is working as intended, and I should probably look at the code we're using to attach the objects & subframes (which differs from the tests in that we do it through the PlanningSceneInterface rather than a RobotModel instance)

rr-tom-noble · 2024-11-19T08:50:43Z

@sea-bass I'd like to test this again using 2.5.6 as a base to rule out source and binary mixing issues. It seems like the apt repository is still hosting 2.5.5. Do you know how often this gets updated?

No worries either way, as I can build from source from the tag 👍

rhaschke · 2024-11-19T09:32:38Z

If you want to use latest release packages, you need to use the testing repository. These are manually synced to the production repo on an irregular (usually monthly) basis.

rr-tom-noble · 2024-11-20T14:58:27Z

Added some prints to the subframe lookup code in RobotState::getFrameInfo():

  // Check if an AttachedBody has a subframe with name frame_id
  for (const auto& body : attached_body_map_)
  {
    RCLCPP_WARN(LOGGER, "Checking for frame id: %s", frame_id.c_str());
    RCLCPP_WARN(LOGGER, "Checking if frame is subframe of body %s", body.first.c_str());
    const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found);
    if (frame_found)
    {
      RCLCPP_WARN(LOGGER, "Frame %s found as subframe of body %s", frame_id.c_str(), body.first.c_str());
      robot_link = body.second->getAttachedLink();
      assert(checkLinkTransforms());
      return transform;
    }
    RCLCPP_WARN(LOGGER, "Frame %s not found as subframe of body %s", frame_id.c_str(), body.first.c_str());
  }

Interestingly, it seems to find the subframe (so not an issue with the AttachedBody::getGlobalSubframeTransform()):

[move_group-2] [WARN] [1732114093.982901211] [moveit_robot_state.robot_state]: Checking for frame id: tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration
[move_group-2] [WARN] [1732114093.982906557] [moveit_robot_state.robot_state]: Checking if frame is subframe of body tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0
[move_group-2] [WARN] [1732114093.982908251] [moveit_robot_state.robot_state]: Frame tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration found as subframe of body tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0
[move_group-2] [WARN] [1732114093.982922273] [moveit_robot_state.robot_state]: Checking for frame id: tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration
[move_group-2] [ERROR] [1732114093.982924485] [moveit_robot_state.robot_state]: Unable to find link for frame 'tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration'
[move_group-2] [ERROR] [1732114093.982926863] [moveit_robot_state.robot_state]: The following Pose Frame does not exist: tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration

I'm also unsure about the print after finding the frame... getFrameInfo() shouldn't keep iterating after this point

rr-tom-noble · 2024-11-20T15:29:07Z

Extra prints, extra info (and errors):

const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
{
  bool found;
  const LinkModel* link{ nullptr };
  getFrameInfo(frame, link, found);
  RCLCPP_WARN(LOGGER, "Link Name (getRigidlyConnectedParentLinkModel): %s", link->getName().c_str());
  RCLCPP_WARN(LOGGER, "Frame Found (getRigidlyConnectedParentLinkModel): %d", found);
  if (!found)
    RCLCPP_ERROR(LOGGER, "Unable to find link for frame '%s'", frame.c_str());
  return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
}

const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link,
                                                  bool& frame_found) const
{
  RCLCPP_WARN(LOGGER, "Checking for frame id (top): %s", frame_id.c_str());
  if (!frame_id.empty() && frame_id[0] == '/')
    return getFrameInfo(frame_id.substr(1), robot_link, frame_found);

  static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
  if (frame_id == robot_model_->getModelFrame())
  {
    robot_link = robot_model_->getRootLink();
    frame_found = true;
    return IDENTITY_TRANSFORM;
  }
  if ((robot_link = robot_model_->getLinkModel(frame_id, &frame_found)))
  {
    assert(checkLinkTransforms());
    return global_link_transforms_[robot_link->getLinkIndex()];
  }
  robot_link = nullptr;

  // Check names of the attached bodies
  const auto jt = attached_body_map_.find(frame_id);
  if (jt != attached_body_map_.end())
  {
    RCLCPP_WARN(LOGGER, "Checking for frame id (middle): %s", frame_id.c_str());
    RCLCPP_WARN(LOGGER, "Checking if frame is attached body: %s", frame_id.c_str());
    const Eigen::Isometry3d& transform = jt->second->getGlobalPose();
    robot_link = jt->second->getAttachedLink();
    frame_found = true;
    assert(checkLinkTransforms());
    return transform;
  }

  // Check if an AttachedBody has a subframe with name frame_id
  for (const auto& body : attached_body_map_)
  {
    RCLCPP_WARN(LOGGER, "Checking for frame id (bottom): %s", frame_id.c_str());
    RCLCPP_WARN(LOGGER, "Checking if frame is subframe of body %s", body.first.c_str());
    const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found);
    if (frame_found)
    {
      RCLCPP_WARN(LOGGER, "Frame %s found as subframe of body %s", frame_id.c_str(), body.first.c_str());
      robot_link = body.second->getAttachedLink();
      assert(checkLinkTransforms());
      RCLCPP_WARN(LOGGER, "Frame Found (getFrameInfo): %d", frame_found);
      RCLCPP_WARN_STREAM(LOGGER, "Frame Transform: " << transform.matrix());
      return transform;
    }
    RCLCPP_WARN(LOGGER, "Frame %s not found as subframe of body %s", frame_id.c_str(), body.first.c_str());
  }

  robot_link = nullptr;
  frame_found = false;
  return IDENTITY_TRANSFORM;
}

... (searching for the frame as a link, body, or on other bodies which don't have it)...

[move_group-2] [WARN] [1732116228.154644847] [moveit_robot_state.robot_state]: Checking if frame is subframe of body tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0
[move_group-2] [WARN] [1732116228.154646593] [moveit_robot_state.robot_state]: Frame tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration found as subframe of body tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0
[move_group-2] [WARN] [1732116228.154649026] [moveit_robot_state.robot_state]: Frame Found (getFrameInfo): 1
[move_group-2] [WARN] [1732116228.154661704] [moveit_robot_state.robot_state]: Frame Transform:    0.997363  -0.0122091   0.0715459     1.32627
[move_group-2]  -0.0128309   -0.999884  0.00823744  -0.0018478
[move_group-2]    0.071437 -0.00913372   -0.997403     1.24851
[move_group-2]           0           0           0           1
[move_group-2] [WARN] [1732116228.154714346] [moveit_robot_state.robot_state]: Checking for frame id (top): tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration
[move_group-2] Stack trace (most recent call last) in thread 668:
[move_group-2] #23   Object "", at 0xffffffffffffffff, in 
[move_group-2] #22   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ea7a30e2a03, in __clone
[move_group-2] #21   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ea7a3051ac2, in 
[move_group-2] #20   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7ea7a32e2252, in 
[move_group-2] #19   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7ea7a359b349, in rclcpp::executors::MultiThreadedExecutor::run(unsigned long)
[move_group-2] #18   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7ea7a359411e, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[move_group-2] #17   Object "/opt/ros/humble/lib/librclcpp_action.so", at 0x7ea7a286e246, in rclcpp_action::ServerBase::execute_goal_request_received(std::shared_ptr<void>&)
[move_group-2] #16   Object "/opt/ros/humble/lib/libmoveit_move_group_default_capabilities.so.2.5.6", at 0x7ea78ca20c17, in 
[move_group-2] #15   Object "/opt/ros/humble/lib/libmoveit_move_group_default_capabilities.so.2.5.6", at 0x7ea78ca15ae7, in move_group::MoveGroupMoveAction::executeMoveCallback(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::MoveGroup> > const&)
[move_group-2] #14   Object "/opt/ros/humble/lib/libmoveit_move_group_default_capabilities.so.2.5.6", at 0x7ea78ca0fbc8, in move_group::MoveGroupMoveAction::executeMoveCallbackPlanOnly(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::MoveGroup> > const&, std::shared_ptr<moveit_msgs::action::MoveGroup_Result_<std::allocator<void> > >&)
[move_group-2] #13   Object "/opt/ros/humble/lib/libmoveit_planning_pipeline.so.2.5.6", at 0x7ea7a2eb2fff, in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&) const
[move_group-2] #12   Object "/opt/ros/humble/lib/libmoveit_planning_pipeline.so.2.5.6", at 0x7ea7a2eb09c8, in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const
[move_group-2] #11   Object "/opt/ros/humble/lib/libmoveit_planning_request_adapter.so.2.5.6", at 0x7ea7a27ad4e1, in planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(std::shared_ptr<planning_interface::PlannerManager> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const
[move_group-2] #10   Object "/opt/ros/humble/lib/libmoveit_planning_request_adapter.so.2.5.6", at 0x7ea7a27ac939, in planning_request_adapter::(anonymous namespace)::callAdapter(planning_request_adapter::PlanningRequestAdapter const&, std::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&)
[move_group-2] #9    Object "/opt/ros/humble/lib/libmoveit_default_planning_request_adapter_plugins.so.2.5.6", at 0x7ea78dcac237, in 
[move_group-2] #8    Object "/opt/ros/humble/lib/libmoveit_planning_request_adapter.so.2.5.6", at 0x7ea7a27acfae, in std::_Function_handler<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&), planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(std::shared_ptr<planning_interface::PlannerManager> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const::{lambda(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)#1}>::_M_invoke(std::_Any_data const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)
[move_group-2] #7    Object "/opt/ros/humble/lib/libtrajectory_generation_common.so", at 0x7ea78dd0837e, in pilz_industrial_motion_planner::TrajectoryGenerator::generate(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, double)
[move_group-2] #6    Object "/opt/ros/humble/lib/libplanning_context_loader_lin.so", at 0x7ea78dce4389, in pilz_industrial_motion_planner::TrajectoryGeneratorLIN::plan(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, pilz_industrial_motion_planner::TrajectoryGenerator::MotionPlanInfo const&, double const&, trajectory_msgs::msg::JointTrajectory_<std::allocator<void> >&)
[move_group-2] #5    Object "/opt/ros/humble/lib/libtrajectory_generation_common.so", at 0x7ea78dd03ad9, in pilz_industrial_motion_planner::generateJointTrajectory(std::shared_ptr<planning_scene::PlanningScene const> const&, pilz_industrial_motion_planner::JointLimitsContainer const&, KDL::Trajectory const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > > const&, double const&, trajectory_msgs::msg::JointTrajectory_<std::allocator<void> >&, moveit_msgs::msg::MoveItErrorCodes_<std::allocator<void> >&, bool)
[move_group-2] #4    Object "/opt/ros/humble/lib/libtrajectory_generation_common.so", at 0x7ea78dcfd718, in pilz_industrial_motion_planner::computePoseIK(std::shared_ptr<planning_scene::PlanningScene const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Transform<double, 3, 1, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > >&, bool, double)
[move_group-2] #3    Object "/opt/ros/humble/lib/libmoveit_robot_state.so.2.5.6", at 0x7ea7a2e2ef2f, in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, Eigen::Transform<double, 3, 1, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double, std::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&, std::function<double (geometry_msgs::msg::Pose_<std::allocator<void> > const&, moveit::core::RobotState const&, moveit::core::JointModelGroup const*, std::vector<double, std::allocator<double> > const&)> const&)
[move_group-2] #2    Object "/opt/ros/humble/lib/libmoveit_robot_state.so.2.5.6", at 0x7ea7a2e2e223, in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, Eigen::Transform<double, 3, 1, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> > const&, double, std::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&, std::function<double (geometry_msgs::msg::Pose_<std::allocator<void> > const&, moveit::core::RobotState const&, moveit::core::JointModelGroup const*, std::vector<double, std::allocator<double> > const&)> const&)
[move_group-2] #1    Object "/opt/ros/humble/lib/libmoveit_robot_state.so.2.5.6", at 0x7ea7a2e2c576, in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, std::vector<Eigen::Transform<double, 3, 1, 0>, Eigen::aligned_allocator<Eigen::Transform<double, 3, 1, 0> > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, double, std::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&, std::function<double (geometry_msgs::msg::Pose_<std::allocator<void> > const&, moveit::core::RobotState const&, moveit::core::JointModelGroup const*, std::vector<double, std::allocator<double> > const&)> const&)
[move_group-2] #0    Object "/opt/ros/humble/lib/libmoveit_robot_state.so.2.5.6", at 0x7ea7a2e2673d, in moveit::core::RobotState::getRigidlyConnectedParentLinkModel(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const
[move_group-2] Segmentation fault (Address not mapped to object [(nil)])

rr-tom-noble · 2024-11-20T15:31:23Z

I'm still very suspicious of this print:

[move_group-2] [WARN] [1732116228.154714346] [moveit_robot_state.robot_state]: Checking for frame id (top): tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration

We print the frame found flag shortly before returning in getFrameInfo() and shortly after returning to the caller (getRigidlyConnectedParentLinkModel()). Only the first print is seen, so some time within the return we appear to be calling getFrameInfo() again with the same frame (and crashing shortly afterwards).

This feels very race condition-y

rr-tom-noble · 2024-11-20T15:36:57Z

Ah actually, we print out the link name first in getRigidlyConnectedParentModel(). I suppose it could be null? This would suggest that either the attached body that contains the subframe is returning a null parent link, or some kind of race condition is occurring and we're getting undefined behaviour.

Given the rogue print, and the fact that link is a mutable pointer, I'm suspicious of the latter.

rr-tom-noble · 2024-11-20T15:49:53Z

More prints:

const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
{
  bool found;
  const LinkModel* link{ nullptr };
  getFrameInfo(frame, link, found);
  RCLCPP_WARN(LOGGER, "Frame Found (getRigidlyConnectedParentLinkModel): %d", found);
  RCLCPP_WARN_STREAM(LOGGER, "Is Link nullptr? : " << (link == nullptr ? "true" : "false"));
  RCLCPP_WARN(LOGGER, "Link Name (getRigidlyConnectedParentLinkModel): %s", link->getName().c_str());
  if (!found)
    RCLCPP_ERROR(LOGGER, "Unable to find link for frame '%s'", frame.c_str());
  return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
}

And a nullptr check when we fetch the parent link of the attached body that has the subframe, after found.

[move_group-2] [WARN] [1732117734.110083949] [moveit_robot_state.robot_state]: Checking if frame is subframe of body tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0
[move_group-2] [WARN] [1732117734.110085725] [moveit_robot_state.robot_state]: Frame tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration found as subframe of body tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0
[move_group-2] [WARN] [1732117734.110092335] [moveit_robot_state.robot_state]: Is body's attached link nullptr? : false
[move_group-2] [WARN] [1732117734.110095713] [moveit_robot_state.robot_state]: Frame Found (getFrameInfo): 1
[move_group-2] [WARN] [1732117734.110106816] [moveit_robot_state.robot_state]: Frame Transform:    0.997363  -0.0122091   0.0715459     1.32627
[move_group-2]  -0.0128309   -0.999884  0.00823744  -0.0018478
[move_group-2]    0.071437 -0.00913372   -0.997403     1.24851
[move_group-2]           0           0           0           1
[move_group-2] [WARN] [1732117734.110156118] [moveit_robot_state.robot_state]: Checking for frame id (top): tb_4_3-d536e569-fa2b-4bab-9c53-4d73e8842fa0/length_calibration
[move_group-2] [WARN] [1732117734.110159444] [moveit_robot_state.robot_state]: Frame Found (getRigidlyConnectedParentLinkModel): 0
[move_group-2] [WARN] [1732117734.110162190] [moveit_robot_state.robot_state]: Is Link nullptr? : true

rr-tom-noble · 2024-11-20T15:55:46Z

In summary, the order of operations is:

getRigidlyConnectedParentLinkModel() calls getFrameInfo() with a subframe
getFrameInfo() finds the subframe on an attached body
both the link and found values seem to be sensible at this point (within getFrameInfo())
getFrameInfo() returns back to getRigidlyConnectedParentLinkModel()
Checking the returned values on the getRigidlyConnectedParentLinkModel() side shows they have changed to nullptr and false
Some time between 3 and 5, something else has called getFrameInfo() with the same frame

rr-tom-noble · 2024-11-20T16:01:47Z

Actually, I could be incorrect regarding 5. It might be the case that the nullptr, false are associated with the second getFrameInfo() call. Though this would raise another question: where are the corresponding prints for the first call?

rhaschke · 2024-11-21T09:35:34Z

Can you provide a minimal code snippet? Then I can have a look at this.
You are using Humble 2.5.6 (c2b0e3d) + #3077, right?

TSNoble · 2024-11-21T09:41:06Z

@rhaschke

Thanks, and apologies for any incoherence in my previous comments 😅

I'm off work today, but I believe @rr-mark might be looking into the issue as well.

We are now using 2.5.6 (which contains #3077). The only changes have been prints within the RobotState methods

rhaschke · 2024-11-21T09:46:36Z

Ok. Is there already code available (e.g. a test), adding an object + subframe and then calling getRigidlyConnectedParentLinkModel(), which you are using? Providing a code link (or code snippet) would save me some time to search for it.

TSNoble · 2024-11-21T09:52:55Z

There's tests I added in pilz which call getRigidlyConnectedParentModel() via setFromIK() here, however these are all passing. Unfortunately our stack is quite dense (I think the mode of failure is seen through calling pilz's planMotionSequence() service with a subframe planning frame, which reaches getRigidlyConnectedParentModel() via a series of method calls).

I did take a brief look over the moveit and pilz code to check callback group types (thinking that two of the same services that query the robot state might be running concurrently, but it appeared that only the pilz planMotionSequence() action uses REENTRANT, and the service uses the node's default (which I believe is MUTUALLY_EXCLUSIVE). The rest of moveit also appears to use node defaults or explicitly MUTUALLY_EXCLUSIVE (as do we in all of our nodes)

TSNoble · 2024-11-21T10:06:05Z

I'll be partially available on my personal account, so I might try adding a test to the pilz getMotionSequence() integration tests which uses subframes, and update with findings.

rr-mark · 2024-11-21T12:48:25Z

@rhaschke @TSNoble I'm now looking at this on our end. I'll see if I can diagnose it myself, and if not I'll try to put together an MWE.

rr-mark · 2024-11-21T13:43:25Z

It looks like there is a call to getFrameTransform which succeeds, followed by a call to getGlobalSubframeTransform which fails. The call to getGlobalSubframeTransform doesn't seem to have any entries in attached_body_map_ in getFrameInfo.

backtrace:

Thread 21 "move_group" received signal SIGABRT, Aborted.
[Switching to Thread 0x77f123400640 (LWP 660)]
0x000077f14886c9fc in pthread_kill () from /lib/x86_64-linux-gnu/libc.so.6
(gdb) 
(gdb) bt
#0  0x000077f14886c9fc in pthread_kill () from /lib/x86_64-linux-gnu/libc.so.6
#1  0x000077f148818476 in raise () from /lib/x86_64-linux-gnu/libc.so.6
#2  0x000077f1487fe7f3 in abort () from /lib/x86_64-linux-gnu/libc.so.6
#3  0x000077f148ac1b9e in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x000077f148acd20c in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x000077f148acd277 in std::terminate() ()
   from /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x000077f148acd4d8 in __cxa_throw ()
   from /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x000077f14861d8a8 in moveit::core::RobotState::getRigidlyConnectedParentLinkModel (this=<optimized out>, 
    frame="stylus_D3L50-2960fa52-8527-412d-8376-be31b1871ada/length_calibration") at /usr/include/c++/11/ext/new_allocator.h:89
#8  0x000077f1486461b7 in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, std::vector<Eigen::Transform<double, 3, 1, 0>, Eigen::aligned_allocator<Eigen::Transform<double, 3, 1, 0> > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, double, std::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&, std::function<double (geometry_msgs::msg::Pose_<std::allocator<void> > const&, moveit::core:--Type <RET> for more, q to quit, c to continue without paging--
:RobotState const&, moveit::core::JointModelGroup const*, std::vector<double, std::allocator<double> > const&)> const&) (this=<optimized out>, 
    jmg=0x64bc8d5b6bb0, poses_in=std::vector of length 1, capacity 1 = {...}, 
    tips_in=std::vector of length 1, capacity 1 = {...}, 
    consistency_limit_sets=..., timeout=0, constraint=..., options=..., 
    cost_function=...)
    at /root/workspace/src/moveit2/moveit_core/robot_state/src/robot_state.cpp:1688
#9  0x000077f148647e64 in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, Eigen::Transform<double, 3, 1, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> > const&, double, std::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&, std::function<double (geometry_msgs::msg::Pose_<std::allocator<void> > const&, moveit::core::RobotState const&, moveit::core::JointModelGroup const*, std::vector<double, std::allocator<double> > const&)> const&) (this=0x77f1233f8ab0, jmg=0x64bc8d5b6bb0, pose_in=..., 
    tip_in=..., consistency_limits_in=std::vector of length 0, capacity 0, 
    timeout=0, constraint=..., options=..., cost_function=...)
    at /root/workspace/src/moveit2/moveit_core/robot_state/src/robot_state.cpp:1568
#10 0x000077f148648b70 in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, Eigen::Transform<double, 3, 1, 0> const&, std::__cxx11::basi--Type <RET> for more, q to quit, c to continue without paging--
c_string<char, std::char_traits<char>, std::allocator<char> > const&, double, std::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&, std::function<double (geometry_msgs::msg::Pose_<std::allocator<void> > const&,re::RobotState const&, moveit::core::JointModelGroup const*, std::vector<double, std::allocator<double> > const&)> const&) (this=0x77f1233f8ab0, 
    jmg=0x64bc8d5b6bb0, pose_in=..., 
    tip_in="stylus_D3L50-2960fa52-8527-412d-8376-be31b1871ada/length_calibration", timeout=<optimized out>, constraint=..., options=..., cost_function=...)
    at /root/workspace/src/moveit2/moveit_core/robot_state/src/robot_state.cpp:1509
#11 0x000077f1434f9719 in pilz_industrial_motion_planner::computePoseIK(std::shared_ptr<planning_scene::PlanningScene const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&,nsform<double, 3, 1, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > ocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char:al--Type <RET> for more, q to quit, c to continue without paging--
locator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > >&, bool, double) ()
   from /opt/ros/humble/lib/libtrajectory_generation_common.so
#12 0x000077f1434ffada in pilz_industrial_motion_planner::generateJointTrajectory(std::shared_ptr<planning_scene::PlanningScene const> const&, pilz_industrial_motion_planner::JointLimitsContainer const&, KDL::Trajectory const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > ::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__c_string<char, std::char_traits<char>, std::allocator<char> > const, double> > > const&, double const&, trajectory_msgs::msg::JointTrajectory_<std::allocator<void> >&, moveit_msgs::msg::MoveItErrorCodes_<std::allocator<void> >&, bool) ()
   from /opt/ros/humble/lib/libtrajectory_generation_common.so

#13 0x000077f1434e538a in pilz_industrial_motion_planner::TrajectoryGeneratorLIN::plan(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, pilz_industrial_motion_planner::TrajectoryGenerator::MotionPlanInfo const&, double const&, tsgs::msg::JointTrajectory_<std::allocator<void> >&) ()
   from /opt/ros/humble/lib/libplanning_context_loader_lin.so

#14 0x000077f14350437f in pilz_industrial_motion_planner::TrajectoryGenerator::generate(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs--Type <RET> for more, q to quit, c to continue without paging--
::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, double) ()
   from /opt/ros/humble/lib/libtrajectory_generation_common.so
#15 0x000077f147fc5faf in planning_request_adapter::(anonymous namespace)::callPlannerInterfaceSolve (res=..., req=..., planning_scene=..., planner=...)
    at /root/workspace/src/moveit2/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:55
#16 operator() (res=..., req=..., scene=..., __closure=<optimized out>)
    at /root/workspace/src/moveit2/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:135
#17 std::__invoke_impl<bool, planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const PlannerManagerPtr&, const PlanningSceneConstPtr&, const MotionPlanRequest&, planning_interface::MotionPlanResponse&, std::vector<long unsigned int>&) const::<lambda(const PlanningSceneConstPtr&, const Motist&, planning_interface::MotionPlanResponse&)>&, const std::shared_ptr<const planning_scene::PlanningScene>&, const moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> >&, planning_interface::MotionPlanResponse&> (__f=...)
    at /usr/include/c++/11/bits/invoke.h:61
#18 std::__invoke_r<bool, planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const PlannerManagerPtr&, const PlanningSceneConstPtr&, const MotionPlanRequest&, planning_interface::MotionPlanResponse&, std::vector<long unsigned int>&) const::<lambda(const PlanningSceneConstPtr&, const MotionP, planning_interface::MotionPlanResponse&)>&, const std::shared_ptr<const plannin--Type <RET> for more, q to quit, c to continue without paging--
g_scene::PlanningScene>&, const moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> >&, planning_interface::MotionPlanResponse&> (__fn=...)
    at /usr/include/c++/11/bits/invoke.h:114
#19 std::_Function_handler<bool(const std::shared_ptr<const planning_scene::PlanningScene>&, const moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> >&, planning_interface::MotionPlanResponse&), planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const PlannerManagerPtr&, const PlannstPtr&, const MotionPlanRequest&, planning_interface::MotionPlanResponse&, std::vector<long unsigned int>&) const::<lambda(const PlanningSceneConstPtr&, const MotionPlanRequest&, planning_interface::MotionPlanResponse&)> >::_M_invoke(const std::_Any_data &, const std::shared_ptr<planning_scene::PlanningScen const moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > &, planning_interface::MotionPlanResponse &) (__functor=..., __args#0=..., __args#1=..., 
    __args#2=...) at /usr/include/c++/11/bits/std_function.h:290
#20 0x000077f1434ad238 in ?? ()
   from /opt/ros/humble/lib/libmoveit_default_planning_request_adapter_plugins.so

#21 0x000077f147fc593a in planning_request_adapter::(anonymous namespace)::callAdapter (adapter=..., planner=..., 
    planning_scene=std::shared_ptr<const planning_scene::PlanningScene> (use count 5, weak count 1) = {...}, req=..., res=..., 
    added_path_index=std::vector of length 0, capacity 0)
    at /root/workspace/src/moveit2/moveit_core/planning_request_adapter/src/plan--Type <RET> for more, q to quit, c to continue without paging--
ning_request_adapter.cpp:67
#22 0x000077f147fc64e2 in std::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)>::operator()(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::equest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&) const (__args#2=..., __args#1=..., 
    __args#0=std::shared_ptr<const planning_scene::PlanningScene> (use count 5, weak count 1) = {...}, this=0x77f1233fc250)
    at /usr/include/c++/11/bits/std_function.h:590
#23 planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan (
    this=<optimized out>, planner=..., 
    planning_scene=std::shared_ptr<const planning_scene::PlanningScene> (use cou
    added_path_index=std::vector of length 0, capacity 0)
    at /root/workspace/src/moveit2/moveit_core/planning_request_adapter/src/plan
#24 0x000077f1486c99c9 in planning_pipeline::PlanningPipeline::generatePlan(std:
   from /opt/ros/humble/lib/libmoveit_planning_pipeline.so.2.5.6
#25 0x000077f1486cc000 in planning_pipeline::PlanningPipeline::generatePlan(std:
:shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::msg::Motio
#26 0x000077f14178dbc9 in move_group::MoveGroupMoveAction::executeMoveCallbackPl
   from /opt/ros/humble/lib/libmoveit_move_group_default_capabilities.so
#27 0x000077f141793ae8 in move_group::MoveGroupMoveAction::executeMoveCallback(s
   from /opt/ros/humble/lib/libmoveit_move_group_default_capabilities.so
#28 0x000077f14179ec18 in ?? ()
   from /opt/ros/humble/lib/libmoveit_move_group_default_capabilities.so
#29 0x000077f148087247 in rclcpp_action::ServerBase::execute_goal_request_receiv
#30 0x000077f148dad11f in rclcpp::Executor::execute_any_executable(rclcpp::AnyEx
#31 0x000077f148db434a in rclcpp::executors::MultiThreadedExecutor::run(unsigned
#32 0x000077f148afb253 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#33 0x000077f14886aac3 in ?? () from /lib/x86_64-linux-gnu/libc.so.6
#34 0x000077f1488fba04 in clone () from /lib/x86_64-linux-gnu/libc.so.6
(gdb)

rr-mark · 2024-11-21T14:06:56Z

Yeah, the robot state used to call setFromIK in pilz_industrial_motion_planner::computePoseIK has no attached bodies. This is feeling awfully familiar from the ROS1 work we did a few months ago. I'll have a look at the differences between ROS1 and ROS2 and see if I can find the missing PR.

rr-mark · 2024-11-21T14:22:00Z

Oh, I bet it's the passing of collision objects as diffs through the move group interface. I'll investigate that now.

rr-mark · 2024-11-21T14:36:22Z

Yeah, it's moveit/moveit#3592. I'll port it now.

rr-mark · 2024-11-21T15:03:26Z

Wait, no, I'm wrong, the version of moveit2 I was looking at was out of date. #3592 is already ported. I'll keep digging.

rr-mark · 2024-11-21T15:09:58Z

Okay, it looks like main has #3592, but the 2.5.6 release does not.

rr-mark · 2024-11-21T15:15:23Z

Yeah, #3041 ports moveit/moveit#3592, but was never back-ported to humble.

sea-bass · 2024-11-21T15:30:51Z

Yeah, #3041 ports moveit/moveit#3592, but was never back-ported to humble.

Nice! Just kicked off the backport and it has conflicts. Would you have time to poke at that?

#3118

rr-mark · 2024-11-21T15:33:32Z

Ooh, you just beat me to it. I'll fix your PR rather than trying to sort out mine.

rr-mark · 2024-11-21T15:49:13Z

Okay, I'm a bit confused. I've made a PR to your PR branch #3120

Or there's #3119 which I just closed, which is the same PR but directly to the humble branch.

I wasn't sure why the humble branch had conflicts; it looks like the last change to the relevant file was a year ago, long before any of the changes in moveit/moveit#3592

rr-mark · 2024-11-21T15:52:01Z

The diff https://github.com/moveit/moveit2/pull/3119/files is almost identical to the diff https://github.com/moveit/moveit/pull/3592/files, so I think accepting all incoming changes is the right thing to do.

TSNoble · 2024-11-23T16:53:43Z

The backport has been merged, but I think @rr-mark was getting linker issues testing it out. I'll be in work tomorrow so will give it a shot then

sea-bass · 2024-11-23T16:55:05Z

Make sure you guys blow away all the binary installs of MoveIt sudo apt remove ros-humble-moveit-* before rebuilding!

rr-tom-noble · 2024-11-25T10:36:00Z

@sea-bass

Just got the compilation working and all looks good! No subframe issues. Closing as resolved

rr-tom-noble added the bug Something isn't working label Nov 14, 2024

This was referenced Nov 15, 2024

Request for a new release of MoveIt into Humble #3012

Closed

Fixes pilz tests #3095

Merged

rr-tom-noble closed this as completed Nov 25, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

`RobotState::getRigidlyConnectedParentLinkModel()` does not recognise subframes #3089

`RobotState::getRigidlyConnectedParentLinkModel()` does not recognise subframes #3089

rr-tom-noble commented Nov 14, 2024 •

edited

Loading

TSNoble commented Nov 16, 2024

rr-tom-noble commented Nov 19, 2024

rhaschke commented Nov 19, 2024

rr-tom-noble commented Nov 20, 2024 •

edited

Loading

rr-tom-noble commented Nov 20, 2024

rr-tom-noble commented Nov 20, 2024 •

edited

Loading

rr-tom-noble commented Nov 20, 2024 •

edited

Loading

rr-tom-noble commented Nov 20, 2024

rr-tom-noble commented Nov 20, 2024

rr-tom-noble commented Nov 20, 2024

rhaschke commented Nov 21, 2024

TSNoble commented Nov 21, 2024

rhaschke commented Nov 21, 2024

TSNoble commented Nov 21, 2024 •

edited

Loading

TSNoble commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024 •

edited

Loading

sea-bass commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024 •

edited

Loading

rr-mark commented Nov 21, 2024

TSNoble commented Nov 23, 2024

sea-bass commented Nov 23, 2024

rr-tom-noble commented Nov 25, 2024

RobotState::getRigidlyConnectedParentLinkModel() does not recognise subframes #3089

RobotState::getRigidlyConnectedParentLinkModel() does not recognise subframes #3089

Comments

rr-tom-noble commented Nov 14, 2024 • edited Loading

Description

ROS Distro

OS and version

Source or binary build?

If binary, which release version?

If source, which branch?

Which RMW are you using?

Steps to Reproduce

Expected behavior

Actual behavior

Backtrace or Console output

TSNoble commented Nov 16, 2024

rr-tom-noble commented Nov 19, 2024

rhaschke commented Nov 19, 2024

rr-tom-noble commented Nov 20, 2024 • edited Loading

rr-tom-noble commented Nov 20, 2024

rr-tom-noble commented Nov 20, 2024 • edited Loading

rr-tom-noble commented Nov 20, 2024 • edited Loading

rr-tom-noble commented Nov 20, 2024

rr-tom-noble commented Nov 20, 2024

rr-tom-noble commented Nov 20, 2024

rhaschke commented Nov 21, 2024

TSNoble commented Nov 21, 2024

rhaschke commented Nov 21, 2024

TSNoble commented Nov 21, 2024 • edited Loading

TSNoble commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024 • edited Loading

sea-bass commented Nov 21, 2024

rr-mark commented Nov 21, 2024

rr-mark commented Nov 21, 2024 • edited Loading

rr-mark commented Nov 21, 2024

TSNoble commented Nov 23, 2024

sea-bass commented Nov 23, 2024

rr-tom-noble commented Nov 25, 2024

`RobotState::getRigidlyConnectedParentLinkModel()` does not recognise subframes #3089

`RobotState::getRigidlyConnectedParentLinkModel()` does not recognise subframes #3089

rr-tom-noble commented Nov 14, 2024 •

edited

Loading

rr-tom-noble commented Nov 20, 2024 •

edited

Loading

rr-tom-noble commented Nov 20, 2024 •

edited

Loading

rr-tom-noble commented Nov 20, 2024 •

edited

Loading

TSNoble commented Nov 21, 2024 •

edited

Loading

rr-mark commented Nov 21, 2024 •

edited

Loading

rr-mark commented Nov 21, 2024 •

edited

Loading