Port loggers

core/include/moveit/task_constructor/stage_p.h

@@ -116,7 +116,8 @@ class StagePrivate
 	/// enforce only one parent exists
 	inline bool setParent(ContainerBase* parent) {
 		if (parent_) {
-			RCLCPP_ERROR_STREAM("Tried to add stage '" << name() << "' to two parents");
+			RCLCPP_ERROR_STREAM(rclcpp::get_logger("stage_private"),
+			                    "Tried to add stage '" << name() << "' to two parents");
 			return false;  // it's not allowed to add a stage to a parent if it already has one
 		}
 		parent_ = parent;

core/src/container.cpp

@@ -189,7 +189,11 @@ void ContainerBase::add(Stage::pointer&& stage) {
 
 bool ContainerBase::insert(Stage::pointer&& stage, int before) {
 	if (!stage) {
+<<<<<<< HEAD
 		RCLCPP_ERROR_STREAM(name() << ": received invalid stage pointer");
+=======
+		RCLCPP_ERROR_STREAM(rclcpp::get_logger("ContainerBase"), name() << ": reveived invalid stage pointer");
+>>>>>>> Port loggers
 		return false;
 	}
 
@@ -517,7 +521,7 @@ void SerialContainer::compute() {
 			if (!stage->pimpl()->canCompute())
 				continue;
 
-			RCLCPP_DEBUG("Computing stage '%s'", stage->name().c_str());
+			RCLCPP_DEBUG(rclcpp::get_logger("SerialContainer"), "Computing stage '%s'", stage->name().c_str());
 			stage->pimpl()->runCompute();
 		} catch (const Property::error& e) {
 			stage->reportPropertyError(e);
@@ -790,8 +794,13 @@ void Merger::onNewSolution(const SolutionBase& s) {
 
 void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
 	const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
+<<<<<<< HEAD
 	if (!trajectory || !trajectory->trajectory()) {
       RCLCPP_ERROR("Merger", "Only simple, valid trajectories are supported");
+=======
+	if (!trajectory) {
+		RCLCPP_ERROR(rclcpp::get_logger("Merger"), "Only simple trajectories are supported");
+>>>>>>> Port loggers
 		return;
 	}
 
@@ -897,10 +906,14 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
 	try {
 		merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
 	} catch (const std::runtime_error& e) {
+<<<<<<< HEAD
 		SubTrajectory t;
 		t.markAsFailure();
 		t.setComment(e.what());
 		spawner(std::move(t));
+=======
+		RCLCPP_INFO_STREAM(rclcpp::get_logger("Merger"), this->name() << "Merging failed: " << e.what());
+>>>>>>> Port loggers
 		return;
 	}
 	if (jmg_merged_.get() != jmg)

core/src/properties.cpp

@@ -39,12 +39,12 @@
 #include <moveit/task_constructor/properties.h>
 #include <boost/format.hpp>
 #include <functional>
-#include <ros/console.h>
+#include <rclcpp/logging.hpp>
 
 namespace moveit {
 namespace task_constructor {
 
-const std::string LOGNAME = "Properties";
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("Properties");
 
 class PropertyTypeRegistry
 {
@@ -73,7 +73,7 @@ class PropertyTypeRegistry
 	const Entry& entry(const std::type_index& type_index) const {
 		auto it = types_.find(type_index);
 		if (it == types_.end()) {
-			RCLCPP_ERROR_STREAM(LOGNAME, "Unregistered type: " << type_index.name());
+			RCLCPP_ERROR_STREAM(LOGGER, "Unregistered type: " << type_index.name());
 			return dummy_;
 		}
 		return it->second;
@@ -289,8 +289,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa
 		} catch (const Property::undefined&) {
 		}
 
-		RCLCPP_DEBUG_STREAM(LOGNAME, pair.first << ": " << p.initialized_from_ << " -> " << source << ": "
-		                                           << Property::serialize(value));
+		RCLCPP_DEBUG_STREAM(LOGGER, pair.first << ": " << p.initialized_from_ << " -> " << source << ": "
+		                                       << Property::serialize(value));
 		p.setCurrentValue(value);
 		p.initialized_from_ = source;
 	}

core/src/solvers/cartesian_path.cpp

@@ -47,6 +47,8 @@ namespace moveit {
 namespace task_constructor {
 namespace solvers {
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("CartesianPath");
+
 CartesianPath::CartesianPath() {
 	auto& p = properties();
 	p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
@@ -62,7 +64,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
                          const moveit_msgs::msg::Constraints& path_constraints) {
 	const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
 	if (!link) {
-		RCLCPP_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
+		RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName());
 		return false;
 	}
 

core/src/stage.cpp

@@ -40,7 +40,7 @@
 #include <moveit/task_constructor/introspection.h>
 #include <moveit/planning_scene/planning_scene.h>
 
-#include <ros/console.h>
+#include <rclcpp/logging.hpp>
 
 #include <boost/format.hpp>
 
@@ -318,7 +318,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
 	impl->properties_.reset();
 	if (impl->parent()) {
 		try {
-			RCLCPP_DEBUG_STREAM("Properties", "init '" << name() << "'");
+			RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Properties"), "init '" << name() << "'");
 			impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
 		} catch (const Property::error& e) {
 			std::ostringstream oss;
@@ -719,6 +719,8 @@ void ConnectingPrivate::compute() {
 	static_cast<Connecting*>(me_)->compute(from, to);
 }
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connecting");
+
 Connecting::Connecting(const std::string& name) : ComputeBase(new ConnectingPrivate(this, name)) {}
 
 void Connecting::reset() {
@@ -732,7 +734,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
 	const planning_scene::PlanningSceneConstPtr& to = to_state.scene();
 
 	if (from->getWorld()->size() != to->getWorld()->size()) {
-		RCLCPP_DEBUG_STREAM("Connecting", name() << ": different number of collision objects");
+		RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different number of collision objects");
 		return false;
 	}
 
@@ -741,19 +743,19 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
 		const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
 		const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_pair.first);
 		if (!to_object) {
-			RCLCPP_DEBUG_STREAM("Connecting", name() << ": object missing: " << from_object_pair.first);
+			RCLCPP_DEBUG_STREAM(LOGGER, name() << ": object missing: " << from_object_pair.first);
 			return false;
 		}
 		if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
-			RCLCPP_DEBUG_STREAM("Connecting", name() << ": different object shapes: " << from_object_pair.first);
+			RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object_pair.first);
 			return false;  // shapes not matching
 		}
 
 		for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
 		          to_it = to_object->shape_poses_.cbegin();
 		     from_it != from_end; ++from_it, ++to_it)
 			if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
-				RCLCPP_DEBUG_STREAM("Connecting", name() << ": different object pose: " << from_object_pair.first);
+				RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object pose: " << from_object_pair.first);
 				return false;  // transforms do not match
 			}
 	}
@@ -764,7 +766,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
 	from->getCurrentState().getAttachedBodies(from_attached);
 	to->getCurrentState().getAttachedBodies(to_attached);
 	if (from_attached.size() != to_attached.size()) {
-		RCLCPP_DEBUG_STREAM("Connecting", name() << ": different number of objects");
+		RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different number of objects");
 		return false;
 	}
 
@@ -774,26 +776,26 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
 			                       return object->getName() == from_object->getName();
 		                       });
 		if (it == to_attached.cend()) {
-			RCLCPP_DEBUG_STREAM("Connecting", name() << ": object missing: " << from_object->getName());
+			RCLCPP_DEBUG_STREAM(LOGGER, name() << ": object missing: " << from_object->getName());
 			return false;
 		}
 		const moveit::core::AttachedBody* to_object = *it;
 		if (from_object->getAttachedLink() != to_object->getAttachedLink()) {
-			RCLCPP_DEBUG_STREAM("Connecting", name() << ": different attach links: " << from_object->getName()
-			                                            << " attached to " << from_object->getAttachedLinkName() << " / "
-			                                            << to_object->getAttachedLinkName());
+			RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different attach links: " << from_object->getName() << " attached to "
+			                                   << from_object->getAttachedLinkName() << " / "
+			                                   << to_object->getAttachedLinkName());
 			return false;  // links not matching
 		}
 		if (from_object->getFixedTransforms().size() != to_object->getFixedTransforms().size()) {
-			RCLCPP_DEBUG_STREAM("Connecting", name() << ": different object shapes: " << from_object->getName());
+			RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object->getName());
 			return false;  // shapes not matching
 		}
 
 		for (auto from_it = from_object->getFixedTransforms().cbegin(),
 		          from_end = from_object->getFixedTransforms().cend(), to_it = to_object->getFixedTransforms().cbegin();
 		     from_it != from_end; ++from_it, ++to_it)
 			if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
-				RCLCPP_DEBUG_STREAM("Connecting", name() << ": different object pose: " << from_object->getName());
+				RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object pose: " << from_object->getName());
 				return false;  // transforms do not match
 			}
 	}

core/src/stages/compute_ik.cpp

@@ -47,12 +47,14 @@
 #include <chrono>
 #include <functional>
 #include <iterator>
-#include <ros/console.h>
+#include <rclcpp/logging.hpp>
 
 namespace moveit {
 namespace task_constructor {
 namespace stages {
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("ComputeIK");
+
 ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {
 	auto& p = properties();
 	p.declare<std::string>("eef", "name of end-effector group");
@@ -240,15 +242,15 @@ void ComputeIK::compute() {
 	std::string msg;
 
 	if (!validateEEF(props, robot_model, eef_jmg, &msg)) {
-		RCLCPP_WARN_STREAM("ComputeIK", msg);
+		RCLCPP_WARN_STREAM(LOGGER, msg);
 		return;
 	}
 	if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) {
-		RCLCPP_WARN_STREAM("ComputeIK", msg);
+		RCLCPP_WARN_STREAM(LOGGER, msg);
 		return;
 	}
 	if (!eef_jmg && !jmg) {
-		RCLCPP_WARN_STREAM("ComputeIK", "Neither eef nor group are well defined");
+		RCLCPP_WARN_STREAM(LOGGER, "Neither eef nor group are well defined");
 		return;
 	}
 	properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
@@ -262,8 +264,7 @@ void ComputeIK::compute() {
 	tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
 	if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) {
 		if (!sandbox_scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
-			RCLCPP_WARN_STREAM("ComputeIK",
-			                      "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
+			RCLCPP_WARN_STREAM(LOGGER, "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
 			return;
 		}
 		// transform target_pose w.r.t. planning frame
@@ -278,7 +279,7 @@ void ComputeIK::compute() {
 		//  determine IK link from eef/group
 		if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) :
 		                       jmg->getOnlyOneEndEffectorTip())) {
-			RCLCPP_WARN_STREAM("ComputeIK", "Failed to derive IK target link");
+			RCLCPP_WARN_STREAM(LOGGER, "Failed to derive IK target link");
 			return;
 		}
 		ik_pose_msg.header.frame_id = link->getName();
@@ -293,12 +294,12 @@ void ComputeIK::compute() {
 			const robot_state::AttachedBody* attached =
 			    sandbox_scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id);
 			if (!attached) {
-				RCLCPP_WARN_STREAM("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
+				RCLCPP_WARN_STREAM(LOGGER, "Unknown frame: " << ik_pose_msg.header.frame_id);
 				return;
 			}
 			const EigenSTL::vector_Isometry3d& tf = attached->getFixedTransforms();
 			if (tf.empty()) {
-				RCLCPP_WARN_STREAM("ComputeIK", "Attached body doesn't have shapes.");
+				RCLCPP_WARN_STREAM(LOGGER, "Attached body doesn't have shapes.");
 				return;
 			}
 			// prepend link

core/src/stages/connect.cpp

@@ -46,6 +46,8 @@ namespace moveit {
 namespace task_constructor {
 namespace stages {
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connect");
+
 Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) {
 	setTimeout(1.0);
 	setCostTerm(std::make_unique<cost::PathLength>());
@@ -90,7 +92,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
 		try {
 			merged_jmg_.reset(task_constructor::merge(groups));
 		} catch (const std::runtime_error& e) {
-			RCLCPP_INFO_STREAM("Connect", this->name() << ": " << e.what() << ". Disabling merging.");
+			RCLCPP_INFO_STREAM(LOGGER, this->name() << ": " << e.what() << ". Disabling merging.");
 		}
 	}
 
@@ -121,8 +123,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
 		Eigen::Map<const Eigen::VectorXd> positions_from(from.getJointPositions(jm), num);
 		Eigen::Map<const Eigen::VectorXd> positions_to(to.getJointPositions(jm), num);
 		if (!(positions_from - positions_to).isZero(1e-4)) {
-			RCLCPP_INFO_STREAM("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose()
-			                                                       << "] != [" << positions_to.transpose() << "]");
+			RCLCPP_INFO_STREAM(LOGGER, "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose()
+			                                                 << "] != [" << positions_to.transpose() << "]");
 			return false;
 		}
 	}

core/src/stages/current_state.cpp

@@ -47,6 +47,8 @@ namespace moveit {
 namespace task_constructor {
 namespace stages {
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("CurrentState");
+
 CurrentState::CurrentState(const std::string& name) : Generator(name) {
 	auto& p = properties();
 	Property& timeout = p.property("timeout");
@@ -91,7 +93,7 @@ void CurrentState::compute() {
 			return;
 		}
 	}
-	RCLCPP_WARN("failed to acquire current PlanningScene");
+	RCLCPP_WARN(LOGGER, "failed to acquire current PlanningScene");
 }
 }  // namespace stages
 }  // namespace task_constructor

core/src/stages/fix_collision_objects.cpp

@@ -45,7 +45,7 @@
 #include <rviz_marker_tools/marker_creation.h>
 #include <Eigen/Geometry>
 #include <eigen_conversions/eigen_msg.h>
-#include <ros/console.h>
+#include <rclcpp/logging.hpp>
 
 namespace vm = visualization_msgs;
 namespace cd = collision_detection;
@@ -54,6 +54,8 @@ namespace moveit {
 namespace task_constructor {
 namespace stages {
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("FixCollisionObjects");
+
 FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) {
 	// TODO: possibly weight solutions based on the required displacement?
 	setCostTerm(std::make_unique<cost::Constant>(0.0));
@@ -77,8 +79,7 @@ bool computeCorrection(const std::vector<cd::Contact>& contacts, Eigen::Vector3d
 	correction.setZero();
 	for (const cd::Contact& c : contacts) {
 		if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) {
-         RCLCPP_WARN_STREAM("FixCollisionObjects",
-			                      "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2);
+			RCLCPP_WARN_STREAM(LOGGER, "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2);
 			return false;
 		}
 		if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT)

core/src/stages/fixed_cartesian_poses.cpp

@@ -46,6 +46,8 @@ namespace moveit {
 namespace task_constructor {
 namespace stages {
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("FixedCartesianPoses");
+
 using PosesList = std::vector<geometry_msgs::msg::PoseStamped>;
 
 FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) {
@@ -86,7 +88,7 @@ void FixedCartesianPoses::compute() {
 		if (pose.header.frame_id.empty())
 			pose.header.frame_id = scene->getPlanningFrame();
 		else if (!scene->knowsFrameTransform(pose.header.frame_id)) {
-			RCLCPP_WARN("FixedCartesianPoses", "Unknown frame: '%s'", pose.header.frame_id.c_str());
+			RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", pose.header.frame_id.c_str());
 			continue;
 		}
 

core/src/stages/generate_grasp_pose.cpp

@@ -48,6 +48,8 @@ namespace moveit {
 namespace task_constructor {
 namespace stages {
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("GenerateGraspPose");
+
 GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(name) {
 	auto& p = properties();
 	p.declare<std::string>("eef", "name of end-effector");
@@ -105,7 +107,7 @@ void GenerateGraspPose::onNewSolution(const SolutionBase& s) {
 			solution.setComment(msg);
 			spawn(std::move(state), std::move(solution));
 		} else
-			RCLCPP_WARN_STREAM("GenerateGraspPose", msg);
+			RCLCPP_WARN_STREAM(LOGGER, msg);
 		return;
 	}