Avoid segfault and improve error warnings

moveit · Aug 12, 2024 · a9c983d · a9c983d
1 parent 5519162
commit a9c983d
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 21 deletions.
diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
@@ -46,7 +46,7 @@
 #include <fmt/format.h>
 
 namespace {
-
+const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");
 // TODO: move to moveit::core::RobotModel
 const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::RobotModel& model,
                                                          const std::vector<std::string>& joints) {
@@ -73,13 +73,11 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
 				return jmg;
 		}
 	}
-
+	RCLCPP_WARN(LOGGER, "Could not find JointModelGroup that actuates {}", fmt::join(joints, ", "));
 	return nullptr;
 }
 }  // namespace
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");
-
 namespace move_group {
 
 ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}

diff --git a/core/src/container.cpp b/core/src/container.cpp
@@ -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));
 	return nullptr;
 }
 

diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp
@@ -314,6 +314,11 @@ void ComputeIK::compute() {
 
 		link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
 
+		if (!link) {
+			RCLCPP_WARN_STREAM(
+			    LOGGER, fmt::format("ik frame '{}' is not rigidly connected to any link", ik_pose_msg.header.frame_id));
+			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());
 	}
@@ -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();
+		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;