Apply suggestions from code review

sjahr · Aug 19, 2024 · 677529a · 677529a
1 parent f529160
commit 677529a
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 21 deletions.
diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
@@ -73,7 +73,6 @@ 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, ", ")));
 	return nullptr;
 }
 }  // namespace

diff --git a/core/src/container.cpp b/core/src/container.cpp
@@ -345,7 +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));
+	RCLCPP_DEBUG_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
@@ -341,25 +341,23 @@ void ComputeIK::compute() {
 		marker.color.a *= 0.5;
 		eef_markers.push_back(marker);
 	};
-	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);
-		}
-	}
+	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);
 
 	// determine joint values of robot pose to compare IK solution with for costs
 	std::vector<double> compare_pose;