mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Fix getRobotTipForFrame()
When passing the root frame, getRigidlyConnectedParentLinkModel() returns a nullptr for robot_link, causing a segfault. Actually, we don't need to use that method at all. We just need to find the robot_link of an associated body.
This commit is contained in:
parent
fd123cc4a7
commit
32d3454c1f
@ -69,23 +69,27 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
|
||||
} else {
|
||||
auto ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(property.value());
|
||||
if (ik_pose_msg.header.frame_id.empty()) {
|
||||
if (!(robot_link = get_tip())) {
|
||||
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
|
||||
return false;
|
||||
}
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame;
|
||||
} else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) {
|
||||
robot_link = scene.getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame;
|
||||
} else {
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
|
||||
robot_link = nullptr;
|
||||
bool found = false;
|
||||
auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found);
|
||||
if (!found && !ik_pose_msg.header.frame_id.empty()) {
|
||||
std::stringstream ss;
|
||||
ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'";
|
||||
ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'";
|
||||
solution.markAsFailure(ss.str());
|
||||
return false;
|
||||
}
|
||||
if (!robot_link)
|
||||
robot_link = get_tip();
|
||||
if (!robot_link) {
|
||||
solution.markAsFailure("ik_frame doesn't specify a link frame");
|
||||
return false;
|
||||
} else if (!found) { // use robot link's frame as reference by default
|
||||
ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
|
||||
}
|
||||
|
||||
tip_in_global_frame = ref_frame * tip_in_global_frame;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user