Avoid segfault and improve error warnings (#15)

Author: sjahr

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) {
@@ -78,8 +78,6 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
 }
 }  // namespace
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");
-
 namespace move_group {
 
 ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}

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_DEBUG_STREAM(rclcpp::get_logger("ContainerBase"), fmt::format("Child '{}' not found", name));
 	return nullptr;
 }
 

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());
 	}