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.

Sign up for GitHub

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

Jump to bottom

Avoid segfault and improve error warnings #603

Closed
wants to merge 7 commits into from
Closed

Avoid segfault and improve error warnings #603

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
a9c983d
Avoid segfault and improve error warnings
sjahr Aug 12, 2024
4bac6ac
Fix formatting
sjahr Aug 12, 2024
fe953d0
Remove duplicate print statement
sjahr Aug 12, 2024
3d3d1b7
Revert unnecessary changes
sjahr Aug 12, 2024
08eaab1
Revert logging change & change log level
sjahr Aug 12, 2024
331652f
Formatting
sjahr Aug 12, 2024
7ba59e8
Change log level
sjahr Aug 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 2 additions & 4 deletions capabilities/src/execute_task_solution_capability.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@
#include <fmt/format.h>

namespace {
sjahr marked this conversation as resolved.
Show resolved Hide resolved

const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please revert those changes too.

sjahr marked this conversation as resolved.
Show resolved Hide resolved
// TODO: move to moveit::core::RobotModel
const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::RobotModel& model,
const std::vector<std::string>& joints) {
Expand All @@ -73,13 +73,11 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
return jmg;
}
}

RCLCPP_WARN(LOGGER, fmt::format("Could not find JointModelGroup that actuates {}", fmt::join(joints, ", ")));
sjahr marked this conversation as resolved.
Show resolved Hide resolved
sjahr marked this conversation as resolved.
Show resolved Hide resolved
return nullptr;
}
} // namespace
sjahr marked this conversation as resolved.
Show resolved Hide resolved

sjahr marked this conversation as resolved.
Show resolved Hide resolved
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");

namespace move_group {

ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}
Expand Down
1 change: 1 addition & 0 deletions core/src/container.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -345,6 +345,7 @@ Stage* ContainerBase::findChild(const std::string& name) const {
else if (auto* parent = dynamic_cast<const ContainerBase*>(child.get()))
return parent->findChild(name.substr(pos + 1));
}
RCLCPP_WARN_STREAM(rclcpp::get_logger("ContainerBase"), fmt::format("Child '{}' not found", name));
sjahr marked this conversation as resolved.
Show resolved Hide resolved
sjahr marked this conversation as resolved.
Show resolved Hide resolved
return nullptr;
}

Expand Down
41 changes: 24 additions & 17 deletions core/src/stages/compute_ik.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -314,6 +314,11 @@ void ComputeIK::compute() {

link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);

if (!link) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

knowsFrameTransform(), a few lines above, should have validated that the frame exists.
Under what circumstances do you run into a segfault here?
I cannot imagine a valid frame that returns NULL here.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There seem to be cases under which getRigidlyConnectedParentLinkModel returns a nullptr. Are they all covered by knowsFrameTransform?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure if I am understanding it correctly. I tried to add comments to the code to make it more comprehensible in https://github.com/moveit/moveit2/pull/2969/files. Do you think they are accurate? My current understanding is that this function returns a nullptr when there is nothing connected to the parent or when the transform between the parent body and the given frame is unknown.

Copy link
Contributor

@rhaschke rhaschke Aug 12, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There seem to be cases under which getRigidlyConnectedParentLinkModel returns a nullptr. Are they all covered by knowsFrameTransform?

Yes, they should be covered by knowsFrameTransform. If not, that's probably a bug. Please state your concrete problem.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My current understanding is that this function returns a nullptr when there is nothing connected to the parent or when the transform between the parent body and the given frame is unknown.

IMO, this function should return NULL only if the given frame is unknown. Note that this function finds the parent link, which frame is connected to.

RCLCPP_WARN_STREAM(
LOGGER, fmt::format("ik frame '{}' is not rigidly connected to any link", ik_pose_msg.header.frame_id));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The error message doesn't make sense: an existing frame needs to be connected to some link.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would this be more precise?

Suggested change
LOGGER, fmt::format("ik frame '{}' is not rigidly connected to any link", ik_pose_msg.header.frame_id));
LOGGER, fmt::format("ik frame '{}' is not connected to any parent link", ik_pose_msg.header.frame_id));

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As I said, I expect this to return NULL if and only if frame doesn't exist. Do you have other evidence?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@sjahr: I'm still curious to learn about your concrete issue.
In which case does getRigidlyConnectedParentLinkModel returns NULL, which is not handled before by knowsFrameTransform?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, I cannot share more info on this at the moment 😞 I'll try debugging it further and hopefully figure out what the real source behind this is.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not expecting a detailed error analysis, but ideally a code snippet reproducing your segfault - I assume you observed a segfault in some code.

Copy link
Contributor Author

@sjahr sjahr Aug 20, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, unfortunately I don't have access to the code that causes the error and it seems to happen only in a HW setup. If you'd prefer that I can close this PR for now and come back later with more solid changes.

return;
}
// transform target pose such that ik frame will reach there if link does
target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName());
}
Expand All @@ -336,23 +341,25 @@ void ComputeIK::compute() {
marker.color.a *= 0.5;
eef_markers.push_back(marker);
};
const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
->getParentJointModel()
->getDescendantLinkModels();
if (colliding) {
SubTrajectory solution;
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
solution.markAsFailure();
// TODO: visualize collisions
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
auto colliding_scene{ scene->diff() };
colliding_scene->setCurrentState(sandbox_state);
spawn(InterfaceState(colliding_scene), std::move(solution));
return;
} else
generateVisualMarkers(sandbox_state, appender, links_to_visualize);
auto const& connected_parent_link_model = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link);
if (connected_parent_link_model && connected_parent_link_model->getParentJointModel()) {
const auto& links_to_visualize = connected_parent_link_model->getParentJointModel()->getDescendantLinkModels();
sjahr marked this conversation as resolved.
Show resolved Hide resolved
if (colliding) {
SubTrajectory solution;
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
solution.markAsFailure();
// TODO: visualize collisions
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
auto colliding_scene{ scene->diff() };
colliding_scene->setCurrentState(sandbox_state);
spawn(InterfaceState(colliding_scene), std::move(solution));
return;
} else {
generateVisualMarkers(sandbox_state, appender, links_to_visualize);
}
}

// determine joint values of robot pose to compare IK solution with for costs
std::vector<double> compare_pose;
Expand Down
Loading